From 0c354da1452491bc9ada6e4106ab263be1bfe156 Mon Sep 17 00:00:00 2001 From: Manolis Surligas Date: Fri, 8 Apr 2016 21:41:21 +0300 Subject: [PATCH] Finish the FSK frame encoder for the CC1120 --- grc/satnogs_upsat_fsk_frame_acquisition.xml | 16 ++- grc/satnogs_upsat_fsk_frame_encoder.xml | 16 ++- include/satnogs/upsat_fsk_frame_acquisition.h | 4 +- include/satnogs/upsat_fsk_frame_encoder.h | 6 +- include/satnogs/utils.h | 16 +++ lib/upsat_fsk_frame_acquisition_impl.cc | 45 ++++++-- lib/upsat_fsk_frame_acquisition_impl.h | 6 +- lib/upsat_fsk_frame_encoder_impl.cc | 101 +++++++++++++++--- lib/upsat_fsk_frame_encoder_impl.h | 11 +- 9 files changed, 196 insertions(+), 25 deletions(-) diff --git a/grc/satnogs_upsat_fsk_frame_acquisition.xml b/grc/satnogs_upsat_fsk_frame_acquisition.xml index 1a87b15..f06b07e 100644 --- a/grc/satnogs_upsat_fsk_frame_acquisition.xml +++ b/grc/satnogs_upsat_fsk_frame_acquisition.xml @@ -4,7 +4,7 @@ satnogs_upsat_fsk_frame_acquisition satnogs import satnogs - satnogs.upsat_fsk_frame_acquisition($preamble, $sync_word, $whitening, $manchester) + satnogs.upsat_fsk_frame_acquisition($preamble, $sync_word, $whitening, $manchester, $check_crc) Frame Preamble @@ -45,6 +45,20 @@ True + + + Check CRC + check_crc + enum + + + in diff --git a/grc/satnogs_upsat_fsk_frame_encoder.xml b/grc/satnogs_upsat_fsk_frame_encoder.xml index d8cddb1..48efc6c 100644 --- a/grc/satnogs_upsat_fsk_frame_encoder.xml +++ b/grc/satnogs_upsat_fsk_frame_encoder.xml @@ -4,7 +4,7 @@ satnogs_upsat_fsk_frame_encoder satnogs import satnogs - satnogs.upsat_fsk_frame_encoder($preamble, $sync_word, $append_crc, $whitening, $manchester) + satnogs.upsat_fsk_frame_encoder($preamble, $sync_word, $append_crc, $whitening, $manchester, $msb_first) Frame Preamble @@ -59,6 +59,20 @@ True + + + Send MS bit first + msb_first + enum + + + pdu diff --git a/include/satnogs/upsat_fsk_frame_acquisition.h b/include/satnogs/upsat_fsk_frame_acquisition.h index 490ebcc..a9f23fb 100644 --- a/include/satnogs/upsat_fsk_frame_acquisition.h +++ b/include/satnogs/upsat_fsk_frame_acquisition.h @@ -52,11 +52,13 @@ namespace gr * the whitening algorithm of the CC1120 chip. False otherwise. * @param manchester true if the transmitted data have been processed by * the Manchester algorithm of the CC1120 chip. False otherwise. + * @param check_crc if set to true the decoder will push frames only if + * their CRC field in correct. */ static sptr make (const std::vector &preamble, const std::vector &sync_word, bool whitening = false, - bool manchester = false); + bool manchester = false, bool check_crc = true); }; } // namespace satnogs diff --git a/include/satnogs/upsat_fsk_frame_encoder.h b/include/satnogs/upsat_fsk_frame_encoder.h index 3bb9c9e..23a7902 100644 --- a/include/satnogs/upsat_fsk_frame_encoder.h +++ b/include/satnogs/upsat_fsk_frame_encoder.h @@ -61,11 +61,15 @@ namespace gr * * @param manchester true if the transmitted data have been processed by * the Manchester algorithm of the CC1120 chip. False otherwise. + * + * @param msb_first if set to true, the the treansmission starts from the + * MSB of each byte. */ static sptr make (const std::vector& preamble, const std::vector& sync_word, bool append_crc = true, - bool whitening = false, bool manchester = false); + bool whitening = false, bool manchester = false, + bool msb_first = true); }; } // namespace satnogs diff --git a/include/satnogs/utils.h b/include/satnogs/utils.h index 129d663..741f5fe 100644 --- a/include/satnogs/utils.h +++ b/include/satnogs/utils.h @@ -76,6 +76,22 @@ namespace gr 0xE606, 0xD49D, 0xC514, 0xB1AB, 0xA022, 0x92B9, 0x8330, 0x7BC7, 0x6A4E, 0x58D5, 0x495C, 0x3DE3, 0x2C6A, 0x1EF1, 0x0F78 }; + static inline uint16_t + update_crc16_ccitt(uint16_t crc, const uint8_t *data, size_t len) + { + register size_t i; + for (i = 0; i < len; i++) { + crc = (crc >> 8) ^ crc16_ccitt_table_reverse[(crc ^ data[i]) & 0xff]; + } + return crc; + } + + static inline uint16_t + crc16_ccitt(const uint8_t *data, size_t len) + { + return update_crc16_ccitt(0xFFFF, data, len) ^ 0xFFFF; + } + /** * Counts the number of active bits in x */ diff --git a/lib/upsat_fsk_frame_acquisition_impl.cc b/lib/upsat_fsk_frame_acquisition_impl.cc index b18ef50..974f8b7 100644 --- a/lib/upsat_fsk_frame_acquisition_impl.cc +++ b/lib/upsat_fsk_frame_acquisition_impl.cc @@ -25,6 +25,7 @@ #include #include "upsat_fsk_frame_acquisition_impl.h" #include +#include namespace gr { @@ -34,11 +35,12 @@ namespace gr upsat_fsk_frame_acquisition::sptr upsat_fsk_frame_acquisition::make (const std::vector &preamble, const std::vector &sync_word, - bool whitening, bool manchester) + bool whitening, bool manchester, + bool check_crc) { return gnuradio::get_initial_sptr ( new upsat_fsk_frame_acquisition_impl (preamble, sync_word, whitening, - manchester)); + manchester, check_crc)); } /* @@ -46,7 +48,8 @@ namespace gr */ upsat_fsk_frame_acquisition_impl::upsat_fsk_frame_acquisition_impl ( const std::vector &preamble, - const std::vector &sync_word, bool whitening, bool manchester) : + const std::vector &sync_word, bool whitening, bool manchester, + bool check_crc) : gr::sync_block ("upsat_fsk_frame_acquisition", gr::io_signature::make (1, 1, sizeof(float)), gr::io_signature::make (0, 0, 0)), @@ -61,6 +64,9 @@ namespace gr * searching for the SYNC word. */ d_search_for_sync_thrhld(d_preamble_len / 3), + d_whitening(whitening), + d_manchester(manchester), + d_check_crc(check_crc), d_state (SEARCHING), d_shifting_byte (0x0), d_decoded_bytes (0), @@ -157,7 +163,7 @@ namespace gr { LOG_DEBUG("Enter have payload"); d_state = HAVE_PAYLOAD; - d_decoded_bytes = 0; + d_decoded_bytes = 1; d_decoded_bits = 0; } @@ -167,6 +173,8 @@ namespace gr gr_vector_void_star &output_items) { int i; + uint16_t crc_received; + uint16_t crc_calc; const float *in = (const float *) input_items[0]; for (i = 0; i < noutput_items; i++) { slice_and_shift (in[i]); @@ -234,7 +242,10 @@ namespace gr case HAVE_FRAME_LEN: d_decoded_bits++; if(d_decoded_bits == 8) { - d_frame_len = d_shifting_byte; + /* Frame length field is needed for the CRC calculation */ + d_pdu[0] = d_shifting_byte; + /* CRC is not included in the frame length field, but we want it */ + d_frame_len = 1 + d_shifting_byte + sizeof(uint16_t); have_payload(); } break; @@ -246,8 +257,28 @@ namespace gr d_decoded_bytes++; if (d_decoded_bytes == d_frame_len) { - message_port_pub (pmt::mp ("pdu"), - pmt::make_blob (d_pdu, d_frame_len)); + if(!d_check_crc){ + message_port_pub ( + pmt::mp ("pdu"), + pmt::make_blob (d_pdu + 1, + d_frame_len - 1 - sizeof(uint16_t))); + reset_state (); + break; + } + + /* Retrieve and check the CRC */ + memcpy(&crc_received, d_pdu + d_frame_len - sizeof(uint16_t), + sizeof(uint16_t)); + crc_calc = crc16_ccitt(d_pdu, d_frame_len - sizeof(uint16_t)); + if(crc_calc == crc_received) { + message_port_pub ( + pmt::mp ("pdu"), + pmt::make_blob (d_pdu + 1, + d_frame_len - 1 - sizeof(uint16_t))); + } + else{ + LOG_WARN("Frame with wrong CRC"); + } reset_state (); } } diff --git a/lib/upsat_fsk_frame_acquisition_impl.h b/lib/upsat_fsk_frame_acquisition_impl.h index 519cff2..ad8a1cb 100644 --- a/lib/upsat_fsk_frame_acquisition_impl.h +++ b/lib/upsat_fsk_frame_acquisition_impl.h @@ -51,6 +51,9 @@ namespace gr const std::vector d_sync_word; const size_t d_sync_word_len; const size_t d_search_for_sync_thrhld; + const bool d_whitening; + const bool d_manchester; + const bool d_check_crc; decoding_state_t d_state; uint8_t d_shifting_byte; size_t d_decoded_bytes; @@ -77,7 +80,8 @@ namespace gr public: upsat_fsk_frame_acquisition_impl (const std::vector &preamble, const std::vector &sync_word, - bool whitening, bool manchester); + bool whitening, bool manchester, + bool check_crc); ~upsat_fsk_frame_acquisition_impl (); // Where all the action really happens diff --git a/lib/upsat_fsk_frame_encoder_impl.cc b/lib/upsat_fsk_frame_encoder_impl.cc index 86dd223..970c0a5 100644 --- a/lib/upsat_fsk_frame_encoder_impl.cc +++ b/lib/upsat_fsk_frame_encoder_impl.cc @@ -25,6 +25,7 @@ #include #include "upsat_fsk_frame_encoder_impl.h" #include +#include namespace gr { @@ -35,11 +36,12 @@ namespace gr upsat_fsk_frame_encoder::make (const std::vector& preamble, const std::vector& sync_word, bool append_crc, bool whitening, - bool manchester) + bool manchester, + bool msb_first) { return gnuradio::get_initial_sptr ( new upsat_fsk_frame_encoder_impl (preamble, sync_word, append_crc, - whitening, manchester)); + whitening, manchester, msb_first)); } /* @@ -48,7 +50,7 @@ namespace gr upsat_fsk_frame_encoder_impl::upsat_fsk_frame_encoder_impl ( const std::vector& preamble, const std::vector& sync_word, bool append_crc, bool whitening, - bool manchester) : + bool manchester, bool msb_first) : gr::sync_block ("upsat_fsk_frame_encoder", gr::io_signature::make (0, 0, 0), gr::io_signature::make (1, 1, sizeof(float))), @@ -59,9 +61,14 @@ namespace gr d_append_crc(append_crc), d_whitening(whitening), d_manchester(manchester), + d_msb_first(msb_first), + d_max_pdu_len(d_preamble_len + d_sync_word_len + sizeof(uint8_t) + + UPSAT_MAX_FRAME_LEN + sizeof(uint16_t)), d_encoded(0), d_pdu_len(0) { + /* Simplify the logic of the output samples handling */ + set_output_multiple(8); message_port_register_in (pmt::mp ("pdu")); /* * Allocate memory for the maximum possible frame WITH all headers and @@ -71,8 +78,11 @@ namespace gr * +-------------+----------+--------+--------------------------+------+ * User def. User def. 1B 1-255 B 2 B */ - d_pdu = new uint8_t[d_preamble_len + d_sync_word_len + sizeof(uint8_t) - + UPSAT_MAX_FRAME_LEN + sizeof(uint16_t)]; + d_pdu = new uint8_t[d_max_pdu_len]; + + if(!d_pdu) { + throw std::runtime_error("Could not allocate memory"); + } /* Copy the preamble at the start of the pdu */ memcpy(d_pdu, d_preamble.data(), d_preamble_len); @@ -87,11 +97,51 @@ namespace gr delete [] d_pdu; } + inline void + upsat_fsk_frame_encoder_impl::map_msb_first(float *out, size_t nsamples_out) + { + size_t i; + register uint8_t b; + for(i = 0; i < nsamples_out; i+=8) + { + b = d_pdu[d_encoded + i/8]; + out[i] = ((b >> 7 ) * 2.0f) - 1.0f; + out[i + 1] = (( (b >> 6 ) & 0x1) * 2.0f) - 1.0f; + out[i + 2] = (( (b >> 5 ) & 0x1) * 2.0f) - 1.0f; + out[i + 3] = (( (b >> 4 ) & 0x1) * 2.0f) - 1.0f; + out[i + 4] = (( (b >> 3 ) & 0x1) * 2.0f) - 1.0f; + out[i + 5] = (( (b >> 2 ) & 0x1) * 2.0f) - 1.0f; + out[i + 6] = (( (b >> 1 ) & 0x1) * 2.0f) - 1.0f; + out[i + 7] = ((b & 0x1) * 2.0f) - 1.0f; + } + } + + inline void + upsat_fsk_frame_encoder_impl::map_lsb_first(float *out, size_t nsamples_out) + { + size_t i; + register uint8_t b; + for(i = 0; i < nsamples_out; i+=8) + { + b = d_pdu[d_encoded + i/8]; + out[i + 7] = ((b >> 7 ) * 2.0f) - 1.0f; + out[i + 6] = (( (b >> 6 ) & 0x1) * 2.0f) - 1.0f; + out[i + 5] = (( (b >> 5 ) & 0x1) * 2.0f) - 1.0f; + out[i + 4] = (( (b >> 4 ) & 0x1) * 2.0f) - 1.0f; + out[i + 3] = (( (b >> 3 ) & 0x1) * 2.0f) - 1.0f; + out[i + 2] = (( (b >> 2 ) & 0x1) * 2.0f) - 1.0f; + out[i + 1] = (( (b >> 1 ) & 0x1) * 2.0f) - 1.0f; + out[i] = ((b & 0x1) * 2.0f) - 1.0f; + } + } + int upsat_fsk_frame_encoder_impl::work (int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { + uint16_t crc; + size_t min_available; pmt::pmt_t pdu; float *out = (float *) output_items[0]; @@ -107,21 +157,48 @@ namespace gr LOG_ERROR("PDU is greater than the supported. Dropping the PDU"); return 0; } - /* Set the frame length at the corresponding field */ + /* + * Set the frame length at the corresponding field. + * NOTE: The frame length is calculated taking consideration only + * the address field (if exists) and the payload. Length and CRC fields + * are NOT included. + */ d_pdu[d_preamble_len + d_sync_word_len] = (uint8_t) d_pdu_len; + /* Append the variable length PDU */ - memcpy (d_pdu + d_preamble_len + d_sync_word_len + sizeof(uint8_t), - pmt::blob_data (pdu), - d_pdu_len * sizeof(uint8_t)); - d_pdu_len += d_preamble_len + d_sync_word_len + sizeof(uint8_t); + memcpy (d_pdu + d_preamble_len + d_sync_word_len + 1, + pmt::blob_data (pdu), d_pdu_len); + /* If it is necessary calculate and append the CRC */ if(d_append_crc) { - /* TODO: Calc CRC */ + crc = crc16_ccitt(d_pdu + d_preamble_len + d_sync_word_len, + d_pdu_len + 1); + memcpy(d_pdu + d_preamble_len + d_sync_word_len + 1 + d_pdu_len, + &crc, sizeof(uint16_t)); d_pdu_len += sizeof(uint16_t); } + + d_pdu_len += d_preamble_len + d_sync_word_len + 1; } - return noutput_items; + noutput_items = std::max (0, noutput_items); + min_available = std::min ((size_t) noutput_items, + (d_pdu_len - d_encoded) * 8); + + if(d_msb_first){ + map_msb_first(out, min_available*8); + } + else{ + map_lsb_first(out, min_available*8); + } + d_encoded += min_available/8; + + /* End of the frame reached */ + if(d_encoded == d_pdu_len){ + d_encoded = 0; + } + + return min_available; } } /* namespace satnogs */ diff --git a/lib/upsat_fsk_frame_encoder_impl.h b/lib/upsat_fsk_frame_encoder_impl.h index 59e252f..cffd0c6 100644 --- a/lib/upsat_fsk_frame_encoder_impl.h +++ b/lib/upsat_fsk_frame_encoder_impl.h @@ -38,15 +38,24 @@ namespace gr const bool d_append_crc; const bool d_whitening; const bool d_manchester; + const bool d_msb_first; + const size_t d_max_pdu_len; size_t d_encoded; size_t d_pdu_len; uint8_t *d_pdu; + inline void + map_msb_first (float *out, size_t nsamples_out); + + inline void + map_lsb_first (float *out, size_t nsamples_out); + public: upsat_fsk_frame_encoder_impl (const std::vector& preamble, const std::vector& sync_word, bool append_crc, bool whitening, - bool manchester); + bool manchester, + bool msb_first); ~upsat_fsk_frame_encoder_impl (); // Where all the action really happens