lrpt_sync_impl.cc 8.41 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27
/* -*- c++ -*- */
/*
 * gr-satnogs: SatNOGS GNU Radio Out-Of-Tree Module
 *
 *  Copyright (C) 2018, Libre Space Foundation <http://librespacefoundation.org/>
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <gnuradio/io_signature.h>
#include "lrpt_sync_impl.h"
#include <satnogs/log.h>
Manolis Surligas's avatar
Manolis Surligas committed
28
#include <satnogs/utils.h>
29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59

#include <volk/volk.h>
#include <gnuradio/blocks/count_bits.h>

namespace gr
{
namespace satnogs
{

lrpt_sync::sptr
lrpt_sync::make (size_t threshold)
{
  return gnuradio::get_initial_sptr (new lrpt_sync_impl (threshold));
}

/*
 * The private constructor
 */
lrpt_sync_impl::lrpt_sync_impl (size_t threshold) :
        gr::sync_block ("lrpt_sync",
                   gr::io_signature::make (1, 1, sizeof(gr_complex)),
                   gr::io_signature::make (0, 0, 0)),
                   d_thresh(threshold),
                   d_asm_coded(0xd49c24ff2686b),
                   d_asm_coded_len(52),
                   d_asm_coded_mask((1ULL << d_asm_coded_len) - 1),
                   /*
                    * We process the data in a multiple of 2 frames and a UW
                    * sync word
                    */
                   d_window((72 + 8)/2),
60
                   /* Each CADU has the 4 byte ASM and a VCDU of 1020 bytes*/
61 62 63 64 65 66 67 68
                   /*
                    * NOTE:
                    * Metop violates the standard as many times as possible...
                    * The frame should contain 128 RS check symbols at the end.
                    * For some unknown reasons, it seems that the RS encoding is not performed.
                    * Thus, they dropped the check symbols at the end of the frame.
                    */
                   d_coded_cadu_len(1020 * 2 + 4*2 - 128 * 2),
69
                   d_frame_sync(false),
70
                   d_received(0),
71
                   d_rotate(1.0, 0.0),
72 73 74 75 76 77 78 79 80
                   d_qpsk(digital::constellation_qpsk::make()),
                   d_shift_reg0(0x0),
                   d_shift_reg1(0x0),
                   d_shift_reg2(0x0),
                   d_shift_reg3(0x0)
{
  set_output_multiple(d_window);
  const int alignment_multiple = volk_get_alignment () / sizeof(gr_complex);
  set_alignment (std::max (1, alignment_multiple));
81 82
  d_rotate_pi2 = (gr_complex *) volk_malloc (d_window * sizeof(gr_complex),
                                             volk_get_alignment ());
83 84 85 86
  if(!d_rotate_pi2) {
    throw std::runtime_error("lrpt_sync: Could not allocate memory");
  }

87 88
  d_rotate_2pi2 = (gr_complex *) volk_malloc (d_window * sizeof(gr_complex),
                                              volk_get_alignment ());
89 90 91 92 93
  if(!d_rotate_2pi2) {
    volk_free(d_rotate_pi2);
    throw std::runtime_error("lrpt_sync: Could not allocate memory");
  }

94 95
  d_rotate_3pi2 = (gr_complex *) volk_malloc (d_window * sizeof(gr_complex),
                                              volk_get_alignment ());
96 97 98 99 100
  if(!d_rotate_3pi2) {
    volk_free(d_rotate_pi2);
    volk_free(d_rotate_2pi2);
    throw std::runtime_error("lrpt_sync: Could not allocate memory");
  }
101

102 103
  d_corrected = (gr_complex *)volk_malloc(d_window * sizeof(gr_complex),
                                          volk_get_alignment ());
104 105 106 107 108 109 110
  if(!d_corrected) {
    volk_free(d_rotate_pi2);
    volk_free(d_rotate_2pi2);
    volk_free(d_rotate_3pi2);
    throw std::runtime_error("lrpt_sync: Could not allocate memory");
  }

111
  uint64_t asm_coded = htonll(d_asm_coded);
112
  d_coded_cadu = new uint8_t[d_coded_cadu_len];
Manolis Surligas's avatar
Manolis Surligas committed
113 114
  memcpy(d_coded_cadu, &asm_coded, sizeof(uint64_t));
  d_received =  sizeof(uint64_t);
115 116

  message_port_register_out(pmt::mp("cadu"));
117 118 119 120 121 122 123 124 125 126
}

/*
 * Our virtual destructor.
 */
lrpt_sync_impl::~lrpt_sync_impl ()
{
  volk_free (d_rotate_pi2);
  volk_free (d_rotate_2pi2);
  volk_free (d_rotate_3pi2);
127 128
  volk_free (d_corrected);
  delete [] d_coded_cadu;
129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145
}

bool
lrpt_sync_impl::found_sync(uint64_t reg)
{
  return blocks::count_bits64 ((reg ^ d_asm_coded) & d_asm_coded_mask)
      <= d_thresh;
}


int
lrpt_sync_impl::work_no_sync(const gr_complex *in, int noutput_items)
{
  uint32_t bits;
  int multiple = noutput_items / d_window;
  for(int i = 0; i < multiple; i++) {
    volk_32fc_s32fc_multiply_32fc(d_rotate_pi2, in + i * d_window,
146
                                  gr_complex(0.0, 1.0), d_window);
147
    volk_32fc_s32fc_multiply_32fc(d_rotate_2pi2, in + i * d_window,
148
                                  gr_complex(-1.0, 0.0), d_window);
149
    volk_32fc_s32fc_multiply_32fc(d_rotate_3pi2, in + i * d_window,
150
                                  gr_complex(0.0, -1.0), d_window);
151 152 153 154 155 156
    /*
     * Search for the sync pattern, rotating the QPSK constellation on
     * all possible positions
     */
    for(int j = 0; j < d_window; j++) {
      bits = d_qpsk->decision_maker(in + i * d_window + j);
157
      //bits = (d_conv_deinter.decode_bit(bits >> 1) << 1) | d_conv_deinter.decode_bit(bits & 0x1);
158 159
      d_shift_reg0 = (d_shift_reg0 << 2) | bits;
      if(found_sync(d_shift_reg0)) {
160
        d_rotate = gr_complex(1.0, 0.0);
161
        d_frame_sync = true;
162 163 164
        uint64_t asm_coded = htonll(d_shift_reg0);
        memcpy(d_coded_cadu, &asm_coded, sizeof(uint64_t));
        return i * d_window + j + 1;
165 166 167
      }

      bits = d_qpsk->decision_maker(d_rotate_pi2 + j);
168
      //bits = (d_conv_deinter.decode_bit(bits >> 1) << 1) | d_conv_deinter.decode_bit(bits & 0x1);
169 170 171 172
      d_shift_reg1 = (d_shift_reg1 << 2) | bits;
      if(found_sync(d_shift_reg1)) {
        d_rotate = gr_complex(0.0, 1.0);
        d_frame_sync = true;
173 174 175
        uint64_t asm_coded = htonll(d_shift_reg1);
        memcpy(d_coded_cadu, &asm_coded, sizeof(uint64_t));
        return i * d_window + j + 1;
176 177 178
      }

      bits = d_qpsk->decision_maker(d_rotate_2pi2 + j);
179
      //bits = (d_conv_deinter.decode_bit(bits >> 1) << 1) | d_conv_deinter.decode_bit(bits & 0x1);
180 181
      d_shift_reg2 = (d_shift_reg2 << 2) | bits;
      if(found_sync(d_shift_reg2)) {
182
        d_rotate = gr_complex(-1.0, 0.0);
183
        d_frame_sync = true;
184 185 186
        uint64_t asm_coded = htonll(d_shift_reg2);
        memcpy(d_coded_cadu, &asm_coded, sizeof(uint64_t));
        return i * d_window + j + 1;
187 188 189
      }

      bits = d_qpsk->decision_maker(d_rotate_3pi2 + j);
190
      //bits = (d_conv_deinter.decode_bit(bits >> 1) << 1) | d_conv_deinter.decode_bit(bits & 0x1);
191 192 193 194
      d_shift_reg3 = (d_shift_reg3 << 2) | bits;
      if(found_sync(d_shift_reg3)) {
        d_rotate = gr_complex(0.0, -1.0);
        d_frame_sync = true;
195 196 197
        uint64_t asm_coded = htonll(d_shift_reg3);
        memcpy(d_coded_cadu, &asm_coded, sizeof(uint64_t));
        return i * d_window + j + 1;
198 199 200 201 202 203 204 205 206
      }
    }
  }
  return noutput_items;
}

int
lrpt_sync_impl::work_sync(const gr_complex *in, int noutput_items)
{
207 208 209 210 211
  uint8_t b;
  int multiple = noutput_items / d_window;
  for(int i = 0; i < multiple; i++) {
    volk_32fc_s32fc_multiply_32fc(d_corrected, in + i * d_window,
                                  d_rotate, d_window);
Manolis Surligas's avatar
Manolis Surligas committed
212
    for(int j = 0; j < d_window; j += 4) {
213 214 215 216 217 218 219 220
      b = 0;
      b = d_qpsk->decision_maker(d_corrected + j) << 6;
      b |= d_qpsk->decision_maker(d_corrected + j + 1) << 4;
      b |= d_qpsk->decision_maker(d_corrected + j + 2) << 2;
      b |= d_qpsk->decision_maker(d_corrected + j + 3);

      d_coded_cadu[d_received++] = b;
      if(d_received == d_coded_cadu_len) {
Manolis Surligas's avatar
Manolis Surligas committed
221
        d_received = sizeof(uint64_t);
222
        d_frame_sync = false;
223 224
        message_port_pub (pmt::mp ("cadu"),
                          pmt::make_blob (d_coded_cadu, d_coded_cadu_len));
225 226 227 228
        return i * d_window + j + 4;
      }
    }
  }
229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248
  return noutput_items;
}


int
lrpt_sync_impl::work (int noutput_items,
                      gr_vector_const_void_star &input_items,
                      gr_vector_void_star &output_items)
{
  const gr_complex *in = (const gr_complex *) input_items[0];
  uint32_t bits;
  if(!d_frame_sync) {
    return work_no_sync(in, noutput_items);
  }
  return work_sync(in, noutput_items);
}

} /* namespace satnogs */
} /* namespace gr */