From b96d1707a97cb7a502713dd4b87f919c2169c81d Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Thu, 23 Mar 2017 15:45:41 +0100 Subject: [PATCH 1/8] GPS L2CM telemetry decoder rewrite, borrowed from the Swift navigator open source libraries --- .../PVT/gnuradio_blocks/hybrid_pvt_cc.cc | 76 ++- src/algorithms/PVT/libs/hybrid_ls_pvt.cc | 19 +- .../galileo_e1_observables_cc.cc | 6 +- .../gps_l1_ca_observables_cc.cc | 1 - .../gnuradio_blocks/hybrid_observables_cc.cc | 8 +- .../telemetry_decoder/adapters/CMakeLists.txt | 1 + .../gnuradio_blocks/CMakeLists.txt | 3 +- .../gps_l2c_telemetry_decoder_cc.cc | 506 +++++------------- .../gps_l2c_telemetry_decoder_cc.h | 69 +-- .../telemetry_decoder/libs/CMakeLists.txt | 7 +- .../libs/libswiftcnav/CMakeLists.txt | 35 ++ .../libs/libswiftcnav/bits.c | 268 ++++++++++ .../libs/libswiftcnav/bits.h | 31 ++ .../libs/libswiftcnav/cnav_msg.c | 432 +++++++++++++++ .../libs/libswiftcnav/cnav_msg.h | 101 ++++ .../libs/{ => libswiftcnav}/convolutional.h | 0 .../telemetry_decoder/libs/libswiftcnav/edc.c | 122 +++++ .../telemetry_decoder/libs/libswiftcnav/edc.h | 21 + .../telemetry_decoder/libs/libswiftcnav/fec.h | 47 ++ .../libs/libswiftcnav/swift_common.h | 64 +++ .../libs/libswiftcnav/viterbi27.c | 216 ++++++++ src/core/receiver/CMakeLists.txt | 3 + src/core/system_parameters/GPS_L2C.h | 42 +- src/core/system_parameters/MATH_CONSTANTS.h | 1 + .../system_parameters/galileo_ephemeris.h | 2 +- .../system_parameters/gps_cnav_ephemeris.cc | 13 +- .../system_parameters/gps_cnav_ephemeris.h | 5 +- .../gps_cnav_navigation_message.cc | 84 ++- .../gps_cnav_navigation_message.h | 15 +- .../system_parameters/gps_cnav_utc_model.cc | 73 +++ .../system_parameters/gps_cnav_utc_model.h | 9 +- src/core/system_parameters/gps_utc_model.cc | 6 + 32 files changed, 1783 insertions(+), 503 deletions(-) create mode 100644 src/algorithms/telemetry_decoder/libs/libswiftcnav/CMakeLists.txt create mode 100644 src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c create mode 100644 src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.h create mode 100644 src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c create mode 100644 src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h rename src/algorithms/telemetry_decoder/libs/{ => libswiftcnav}/convolutional.h (100%) create mode 100644 src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c create mode 100644 src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.h create mode 100644 src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h create mode 100644 src/algorithms/telemetry_decoder/libs/libswiftcnav/swift_common.h create mode 100644 src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c diff --git a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc index 946a4e925..04d8ab6d4 100644 --- a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc @@ -34,6 +34,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -158,7 +161,7 @@ void hybrid_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) gps_cnav_ephemeris = boost::any_cast>(pmt::any_ref(msg)); // update/insert new ephemeris record to the global ephemeris map d_ls_pvt->gps_cnav_ephemeris_map[gps_cnav_ephemeris->i_satellite_PRN] = *gps_cnav_ephemeris; - DLOG(INFO) << "New GPS CNAV ephemeris record has arrived "; + LOG(INFO) << "New GPS CNAV ephemeris record has arrived "; } else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) { @@ -323,6 +326,76 @@ hybrid_pvt_cc::hybrid_pvt_cc(unsigned int nchannels, bool dump, std::string dump hybrid_pvt_cc::~hybrid_pvt_cc() { msgctl(sysv_msqid, IPC_RMID, NULL); + + //save GPS L2CM ephemeris to XML file + std::string file_name="eph_GPS_L2CM.xml"; + + if (d_ls_pvt->gps_cnav_ephemeris_map.size() > 0) + { + try + { + std::ofstream ofs(file_name.c_str(), std::ofstream::trunc | std::ofstream::out); + boost::archive::xml_oarchive xml(ofs); + xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", d_ls_pvt->gps_cnav_ephemeris_map); + ofs.close(); + LOG(INFO) << "Saved GPS L2CM Ephemeris map data"; + } + catch (std::exception& e) + { + LOG(WARNING) << e.what(); + } + } + else + { + LOG(WARNING) << "Failed to save GPS L2CM Ephemeris, map is empty"; + } + + //save GPS L1 CA ephemeris to XML file + file_name="eph_GPS_L1CA.xml"; + + if (d_ls_pvt->gps_ephemeris_map.size() > 0) + { + try + { + std::ofstream ofs(file_name.c_str(), std::ofstream::trunc | std::ofstream::out); + boost::archive::xml_oarchive xml(ofs); + xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", d_ls_pvt->gps_ephemeris_map); + ofs.close(); + LOG(INFO) << "Saved GPS L1 CA Ephemeris map data"; + } + catch (std::exception& e) + { + LOG(WARNING) << e.what(); + } + } + else + { + LOG(WARNING) << "Failed to save GPS L1 CA Ephemeris, map is empty"; + } + + //save Galileo E1 ephemeris to XML file + file_name="eph_Galileo_E1.xml"; + + if (d_ls_pvt->galileo_ephemeris_map.size() > 0) + { + try + { + std::ofstream ofs(file_name.c_str(), std::ofstream::trunc | std::ofstream::out); + boost::archive::xml_oarchive xml(ofs); + xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", d_ls_pvt->galileo_ephemeris_map); + ofs.close(); + LOG(INFO) << "Saved Galileo E1 Ephemeris map data"; + } + catch (std::exception& e) + { + LOG(WARNING) << e.what(); + } + } + else + { + LOG(WARNING) << "Failed to save Galileo E1 Ephemeris, map is empty"; + } + } @@ -451,6 +524,7 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v if ((d_sample_counter % d_output_rate_ms) == 0) { bool pvt_result; + //std::cout<<"obs map size at pvt="<get_PVT(gnss_observables_map, d_rx_time, d_flag_averaging); if (pvt_result == true) diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc index 09fc2b65f..5c8888723 100644 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc +++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc @@ -86,6 +86,8 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou int GPS_week = 0; double utc = 0.0; double GST = 0.0; + double secondsperweek = 604800.0; + //double utc_tx_corrected = 0.0; //utc computed at tx_time_corrected, added for Galileo constellation (in GPS utc is directly computed at TX_time_corrected_s) double TX_time_corrected_s = 0.0; double SV_clock_bias_s = 0.0; @@ -196,8 +198,8 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz; // SV ECEF DEBUG OUTPUT - DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN - << " X=" << gps_ephemeris_iter->second.d_satpos_X + LOG(INFO) << "(new)ECEF GPS L1 CA satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN + << " TX Time corrected="< gnss_observables_map, dou valid_obs++; // compute the UTC time for this SV (just to print the associated UTC timestamp) GPS_week = gps_ephemeris_iter->second.i_GPS_week; - utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week); } else // the ephemeris are not available for this SV { @@ -233,7 +234,8 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou // 3- compute the current ECEF position for this SV using corrected TX time TX_time_corrected_s = Tx_time - SV_clock_bias_s; - gps_cnav_ephemeris_iter->second.satellitePosition(TX_time_corrected_s); + //std::cout<<"TX time["<second.i_satellite_PRN<<"]="<second.satellitePosition(TX_time_corrected_s); //store satellite positions in a matrix satpos.resize(3, valid_obs + 1); @@ -243,15 +245,17 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou // 4- fill the observations vector with the corrected observables obs.resize(valid_obs + 1, 1); - obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GPS_C_m_s; + obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr*GPS_C_m_s + SV_clock_bias_s * GPS_C_m_s; d_visible_satellites_IDs[valid_obs] = gps_cnav_ephemeris_iter->second.i_satellite_PRN; d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz; GPS_week = gps_cnav_ephemeris_iter->second.i_GPS_week; + GPS_week=GPS_week%1024; //Necessary due to the increase of WN bits in CNAV message (10 in GPS NAV and 13 in CNAV) // SV ECEF DEBUG OUTPUT - DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_cnav_ephemeris_iter->second.i_satellite_PRN - << " X=" << gps_cnav_ephemeris_iter->second.d_satpos_X + LOG(INFO) << "(new)ECEF GPS L2M satellite SV ID=" << gps_cnav_ephemeris_iter->second.i_satellite_PRN + << " TX Time corrected="< gnss_observables_map, dou DLOG(INFO) << "Hybrid Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time; DLOG(INFO) << "Accumulated rx clock error=" << d_rx_dt_s << " clock error for this iteration=" << rx_position_and_time(3) / GPS_C_m_s << " [s]"; - double secondsperweek = 604800.0; // Compute GST and Gregorian time if( GST != 0.0) { diff --git a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc index ed75b4c46..aab1e30c8 100644 --- a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc @@ -252,12 +252,12 @@ int galileo_e1_observables_cc::general_work (int noutput_items, gr_vector_int &n { tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol; d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = current_gnss_synchro[i].Prn_timestamp_ms; + tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz; + d_dump_file.write((char*)&tmp_double, sizeof(double)); + tmp_double = current_gnss_synchro[i].Carrier_phase_rads/GALILEO_TWO_PI; d_dump_file.write((char*)&tmp_double, sizeof(double)); tmp_double = current_gnss_synchro[i].Pseudorange_m; d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = (double)(current_gnss_synchro[i].Flag_valid_pseudorange==true); - d_dump_file.write((char*)&tmp_double, sizeof(double)); tmp_double = current_gnss_synchro[i].PRN; d_dump_file.write((char*)&tmp_double, sizeof(double)); } diff --git a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc index 91629e23b..db73b50dc 100644 --- a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc @@ -249,7 +249,6 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni { tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol; d_dump_file.write((char*)&tmp_double, sizeof(double)); - //tmp_double = current_gnss_synchro[i].Prn_timestamp_ms; tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz; d_dump_file.write((char*)&tmp_double, sizeof(double)); tmp_double = current_gnss_synchro[i].Carrier_phase_rads/GPS_TWO_PI; diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 90fd8c6be..4366093ba 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -264,16 +264,16 @@ int hybrid_observables_cc::general_work (int noutput_items, { tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol; d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = current_gnss_synchro[i].d_TOW_hybrid_at_current_symbol; + //tmp_double = current_gnss_synchro[i].Prn_timestamp_ms; + tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz; d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = current_gnss_synchro[i].Prn_timestamp_ms; + tmp_double = current_gnss_synchro[i].Carrier_phase_rads/GPS_TWO_PI; d_dump_file.write((char*)&tmp_double, sizeof(double)); tmp_double = current_gnss_synchro[i].Pseudorange_m; d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = (double)(current_gnss_synchro[i].Flag_valid_pseudorange==true); - d_dump_file.write((char*)&tmp_double, sizeof(double)); tmp_double = current_gnss_synchro[i].PRN; d_dump_file.write((char*)&tmp_double, sizeof(double)); + } } catch (const std::ifstream::failure& e) diff --git a/src/algorithms/telemetry_decoder/adapters/CMakeLists.txt b/src/algorithms/telemetry_decoder/adapters/CMakeLists.txt index 20095b8fe..24c7374f1 100644 --- a/src/algorithms/telemetry_decoder/adapters/CMakeLists.txt +++ b/src/algorithms/telemetry_decoder/adapters/CMakeLists.txt @@ -31,6 +31,7 @@ include_directories( ${CMAKE_SOURCE_DIR}/src/core/receiver ${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/gnuradio_blocks ${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/libs + ${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/libs/libswiftcnav ${Boost_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} ${GFlags_INCLUDE_DIRS} diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt b/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt index 29aafa1b8..e7c8aee05 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt @@ -29,6 +29,7 @@ include_directories( ${CMAKE_SOURCE_DIR}/src/core/system_parameters ${CMAKE_SOURCE_DIR}/src/core/receiver ${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/libs + ${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/libs/libswiftcnav ${GLOG_INCLUDE_DIRS} ${GFlags_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} @@ -39,4 +40,4 @@ file(GLOB TELEMETRY_DECODER_GR_BLOCKS_HEADERS "*.h") list(SORT TELEMETRY_DECODER_GR_BLOCKS_HEADERS) add_library(telemetry_decoder_gr_blocks ${TELEMETRY_DECODER_GR_BLOCKS_SOURCES} ${TELEMETRY_DECODER_GR_BLOCKS_HEADERS}) source_group(Headers FILES ${TELEMETRY_DECODER_GR_BLOCKS_HEADERS}) -target_link_libraries(telemetry_decoder_gr_blocks telemetry_decoder_lib gnss_system_parameters ${GNURADIO_RUNTIME_LIBRARIES}) +target_link_libraries(telemetry_decoder_gr_blocks telemetry_decoder_libswiftcnav telemetry_decoder_lib gnss_system_parameters ${GNURADIO_RUNTIME_LIBRARIES}) \ No newline at end of file diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc index e7fc300af..b7119a7a5 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc @@ -29,6 +29,7 @@ * ------------------------------------------------------------------------- */ +#include #include #include #include @@ -39,14 +40,6 @@ using google::LogMessage; -// logging levels -#define EVENT 2 // logs important events which don't occur every block -#define FLOW 3 // logs the function calls of block processing functions -#define SAMP_SYNC 4 // about 1 log entry per sample -> high output -#define LMORE 5 // - - - gps_l2c_telemetry_decoder_cc_sptr gps_l2c_make_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump) { @@ -56,9 +49,7 @@ gps_l2c_make_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump) gps_l2c_telemetry_decoder_cc::gps_l2c_telemetry_decoder_cc( - Gnss_Satellite satellite, - bool dump) : - gr::block("gps_l2c_telemetry_decoder_cc", + Gnss_Satellite satellite, bool dump) : gr::block("gps_l2c_telemetry_decoder_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { @@ -69,18 +60,19 @@ gps_l2c_telemetry_decoder_cc::gps_l2c_telemetry_decoder_cc( // initialize internal vars d_dump = dump; d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); - LOG(INFO) << "GPS L2C M TELEMETRY PROCESSING: satellite " << d_satellite; - d_block_size = GPS_L2_SAMPLES_PER_SYMBOL * GPS_L2_SYMBOLS_PER_BIT * GPS_L2_CNAV_DATA_PAGE_BITS * 2; // two CNAV frames + DLOG(INFO) << "GPS L2C M TELEMETRY PROCESSING: satellite " << d_satellite; d_decimation_output_factor = 0; //set_output_multiple (1); d_average_count = 0; - d_flag_invert_buffer_symbols = false; - d_flag_invert_input_symbols = false; d_channel = 0; d_flag_valid_word = false; d_TOW_at_current_symbol = 0; d_TOW_at_Preamble = 0; - //set_history(d_samples_per_bit*8); // At least a history of 8 bits are needed to correlate with the preamble + d_state=0; //initial state + d_crc_error_count=0; + + //initialize the CNAV frame decoder (libswiftcnav) + cnav_msg_decoder_init(&d_cnav_decoder); } @@ -91,18 +83,6 @@ gps_l2c_telemetry_decoder_cc::~gps_l2c_telemetry_decoder_cc() } -void gps_l2c_telemetry_decoder_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required) -{ - if (noutput_items != 0) - { - unsigned ninputs = ninput_items_required.size (); - for (unsigned i = 0; i < ninputs; i++) - ninput_items_required[i] = noutput_items; - } - //LOG(INFO) << "forecast(): " << "noutput_items=" << noutput_items << "\tninput_items_required ninput_items_required.size()=" << ninput_items_required.size(); -} - - void gps_l2c_telemetry_decoder_cc::set_decimation(int decimation) { d_decimation_output_factor = decimation; @@ -117,161 +97,123 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__( Gnss_Synchro *out = (Gnss_Synchro *) output_items[0]; // output bool flag_new_cnav_frame = false; - int last_frame_preamble_start = 0; - // copy correlation samples into samples vector - d_sample_buf.push_back(in[0].Prompt_I); + cnav_msg_t msg; + u32 delay = 0; + + //add the symbol to the decoder + u8 symbol_clip=(u8)(in[0].Prompt_I>0) * 255; + flag_new_cnav_frame = cnav_msg_decoder_add_symbol(&d_cnav_decoder, symbol_clip, &msg, &delay); consume_each(1); //one by one - // decode only if enough samples in buffer - if(d_sample_buf.size() >= d_block_size) - { - if (in[0].Flag_valid_symbol_output == false) // check if the tracking is locked - { - LOG(INFO) << "Discarting channel " << d_channel << " tracking not ready!" << std::endl; - d_flag_valid_word = false; - } - else - { - d_flag_invert_buffer_symbols = d_flag_invert_input_symbols; - while (true) - { - if (d_flag_invert_buffer_symbols == true) - { - for (std::vector::iterator symbol_it = d_sample_buf.begin(); symbol_it != d_sample_buf.end(); symbol_it++) - { - *symbol_it = -(*symbol_it); - } - //LOG(INFO)<<"Inverting buffer symbols"; - } - // align symbols in pairs - // and obtain the bits by decoding the symbols (viterbi decoder) - // they can be already aligned or shifted by one position - std::vector bits; - - d_symbol_aligner_and_decoder.get_bits(d_sample_buf, bits); - - //std::stringstream ss; - //for (std::vector::const_iterator bit_it = bits.begin(); bit_it < bits.end(); ++bit_it) - // { - // ss << *bit_it; - // } - //LOG(INFO) << "get_bits=" << ss.str() << std::endl; - - // search for preambles - // and extract the corresponding message candidates - std::vector msg_candidates; - d_frame_detector.get_frame_candidates(bits, msg_candidates); - - // verify checksum - // and return the valid messages - std::vector valid_msgs; - d_crc_verifier.get_valid_frames(msg_candidates, valid_msgs); - if (valid_msgs.size() == 0) - { - if (d_flag_invert_buffer_symbols == d_flag_invert_input_symbols) - { - d_flag_invert_buffer_symbols = not d_flag_invert_buffer_symbols; - } - else - {//already tested the symbol inversion but CRC still fail - LOG(INFO) << "Discarting this buffer, no CNAV frames detected CH " << this->d_channel; - break; - } - } - else - { //at least one frame has good CRC, keep the invert sign for the next frames - d_flag_invert_input_symbols = d_flag_invert_buffer_symbols; - std::vector tmp_msg; - std::string msg; - //todo: now the symbol buffer size is two CNAV frames because the preamble is not detected. - // Use the first valid frame to realign the bufer symbols with the preamble start and not miss a frame - LOG(INFO) << valid_msgs.size() << " GOOD L2C CNAV FRAME DETECTED! CH " <d_channel; - for (unsigned int i = 0;i < valid_msgs.size(); i++) - { - tmp_msg = valid_msgs.at(i).second; - d_CNAV_Message.decode_page(tmp_msg); - std::cout << "Valid CNAV frame with relative preamble start at " << valid_msgs.at(i).first << std::endl; - flag_new_cnav_frame = true; - d_flag_valid_word = true; - last_frame_preamble_start = valid_msgs.at(i).first; - // 4. Push the new navigation data to the queues - if (d_CNAV_Message.have_new_ephemeris() == true) - { - // get ephemeris object for this SV - std::shared_ptr tmp_obj= std::make_shared(d_CNAV_Message.get_ephemeris()); - std::cout << "New GPS CNAV Ephemeris received for SV " << tmp_obj->i_satellite_PRN << std::endl; - this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); - - } - if (d_CNAV_Message.have_new_iono() == true) - { - std::shared_ptr tmp_obj= std::make_shared(d_CNAV_Message.get_iono()); - std::cout << "New GPS CNAV IONO model received for SV " << d_satellite.get_PRN() << std::endl; - this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); - } - } - break; - } - } - } - // clear all processed samples in the input buffer - d_sample_buf.clear(); - } - // UPDATE GNSS SYNCHRO DATA Gnss_Synchro current_synchro_data; //structure to save the synchronization information and send the output object to the next block //1. Copy the current tracking output current_synchro_data = in[0]; - if (d_flag_valid_word == true) - { - double Prn_timestamp_at_preamble_ms = 0; - //2. Add the telemetry decoder information - if (flag_new_cnav_frame == true) - { - //update TOW at the preamble instant - Prn_timestamp_at_preamble_ms = (in[0].Tracking_timestamp_secs * 1000.0) - (d_block_size - last_frame_preamble_start) * GPS_L2_M_PERIOD; - d_TOW_at_Preamble = d_CNAV_Message.d_TOW - GPS_L2_CNAV_DATA_PAGE_DURATION_S; - d_TOW_at_current_symbol = d_TOW_at_Preamble + (d_block_size - last_frame_preamble_start) * GPS_L2_M_PERIOD; - current_synchro_data.d_TOW = d_TOW_at_Preamble; - current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol; - current_synchro_data.d_TOW_hybrid_at_current_symbol = current_synchro_data.d_TOW_at_current_symbol; - current_synchro_data.Flag_preamble = false; - current_synchro_data.Prn_timestamp_ms = in[0].Tracking_timestamp_secs * 1000.0; - current_synchro_data.Prn_timestamp_at_preamble_ms = Prn_timestamp_at_preamble_ms; - } - else - { - d_TOW_at_current_symbol = d_TOW_at_Preamble + (d_block_size - last_frame_preamble_start) * GPS_L2_M_PERIOD; - current_synchro_data.d_TOW = d_TOW_at_Preamble; - current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol; - current_synchro_data.d_TOW_hybrid_at_current_symbol = current_synchro_data.d_TOW_at_current_symbol; - current_synchro_data.Flag_preamble = false; - current_synchro_data.Prn_timestamp_ms = in[0].Tracking_timestamp_secs * 1000.0; - current_synchro_data.Prn_timestamp_at_preamble_ms = Prn_timestamp_at_preamble_ms; - } - current_synchro_data.Flag_valid_word = true; + //2. Add the telemetry decoder information + //check if new CNAV frame is available + if (flag_new_cnav_frame == true) + { + std::bitset raw_bits; + //Expand packet bits to bitsets. Notice the reverse order of the bits sequence, required by the CNAV message decoder + for (u32 i = 0; i < GPS_L2_CNAV_DATA_PAGE_BITS ; i++) { + raw_bits[GPS_L2_CNAV_DATA_PAGE_BITS-1-i]=((msg.raw_msg[i/8] >> (7 - i%8)) & 1u); } + + d_CNAV_Message.decode_page(raw_bits); + + //Push the new navigation data to the queues + if (d_CNAV_Message.have_new_ephemeris() == true) + { + // get ephemeris object for this SV + std::shared_ptr tmp_obj= std::make_shared(d_CNAV_Message.get_ephemeris()); + std::cout << "New GPS CNAV Ephemeris received for SV " << tmp_obj->i_satellite_PRN << std::endl; + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + + } + if (d_CNAV_Message.have_new_iono() == true) + { + std::shared_ptr tmp_obj= std::make_shared(d_CNAV_Message.get_iono()); + std::cout << "New GPS CNAV IONO model received for SV " << d_satellite.get_PRN() << std::endl; + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + } + + if (d_CNAV_Message.have_new_utc_model() == true) + { + std::shared_ptr tmp_obj= std::make_shared(d_CNAV_Message.get_utc_model()); + std::cout << "New GPS CNAV UTC model received for SV " << d_satellite.get_PRN() << std::endl; + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + } + + //update TOW at the preamble instant + double Prn_timestamp_at_preamble_ms = (in[0].Tracking_timestamp_secs * 1000.0); + d_TOW_at_Preamble=(int)msg.tow; + current_synchro_data.d_TOW = d_TOW_at_Preamble; + std::cout<<"["<<(int)msg.prn<<"] deco delay: "<reset(); - d_vd2->reset(); -} - - -bool gps_l2c_telemetry_decoder_cc::symbol_aligner_and_decoder::get_bits(const std::vector & symbols, std::vector & bits) -{ - const int traceback_depth = 5 * d_KK; - int nbits_requested = symbols.size() / GPS_L2_SYMBOLS_PER_BIT; - int nbits_decoded; - // fill two vectors with the two possible symbol alignments - std::vector symbols_vd1(symbols); // aligned symbol vector -> copy input symbol vector - std::vector symbols_vd2; // shifted symbol vector -> add past sample in front of input vector - symbols_vd2.push_back(d_past_symbol); - for (std::vector::const_iterator symbol_it = symbols.begin(); symbol_it != symbols.end() - 1; ++symbol_it) + // ############# ENABLE DATA FILE LOG ################# + if (d_dump == true) { - symbols_vd2.push_back(*symbol_it); - } - // arrays for decoded bits - int * bits_vd1 = new int[nbits_requested]; - int * bits_vd2 = new int[nbits_requested]; - // decode - float metric_vd1 = d_vd1->decode_continuous(symbols_vd1.data(), traceback_depth, bits_vd1, nbits_requested, nbits_decoded); - float metric_vd2 = d_vd2->decode_continuous(symbols_vd2.data(), traceback_depth, bits_vd2, nbits_requested, nbits_decoded); - //LOG(INFO)<<"metric_vd1="<::const_iterator bit_it = bits.begin(); bit_it < bits.end(); ++bit_it) - { - d_buffer.push_back(*bit_it); - //ss << *bit_it; - count++; - } - //LOG(INFO) << ss.str() << " into working buffer (" << count << " bits)"; - int relative_preamble_start = 0; - while(d_buffer.size() >= cnav_msg_length) - { - // compare with all preambles - for (std::vector>::iterator preample_it = preambles.begin(); preample_it < preambles.end(); ++preample_it) + if (d_dump_file.is_open() == false) { - bool preamble_detected = true; - bool inv_preamble_detected = true; - // compare the buffer bits with the preamble bits - for (std::vector::iterator preample_bit_it = preample_it->begin(); preample_bit_it < preample_it->end(); ++preample_bit_it) - { - preamble_detected = *preample_bit_it == d_buffer[preample_bit_it - preample_it->begin()] ? preamble_detected : false ; - inv_preamble_detected = *preample_bit_it != d_buffer[preample_bit_it - preample_it->begin()] ? inv_preamble_detected : false ; - } - if (preamble_detected || inv_preamble_detected) - { - // copy candidate - std::vector candidate; - std::copy(d_buffer.begin(), d_buffer.begin() + cnav_msg_length, std::back_inserter(candidate)); - if(inv_preamble_detected) - { - // invert bits - for (std::vector::iterator candidate_bit_it = candidate.begin(); candidate_bit_it != candidate.end(); candidate_bit_it++) - *candidate_bit_it = *candidate_bit_it == 0 ? 1 : 0; - } - msg_candidates.push_back(std::pair>(relative_preamble_start, candidate)); - //ss.str(""); - //ss << "preamble " << preample_it - preambles.begin() << (inv_preamble_detected?" inverted":" normal") << " detected! candidate="; - //for (std::vector::iterator bit_it = candidate.begin(); bit_it < candidate.end(); ++bit_it) - // ss << *bit_it; - //LOG(INFO) << ss.str(); - } - } - relative_preamble_start++; - // remove bit in front - d_buffer.pop_front(); - } -} - - -// ### helper class for checking the CRC of the message candidates ### - -void gps_l2c_telemetry_decoder_cc::crc_verifier::reset() -{ - -} - -void gps_l2c_telemetry_decoder_cc::crc_verifier::get_valid_frames(const std::vector & msg_candidates, std::vector & valid_msgs) -{ - std::vector tmp_msg; - LOG(INFO) << "get_valid_frames(): " << "msg_candidates.size()=" << msg_candidates.size(); - // for each candidate - for (std::vector::const_iterator candidate_it = msg_candidates.begin(); candidate_it < msg_candidates.end(); ++candidate_it) - { - // convert to bytes - std::vector candidate_bytes; - zerropad_back_and_convert_to_bytes(candidate_it->second, candidate_bytes); - // verify CRC - d_checksum_agent.reset(0); - d_checksum_agent.process_bytes(candidate_bytes.data(), candidate_bytes.size()); - unsigned int crc = d_checksum_agent.checksum(); - //LOG(INFO) << "candidate " << ": final crc remainder= " << std::hex << crc - // << std::setfill(' ') << std::resetiosflags(std::ios::hex); - // the final remainder must be zero for a valid message, because the CRC is done over the received CRC value - if (crc == 0) - { - valid_msgs.push_back(msg_candiate_int_t(candidate_it->first, candidate_it->second)); - std::cout << "Valid CNAV message found!"<(d_channel)); + d_dump_filename.append(".dat"); + d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit ); + d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "Telemetry decoder dump enabled on channel " << d_channel + << " Log file: " << d_dump_filename.c_str(); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); + } } } } - - -void gps_l2c_telemetry_decoder_cc::crc_verifier::zerropad_back_and_convert_to_bytes(const std::vector & msg_candidate, std::vector & bytes) -{ - //std::stringstream ss; - const size_t bits_per_byte = 8; - unsigned char byte = 0; - //LOG(INFO) << "zerropad_back_and_convert_to_bytes():" << byte; - for (std::vector::const_iterator candidate_bit_it = msg_candidate.begin(); candidate_bit_it < msg_candidate.end(); ++candidate_bit_it) - { - int idx_bit = candidate_bit_it - msg_candidate.begin(); - int bit_pos_in_current_byte = (bits_per_byte - 1) - (idx_bit % bits_per_byte); - byte |= (unsigned char)(*candidate_bit_it) << bit_pos_in_current_byte; - // ss << *candidate_bit_it; - if (idx_bit % bits_per_byte == bits_per_byte - 1) - { - bytes.push_back(byte); - //LOG(INFO) << ss.str() << " -> byte=" << std::setw(2) << std::setfill('0') << std::hex << (unsigned int)byte; ss.str(""); - byte = 0; - } - } - bytes.push_back(byte); // implies: insert 6 zeros at the end to fit the 250bits into a multiple of bytes - //LOG(INFO) << " -> byte=" << std::setw(2) - // << std::setfill('0') << std::hex << (unsigned int)byte - // << std::setfill(' ') << std::resetiosflags(std::ios::hex); -} - -void gps_l2c_telemetry_decoder_cc::crc_verifier::zerropad_front_and_convert_to_bytes(const std::vector & msg_candidate, std::vector & bytes) -{ - //std::stringstream ss; - const size_t bits_per_byte = 8; - unsigned char byte = 0; - int idx_bit = 6; // insert 6 zeros at the front to fit the 250bits into a multiple of bytes - //LOG(INFO) << "zerropad_front_and_convert_to_bytes():" << byte; - for (std::vector::const_iterator candidate_bit_it = msg_candidate.begin(); candidate_bit_it < msg_candidate.end(); ++candidate_bit_it) - { - int bit_pos_in_current_byte = (bits_per_byte - 1) - (idx_bit % bits_per_byte); - byte |= (unsigned char)(*candidate_bit_it) << bit_pos_in_current_byte; - // ss << *candidate_bit_it; - if (idx_bit % bits_per_byte == bits_per_byte - 1) - { - bytes.push_back(byte); - //LOG(INFO) << ss.str() << " -> byte=" << std::setw(2) - // << std::setfill('0') << std::hex << (unsigned int)byte; ss.str(""); - byte = 0; - } - idx_bit++; - } - //LOG(INFO) << " -> byte=" << std::setw(2) - // << std::setfill('0') << std::hex << (unsigned int)byte - // << std::setfill(' ') << std::resetiosflags(std::ios::hex); -} - diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.h index 2b0ce1b5b..3e93b0b3f 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.h @@ -37,14 +37,19 @@ #include #include // for pair #include -#include #include #include "gnss_satellite.h" -#include "viterbi_decoder.h" #include "gps_cnav_navigation_message.h" #include "gps_cnav_ephemeris.h" #include "gps_cnav_iono.h" #include "concurrent_queue.h" + +extern "C" { + #include "cnav_msg.h" + #include "edc.h" + #include "bits.h" +} + #include "GPS_L2C.h" class gps_l2c_telemetry_decoder_cc; @@ -72,19 +77,13 @@ public: int general_work (int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); - /*! - * \brief Function which tells the scheduler how many input items - * are required to produce noutput_items output items. - */ - void forecast (int noutput_items, gr_vector_int &ninput_items_required); private: friend gps_l2c_telemetry_decoder_cc_sptr gps_l2c_make_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump); gps_l2c_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump); - void viterbi_decoder(double *page_part_symbols, int *page_part_bits); - void align_samples(); + bool d_dump; Gnss_Satellite d_satellite; @@ -93,59 +92,17 @@ private: std::string d_dump_filename; std::ofstream d_dump_file; + cnav_msg_decoder_t d_cnav_decoder; + + int d_state; + int d_crc_error_count; + double d_TOW_at_current_symbol; double d_TOW_at_Preamble; bool d_flag_valid_word; - bool d_flag_invert_input_symbols; - bool d_flag_invert_buffer_symbols; int d_decimation_output_factor; int d_average_count; - size_t d_block_size; //!< number of samples which are processed during one invocation of the algorithms - std::vector d_sample_buf; //!< input buffer holding the samples to be processed in one block - - typedef std::pair> msg_candiate_int_t; - typedef std::pair> msg_candiate_char_t; - - // helper class for symbol alignment and Viterbi decoding - class symbol_aligner_and_decoder - { - public: - symbol_aligner_and_decoder(); - ~symbol_aligner_and_decoder(); - void reset(); - bool get_bits(const std::vector & symbols, std::vector & bits); - private: - int d_KK; - Viterbi_Decoder * d_vd1; - Viterbi_Decoder * d_vd2; - double d_past_symbol; - } d_symbol_aligner_and_decoder; - - - // helper class for detecting the preamble and collect the corresponding message candidates - class frame_detector - { - public: - void reset(); - void get_frame_candidates(const std::vector & bits, std::vector>> & msg_candidates); - private: - std::deque d_buffer; - } d_frame_detector; - - - // helper class for checking the CRC of the message candidates - class crc_verifier - { - public: - void reset(); - void get_valid_frames(const std::vector & msg_candidates, std::vector & valid_msgs); - private: - typedef boost::crc_optimal<24, 0x1864CFBu, 0x0, 0x0, false, false> crc_24_q_type; - crc_24_q_type d_checksum_agent; - void zerropad_front_and_convert_to_bytes(const std::vector & msg_candidate, std::vector & bytes); - void zerropad_back_and_convert_to_bytes(const std::vector & msg_candidate, std::vector & bytes); - } d_crc_verifier; Gps_CNAV_Navigation_Message d_CNAV_Message; }; diff --git a/src/algorithms/telemetry_decoder/libs/CMakeLists.txt b/src/algorithms/telemetry_decoder/libs/CMakeLists.txt index 9b5ab2c74..091283038 100644 --- a/src/algorithms/telemetry_decoder/libs/CMakeLists.txt +++ b/src/algorithms/telemetry_decoder/libs/CMakeLists.txt @@ -16,9 +16,11 @@ # along with GNSS-SDR. If not, see . # +add_subdirectory(libswiftcnav) + set(TELEMETRY_DECODER_LIB_SOURCES gps_l1_ca_subframe_fsm.cc - viterbi_decoder.cc + viterbi_decoder.cc ) include_directories( @@ -33,7 +35,8 @@ include_directories( ) file(GLOB TELEMETRY_DECODER_LIB_HEADERS "*.h") + list(SORT TELEMETRY_DECODER_LIB_HEADERS) add_library(telemetry_decoder_lib ${TELEMETRY_DECODER_LIB_SOURCES} ${TELEMETRY_DECODER_LIB_HEADERS}) source_group(Headers FILES ${TELEMETRY_DECODER_LIB_HEADERS}) -target_link_libraries(telemetry_decoder_lib gnss_system_parameters) \ No newline at end of file +target_link_libraries(telemetry_decoder_lib gnss_system_parameters) diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/CMakeLists.txt b/src/algorithms/telemetry_decoder/libs/libswiftcnav/CMakeLists.txt new file mode 100644 index 000000000..1ef372aa8 --- /dev/null +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/CMakeLists.txt @@ -0,0 +1,35 @@ +# Copyright (C) 2012-2015 (see AUTHORS file for a list of contributors) +# +# This file is part of GNSS-SDR. +# +# GNSS-SDR 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. +# +# GNSS-SDR 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 GNSS-SDR. If not, see . +# + +set(TELEMETRY_DECODER_LIBSWIFTCNAV_SOURCES + cnav_msg.c + bits.c + edc.c + viterbi27.c +) + +include_directories( + $(CMAKE_CURRENT_SOURCE_DIR) +) + +file(GLOB TELEMETRY_DECODER_LIBSWIFTCNAV_HEADERS "*.h") +list(SORT TELEMETRY_DECODER_LIBSWIFTCNAV_HEADERS) + +add_library(telemetry_decoder_libswiftcnav STATIC ${TELEMETRY_DECODER_LIBSWIFTCNAV_SOURCES} ${TELEMETRY_DECODER_LIBSWIFTCNAV_HEADERS}) +source_group(Headers FILES ${TELEMETRY_DECODER_LIBSWIFTCNAV_HEADERS}) +set_target_properties(telemetry_decoder_libswiftcnav PROPERTIES LINKER_LANGUAGE C) \ No newline at end of file diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c new file mode 100644 index 000000000..b8d4ff832 --- /dev/null +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c @@ -0,0 +1,268 @@ +/* + * Copyright (C) 2013, 2016 Swift Navigation Inc. + * Contact: Fergus Noble + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include "bits.h" + +#include +#include + + +static const u8 bitn[16] = {0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4}; + +/** \defgroup bits Bit Utils + * Bit field packing, unpacking and utility functions. + * \{ */ + +/** Computes the parity of a 32-bit word. + * + * References: + * -# https://graphics.stanford.edu/~seander/bithacks.html#ParityParallel + * + * \param x Word for which to compute parity + * \return 1 if there are an odd number of bits set. + * 0 if there are an even number of bits set. + */ +u8 parity(u32 x) +{ + x ^= x >> 16; + x ^= x >> 8; + x ^= x >> 4; + x &= 0xF; + return (0x6996 >> x) & 1; +} + + +/** Get bit field from buffer as an unsigned integer. + * Unpacks `len` bits at bit position `pos` from the start of the buffer. + * Maximum bit field length is 32 bits, i.e. `len <= 32`. + * + * \param buff + * \param pos Position in buffer of start of bit field in bits. + * \param len Length of bit field in bits. + * \return Bit field as an unsigned value. + */ +u32 getbitu(const u8 *buff, u32 pos, u8 len) +{ + u32 bits = 0; + + for (u32 i = pos; i < pos + len; i++) { + bits = (bits << 1) + + ((buff[i/8] >> (7 - i%8)) & 1u); + } + + return bits; +} + +/** Get bit field from buffer as a signed integer. + * Unpacks `len` bits at bit position `pos` from the start of the buffer. + * Maximum bit field length is 32 bits, i.e. `len <= 32`. + * + * This function sign extends the `len` bit field to a signed 32 bit integer. + * + * \param buff + * \param pos Position in buffer of start of bit field in bits. + * \param len Length of bit field in bits. + * \return Bit field as a signed value. + */ +s32 getbits(const u8 *buff, u32 pos, u8 len) +{ + s32 bits = (s32)getbitu(buff, pos, len); + + /* Sign extend, taken from: + * http://graphics.stanford.edu/~seander/bithacks.html#VariableSignExtend + */ + s32 m = 1u << (len - 1); + return (bits ^ m) - m; +} + +/** Set bit field in buffer from an unsigned integer. + * Packs `len` bits into bit position `pos` from the start of the buffer. + * Maximum bit field length is 32 bits, i.e. `len <= 32`. + * + * \param buff + * \param pos Position in buffer of start of bit field in bits. + * \param len Length of bit field in bits. + * \param data Unsigned integer to be packed into bit field. + */ +void setbitu(u8 *buff, u32 pos, u32 len, u32 data) +{ + u32 mask = 1u << (len - 1); + + if (len <= 0 || 32 < len) + return; + + for (u32 i = pos; i < pos + len; i++, mask >>= 1) { + if (data & mask) + buff[i/8] |= 1u << (7 - i % 8); + else + buff[i/8] &= ~(1u << (7 - i % 8)); + } +} + +/** Set bit field in buffer from a signed integer. + * Packs `len` bits into bit position `pos` from the start of the buffer. + * Maximum bit field length is 32 bits, i.e. `len <= 32`. + * + * \param buff + * \param pos Position in buffer of start of bit field in bits. + * \param len Length of bit field in bits. + * \param data Signed integer to be packed into bit field. + */ +void setbits(u8 *buff, u32 pos, u32 len, s32 data) +{ + setbitu(buff, pos, len, (u32)data); +} + +/** + * Shift MSB bit buffer contents to the left. + * The method performs in-place shift operation. + * + * \param[in,out] buf Pointer to buffer head. + * \param[in] size Number of bytes in the buffer. + * \param[in] shift Number of bits for left shift operation. + * + * \return None + */ +void bitshl(void *buf, u32 size, u32 shift) +{ + if (shift > size * CHAR_BIT) { + /* Quick check: if the shift is larger, than the buffer, zero the data */ + memset(buf, 0, size); + return; + } + + unsigned char *dst = buf; /* Destination byte. */ + const unsigned char *src = dst + shift / CHAR_BIT; /* First source byte, + * possibly incomplete. */ + + u32 copy_bits = size * CHAR_BIT - shift; /* Number of bits to move */ + u32 byte_shift = copy_bits % CHAR_BIT; /* Shift of data */ + u32 full_bytes = copy_bits / CHAR_BIT; /* Number of bytes to move */ + + if (0 == byte_shift) { + /* When moving data in character boundaries, use built-in functions: move + * data, and then zero the tail. */ + memmove(dst, src, full_bytes); + memset(dst + full_bytes, 0, size - full_bytes); + } else { + /* Create an accumulator: it will hold a value of two consecutive bytes */ + u32 acc = *src++; + for (u32 i = 0; i < full_bytes; ++i) { + acc = (acc << CHAR_BIT) | *src++; + *dst++ = acc >> byte_shift; + } + *dst++ = acc << CHAR_BIT >> byte_shift; + if (full_bytes + 1 < size) { + memset(dst, 0, size - full_bytes - 1); + } + } +} + +/** + * Performs block bit copy operation. + * + * The function copies given number of bits from the source to destination. + * + * \param[in,out] dst Pointer to destination buffer. + * \param[in] dst_index Destination bit index. + * \param[in] src Source buffer. + * \param[in] src_index Source bit index. + * \param[in] count Number of bits to copy. + * + * \return None + * + * \todo This function can be optimized for copying aligned data and using + * proper native type like long. + */ +void bitcopy(void *dst, u32 dst_index, const void *src, u32 src_index, + u32 count) +{ + u32 limit1 = count / 32; + u32 limit2 = count % 32; + + for (u32 idx = 0; idx < limit1; ++idx) { + u32 tmp = getbitu(src, src_index, 32); + setbitu(dst, dst_index, 32, tmp); + src_index += 32; + dst_index += 32; + } + if (0 != limit2) { + u32 tmp = getbitu(src, src_index, limit2); + setbitu(dst, dst_index, limit2, tmp); + } +} + +/** + * Count number of bits set to certain value in 64 bits word + * + * \param[in] v input 64 bits word to count bits in + * \param[in] bv 1 or 0 - which value to count + * + * \return Number of bits set to one or zero. + */ +u8 count_bits_u64(u64 v, u8 bv) +{ + u8 r = 0; + for (int i = 0; i < 16; i++) + r += bitn[(v >> (i*4)) & 0xf]; + return bv == 1 ? r : 64 - r; +} + +/** + * Count number of bits set to certain value in 32 bits word + * + * \param[in] v input 32 bits word to count bits in + * \param[in] bv 1 or 0 - which value to count + * + * \return Number of bits set to one or zero. + */ +u8 count_bits_u32(u32 v, u8 bv) +{ + u8 r = 0; + for (int i = 0; i < 8; i++) + r += bitn[(v >> (i*4)) & 0xf]; + return bv == 1 ? r : 32 - r; +} + +/** + * Count number of bits set to certain value in 16 bits word + * + * \param[in] v input 16 bits word to count bits in + * \param[in] bv 1 or 0 - which value to count + * + * \return Number of bits set to one or zero. + */ +u8 count_bits_u16(u16 v, u8 bv) +{ + u8 r = 0; + for (int i = 0; i < 4; i++) + r += bitn[(v >> (i*4)) & 0xf]; + return bv == 1 ? r : 16 - r; +} + +/** + * Count number of bits set to certain value in 8 bits word + * + * \param[in] v input 8 bits word to count bits in + * \param[in] bv 1 or 0 - which value to count + * + * \return Number of bits set to one or zero. + */ +u8 count_bits_u8(u8 v, u8 bv) +{ + u8 r = 0; + for (int i = 0; i < 2; i++) + r += bitn[(v >> (i*4)) & 0xf]; + return bv == 1 ? r : 8 - r; +} + +/** \} */ diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.h b/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.h new file mode 100644 index 000000000..96ae12b7d --- /dev/null +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.h @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2013, 2016 Swift Navigation Inc. + * Contact: Fergus Noble + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#ifndef LIBSWIFTNAV_BITS_H +#define LIBSWIFTNAV_BITS_H + +#include "swift_common.h" + +u8 parity(u32 x); +u32 getbitu(const u8 *buff, u32 pos, u8 len); +s32 getbits(const u8 *buff, u32 pos, u8 len); +void setbitu(u8 *buff, u32 pos, u32 len, u32 data); +void setbits(u8 *buff, u32 pos, u32 len, s32 data); +void bitcopy(void *dst, u32 dst_index, + const void *src, u32 src_index, u32 count); +void bitshl(void *buf, u32 size, u32 shift); +u8 count_bits_u64(u64 v, u8 bv); +u8 count_bits_u32(u32 v, u8 bv); +u8 count_bits_u16(u16 v, u8 bv); +u8 count_bits_u8(u8 v, u8 bv); + +#endif /* LIBSWIFTNAV_BITS_H */ diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c new file mode 100644 index 000000000..e8c733ebd --- /dev/null +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c @@ -0,0 +1,432 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Valeri Atamaniouk + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include "edc.h" +#include "bits.h" +#include "cnav_msg.h" + +#include +#include + +/** \defgroup GPS_L2 GPS L2 + * GPS L2 operations + * \{ */ +/** \defgroup gps_cnav_decoder Decoder + * GPS L2C CNAV message decoding. + * \{ */ +/* + * Block Viterbi decoding parameters. + */ +/** Viterbi decoder reversed polynomial A */ +#define GPS_L2C_V27_POLY_A (0x4F) /* 0b01001111 - reversed 0171*/ +/** Viterbi decoder reversed polynomial B */ +#define GPS_L2C_V27_POLY_B (0x6D) /* 0b01101101 - reversed 0133 */ + +/* + * GPS L2C message constants. + */ + +/** GPS L2C preamble */ +#define GPS_CNAV_PREAMBLE1 (0b10001011u) +/** Inverted GPS L2C preamble */ +#define GPS_CNAV_PREAMBLE2 (0b01110100u) +/** GPS L2C preamble length in bits */ +#define GPS_CNAV_PREAMBLE_LENGTH (8) +/** GPS L2C CNAV message length in bits */ +#define GPS_CNAV_MSG_LENGTH (300) +/** GPS LC2 CNAV CRC length in bits */ +#define GPS_CNAV_MSG_CRC_LENGTH (24) +/** GPS L2C CNAV message payload length in bits */ +#define GPS_CNAV_MSG_DATA_LENGTH (GPS_CNAV_MSG_LENGTH-GPS_CNAV_MSG_CRC_LENGTH) +/** GPS L2C CNAV message lock detector threshold */ +#define GPS_CNAV_LOCK_MAX_CRC_FAILS (10) + +/** + * Computes CRC-24Q from a CNAV message buffer. + * CRC-24Q is computed for 274 bits. For a purpose of 8-bit alignment, the + * message is assumed to be prepended with four zero bits. + * + * \param[in] part Decoder component with payload + * + * \return CRC-24Q value. + * + * \private + */ +static u32 _cnav_compute_crc(cnav_v27_part_t *part) +{ + u32 crc = crc24q_bits(0, part->decoded, GPS_CNAV_MSG_DATA_LENGTH, + part->invert); + + return crc; +} + +/** + * Extracts CRC-24Q from a CNAV message buffer. + * CRC-24Q value is the last 24 bits from 300 bits message buffer. + * + * \param[in] part Decoded component with payload. + * + * \return CRC24-Q value. + * + * \private + */ +static u32 _cnav_extract_crc(const cnav_v27_part_t *part) +{ + u32 crc = getbitu(part->decoded, GPS_CNAV_MSG_DATA_LENGTH, + GPS_CNAV_MSG_CRC_LENGTH); + if (part->invert) { + crc ^= 0xFFFFFF; + } + return crc; +} + +/** + * Helper to rescan for preamble in the received buffer. + * Occasionally there could be a false lock on message contents if it the + * preamble sequence is encountered in the message body. For this case, the + * function performs for a scan for a preamble with a different offset: + * - When found, the preamble octet is moved into the head of the buffer. + * - When not found, only 7 bits are left in the buffer. + * + * \param[in,out] part Decoded component. + * + * \return None + * + * \private + */ +static void _cnav_rescan_preamble(cnav_v27_part_t *part) +{ + part->preamble_seen = false; + + if (part->n_decoded > GPS_CNAV_PREAMBLE_LENGTH + 1) { + for (size_t i = 1, j = part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH; + i < j; ++i) { + u32 c = getbitu(part->decoded, i, GPS_CNAV_PREAMBLE_LENGTH); + if (GPS_CNAV_PREAMBLE1 == c || GPS_CNAV_PREAMBLE2 == c) { + part->preamble_seen = true; + part->invert = (GPS_CNAV_PREAMBLE2 == c); + /* We shift the accumulated bits to the beginning of the buffer */ + bitshl(part->decoded, sizeof(part->decoded), i); + part->n_decoded -= i; + break; + } + } + } + if (!part->preamble_seen && part->n_decoded >= GPS_CNAV_PREAMBLE_LENGTH) { + bitshl(part->decoded, sizeof(part->decoded), + part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH + 1); + part->n_decoded = GPS_CNAV_PREAMBLE_LENGTH - 1; + } +} + +/** + * Feed a symbol into Viterbi decoder instance. + * + * The method uses block Viterbi decoder. It first accumulates initial number of + * symbols, and after that runs decoding every time the buffer is full. Only + * some of the decoded symbols are used. + * + * \param[in,out] part Decoder object + * \param[in] s Symbol (0x00 - Hard 0, 0xFF - Hard 1) + * + * \return None + * + * \private + */ +static void _cnav_add_symbol(cnav_v27_part_t *part, u8 ch) +{ + part->symbols[part->n_symbols++] = ch; + + if (part->init) { + /* Initial step - load more symbols without decoding. */ + if (part->n_symbols < (GPS_L2C_V27_INIT_BITS + GPS_L2C_V27_DECODE_BITS) * 2) { + return; + } + part->init = false; + } + else if (part->n_symbols < GPS_L2C_V27_DECODE_BITS * 2) { + /* Wait until decoding block is accumulated */ + return; + } + + /* Feed accumulated symbols into the buffer, reset the number of accumulated + * symbols. */ + v27_update(&part->dec, part->symbols, part->n_symbols / 2); + part->n_symbols = 0; + + /* Decode N+M bits, where: + * - N - Number of bits to put into decoded buffer + * - M - Number of bits in the tail to ignore. + */ + unsigned char tmp_bits[ (GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS + + CHAR_BIT - 1) / CHAR_BIT]; + + v27_chainback_likely(&part->dec, tmp_bits, + GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS); + + /* Read decoded bits and add them to the decoded buffer */ + bitcopy(part->decoded, part->n_decoded, tmp_bits, 0, GPS_L2C_V27_DECODE_BITS); + part->n_decoded += GPS_L2C_V27_DECODE_BITS; + + /* Depending on the decoder state, one of the following actions are + * possible: + * - If no message lock + * - If no preamble seen - look for preamble + * - If preamble seen - collect 300 bits + * - If 300 bits are collected - verify CRC + * - If CRC is OK - message lock is acquired + * - If CRC fails - rescan for preamble + * - If found - continue collecting 300 bits + * - If not found - continue preamble wait + * - If message lock + * - If 300 bits collected, compute CRC + * - If CRC is OK, message can be decoded + * - If CRC is not OK, discard data + */ + + bool retry = true; + while (retry) { + retry = false; + + if (!part->preamble_seen) { + /* Rescan for preamble if possible. The first bit is ignored. */ + _cnav_rescan_preamble(part); + } + if (part->preamble_seen && GPS_CNAV_MSG_LENGTH <= part->n_decoded) { + + /* We have collected 300 bits starting from message preamble. Now try + * to compute CRC-24Q */ + u32 crc = _cnav_compute_crc(part); + u32 crc2 = _cnav_extract_crc(part); + + if (part->message_lock) { + /* We have message lock */ + part->crc_ok = (crc == crc2); + if (part->crc_ok) { + /* Reset message lock counter */ + part->n_crc_fail = 0; + } else { + /* Increment message lock counter */ + part->n_crc_fail++; + if (part->n_crc_fail > GPS_CNAV_LOCK_MAX_CRC_FAILS) { + /* CRC has failed too many times - drop the lock. */ + part->n_crc_fail = 0; + part->message_lock = false; + part->preamble_seen = false; + /* Try to find a new preamble, reuse data from buffer. */ + retry = true; + } + } + } else if (crc == crc2) { + /* CRC match - message can be decoded */ + part->message_lock = true; + part->crc_ok = true; + part->n_crc_fail = 0; + } else { + /* There is no message lock and the CRC check fails. Assume there is + * false positive lock - rescan for preamble. */ + part->crc_ok = false; + part->preamble_seen = false; + + /* CRC mismatch - try to re-scan for preamble */ + retry = true; + } + } + else + { + /* No preamble or preamble and less than 300 bits decoded */ + } + } +} + +/** + * Invert message bits in the buffer. + * + * The method inverts bits of the decoded data. + * + * \param[in,out] part Decoder component with a message buffer. + * + * \return None + */ +static void _cnav_msg_invert(cnav_v27_part_t *part) +{ + for (size_t i = 0; i < sizeof(part->decoded); i++) { + part->decoded[i] ^= 0xFFu; + } +} + +/** + * Performs CNAV message decoding. + * + * This function decoded CNAV message, if the following conditions are met: + * - Buffer contains 300 bits. + * - First 8 bits are matching direct or inverse preamble. + * - Message data CRC matches one in the buffer. + * + * In case the message starts with inverted preamble, the data is inverted + * before parsing. + * + * \param[in,out] part Decoder component. + * \param[out] msg Container for a decoded message. + * \param[out] delay Delay of the message in symbols. + * + * \retval true The message has been decoded, and \a msg container is populated. + * \retval false Not enough data or CRC is not correct. + * + * \private + */ +static bool _cnav_msg_decode(cnav_v27_part_t *part, cnav_msg_t *msg, u32 *delay) +{ + bool res = false; + if (GPS_CNAV_MSG_LENGTH <= part->n_decoded) { + if (part->crc_ok) { + /* CRC is OK */ + if (part->invert) { + _cnav_msg_invert(part); + } + + msg->prn = getbitu(part->decoded, 8, 6); + msg->msg_id = getbitu(part->decoded, 14, 6); + msg->tow = getbitu(part->decoded, 20, 17); + msg->alert = getbitu(part->decoded, 37, 1) ? true : false; + + /* copy RAW message for GNSS-SDR */ + memcpy(msg->raw_msg,part->decoded,GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS); + + *delay = (part->n_decoded - GPS_CNAV_MSG_LENGTH + GPS_L2C_V27_DELAY_BITS) + * 2 + part->n_symbols; + + if (part->invert) { + _cnav_msg_invert(part); + } + res = true; + } else { + /* CRC mismatch - no decoding */ + } + bitshl(part->decoded, sizeof(part->decoded), GPS_CNAV_MSG_LENGTH); + part->n_decoded -= GPS_CNAV_MSG_LENGTH; + } + + return res; +} + +/** + * Initialize CNAV decoder. + * + * CNAV decoder contains two Viterbi decoders that are used to estimate bit and + * message boundary. + * + * \param[out] dec Decoder structure. + * + * \return None + */ +void cnav_msg_decoder_init(cnav_msg_decoder_t *dec) +{ + memset(dec, 0, sizeof(*dec)); + v27_init(&dec->part1.dec, + dec->part1.decisions, + GPS_L2_V27_HISTORY_LENGTH_BITS, + cnav_msg_decoder_get_poly(), + 0); + v27_init(&dec->part2.dec, + dec->part2.decisions, + GPS_L2_V27_HISTORY_LENGTH_BITS, + cnav_msg_decoder_get_poly(), + 0); + dec->part1.init = true; + dec->part2.init = true; + _cnav_add_symbol(&dec->part2, 0x80); +} + +/** + * Adds a received symbol to decoder. + * + * The method feeds the symbol into the decoder. In case there is a sufficient + * information to produce a message, the message is decoded and symbol delay is + * reported. + * The time of the last input symbol can be computed from the message ToW and + * delay by the formulae: + * \code + * symbolTime_ms = msg->tow * 6000 + *pdelay * 20 + * \endcode + * + * \param[in,out] dec Decoder object. + * \param[in] symbol Symbol value probability, where 0x00 - 100% of 0, + * 0xFF - 100% of 1. + * \param[out] msg Buffer for decoded message. The message is available + * only when message lock is acquired and CRC is correct. + * \param[out] pdelay Delay of message generation in symbols. + * + * \retval true The message has been decoded. ToW parameter is available. + * \retval false More data is required. + */ +bool cnav_msg_decoder_add_symbol(cnav_msg_decoder_t *dec, + u8 symbol, + cnav_msg_t *msg, + u32 *pdelay) +{ + _cnav_add_symbol(&dec->part1, symbol); + _cnav_add_symbol(&dec->part2, symbol); + + if (dec->part1.message_lock) { + /* Flush data in decoder. */ + dec->part2.n_decoded = 0; + dec->part2.n_symbols = 0; + return _cnav_msg_decode(&dec->part1, msg, pdelay); + } + if (dec->part2.message_lock) { + /* Flush data in decoder. */ + dec->part1.n_decoded = 0; + dec->part1.n_symbols = 0; + return _cnav_msg_decode(&dec->part2, msg, pdelay); + } + + return false; +} + +/** + * Provides a singleton polynomial object. + * + * The method constructs and returns polynomial object for CNAV message + * decoding. The same polynomial can be used also for other message handling. + * + * The object is initialized on the first call. The method is thread-safe. + * + * @return Pointer to polynomial object for CNAV message decoding. + */ +const v27_poly_t *cnav_msg_decoder_get_poly(void) +{ + static v27_poly_t instance; + static bool initialized = false; + + if (!initialized) { + /* Coefficients for polynomial object */ + const signed char coeffs[2] = { GPS_L2C_V27_POLY_A, GPS_L2C_V27_POLY_B }; + + /* Racing condition handling: the data can be potential initialized more + * than once in case multiple threads request concurrent access. However, + * nature of the v27_poly_init() function and data alignment ensure that + * the data returned from the earlier finished call is consistent and can + * be used even when re-initialization is happening. + * + * Other possible approaches are: + * - Replace late initialization with an explicit call. + * - Use POSIX synchronization objects like pthread_once_t. + */ + v27_poly_init(&instance, coeffs); + initialized = true; + } + return &instance; +} + +/** \} */ +/** \} */ diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h new file mode 100644 index 000000000..684e381c5 --- /dev/null +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h @@ -0,0 +1,101 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Valeri Atamaniouk + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#ifndef LIBSWIFTNAV_CNAV_MSG_H +#define LIBSWIFTNAV_CNAV_MSG_H + +#include "fec.h" + +#include +#include +#include +#include + +#include "swift_common.h" + +/** \addtogroup GPS_L2 + * \{ */ +/** \addtogroup gps_cnav_decoder + * \{ */ + +/** Size of the Viterbi decoder history. */ +#define GPS_L2_V27_HISTORY_LENGTH_BITS 64 +/** Bits to accumulate before decoding starts. */ +#define GPS_L2C_V27_INIT_BITS (32) +/** Bits to decode at a time. */ +#define GPS_L2C_V27_DECODE_BITS (32) +/** Bits in decoder tail. We ignore them. */ +#define GPS_L2C_V27_DELAY_BITS (32) +/** + * GPS CNAV message container. + * + * @sa cnav_msg_decoder_add_symbol + */ +typedef struct +{ + u8 prn; /**< SV PRN. 0..31 */ + u8 msg_id; /**< Message id. 0..31 */ + u32 tow; /**< GPS ToW in 6-second units. Multiply to 6 to get seconds. */ + bool alert; /**< CNAV message alert flag */ + u8 raw_msg[GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS]; /**< RAW MSG for GNSS-SDR */ +} cnav_msg_t; + +/** + * GPS CNAV decoder component. + * This component controls symbol decoding string. + * + * @sa cnav_msg_decoder_t + */ +typedef struct { + v27_t dec; /**< Viterbi block decoder object */ + v27_decision_t decisions[GPS_L2_V27_HISTORY_LENGTH_BITS]; + /**< Decision graph */ + unsigned char symbols[(GPS_L2C_V27_INIT_BITS + GPS_L2C_V27_DECODE_BITS) * 2]; + /**< Symbol buffer */ + size_t n_symbols; /**< Count of symbols in the symbol buffer */ + unsigned char decoded[GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS]; + /**< Decode buffer */ + size_t n_decoded; /**< Number of bits in the decode buffer */ + bool preamble_seen; /**< When true, the decode buffer is aligned on + * preamble. */ + bool invert; /**< When true, indicates the bits are inverted */ + bool message_lock; /**< When true, indicates the message boundary + * is found. */ + bool crc_ok; /**< Flag that the last message had good CRC */ + size_t n_crc_fail; /**< Counter for CRC failures */ + bool init; /**< Initial state flag. When true, initial bits + * do not produce output. */ +} cnav_v27_part_t; + +/** + * GPS CNAV message lock and decoder object. + * + * Decoder uses two Viterbi decoder objects to ensure the lock is acquired when + * the input symbol phase is not known. + */ +typedef struct +{ + cnav_v27_part_t part1; /**< Decoder for odd symbol pairs */ + cnav_v27_part_t part2; /**< Decoder for even symbol pairs */ +} cnav_msg_decoder_t; + +const v27_poly_t *cnav_msg_decoder_get_poly(void); +void cnav_msg_decoder_init(cnav_msg_decoder_t *dec); +bool cnav_msg_decoder_add_symbol(cnav_msg_decoder_t *dec, + unsigned char symbol, + cnav_msg_t *msg, + u32 *delay); + +/** \} */ +/** \} */ + +#endif /* LIBSWIFTNAV_CNAV_MSG_H */ diff --git a/src/algorithms/telemetry_decoder/libs/convolutional.h b/src/algorithms/telemetry_decoder/libs/libswiftcnav/convolutional.h similarity index 100% rename from src/algorithms/telemetry_decoder/libs/convolutional.h rename to src/algorithms/telemetry_decoder/libs/libswiftcnav/convolutional.h diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c new file mode 100644 index 000000000..280c68c7e --- /dev/null +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c @@ -0,0 +1,122 @@ +/* + * Copyright (C) 2010 Swift Navigation Inc. + * Contact: Fergus Noble + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include "edc.h" + +/** \defgroup edc Error Detection and Correction + * Error detection and correction functions. + * \{ */ + +/** \defgroup crc CRC + * Cyclic redundancy checks. + * \{ */ + +static const u32 crc24qtab[256] = { + 0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17, + 0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E, + 0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E, + 0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7, + 0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE, + 0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7, + 0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077, + 0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E, + 0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5, + 0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC, + 0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C, + 0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375, + 0x15723B, 0x933EC0, 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C, + 0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15, + 0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5, + 0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C, + 0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3, + 0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A, + 0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A, + 0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703, + 0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A, + 0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863, + 0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3, + 0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA, + 0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61, + 0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58, + 0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8, + 0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1, + 0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88, + 0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1, + 0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401, + 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538 +}; + +/** Calculate Qualcomm 24-bit Cyclical Redundancy Check (CRC-24Q). + * + * The CRC polynomial used is: + * \f[ + * x^{24} + x^{23} + x^{18} + x^{17} + x^{14} + x^{11} + x^{10} + + * x^7 + x^6 + x^5 + x^4 + x^3 + x+1 + * \f] + * Mask 0x1864CFB, not reversed, not XOR'd + * + * \param buf Array of data to calculate CRC for + * \param len Length of data array + * \param crc Initial CRC value + * + * \return CRC-24Q value + */ +u32 crc24q(const u8 *buf, u32 len, u32 crc) +{ + for (u32 i = 0; i < len; i++) + crc = ((crc << 8) & 0xFFFFFF) ^ crc24qtab[((crc >> 16) ^ buf[i]) & 0xff]; + return crc; +} + +/** + * Computes CRC-24Q for left-aligned bit message. + * This function is used for left-aligned bit messages, for example SBAS and + * GPS CNAV. + * GPS message is 300 bits total, but 276 bits without CRC. It takes 34.5 + * 8-bit bytes, and when computing CRC the message has to be padded with zero + * bits. + * + * \param[in] crc Initial CRC value + * \param[in] buf Pointer to MSB-aligned data. + * \param[in] n_bits Number of bits in the data buffer. + * \param[in] invert Flag to compute inverted CRC. + * + * \return CRC-24Q value + */ +u32 crc24q_bits(u32 crc, const u8 *buf, u32 n_bits, bool invert) +{ + u16 acc = 0; + u8 b = 0; + u32 shift = 8 - n_bits % 8; + + for (u32 i = 0; i < n_bits / 8; ++i) { + acc = (acc << 8) | *buf++; + if (invert) { + acc ^= 0xFFu; + } + b = (acc >> shift) & 0xFFu; + crc = ((crc << 8) & 0xFFFFFFu) ^ crc24qtab[((crc >> 16) ^ b) & 0xFFu]; + } + acc = (acc << 8) | *buf; + if (invert) { + acc ^= 0xFFu; + } + b = (acc >> shift) & 0xFFu; + crc = ((crc << 8) & 0xFFFFFFu) ^ crc24qtab[((crc >> 16) ^ b) & 0xFFu]; + + return crc; +} + + +/** \} */ + +/** \} */ diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.h b/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.h new file mode 100644 index 000000000..f6d6c28ff --- /dev/null +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.h @@ -0,0 +1,21 @@ +/* + * Copyright (C) 2010 Swift Navigation Inc. + * Contact: Fergus Noble + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#ifndef LIBSWIFTNAV_EDC_H +#define LIBSWIFTNAV_EDC_H + +#include "swift_common.h" + +u32 crc24q(const u8 *buf, u32 len, u32 crc); +u32 crc24q_bits(u32 crc, const u8 *buf, u32 n_bits, bool invert); + +#endif /* LIBSWIFTNAV_EDC_H */ diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h b/src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h new file mode 100644 index 000000000..296358d2d --- /dev/null +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h @@ -0,0 +1,47 @@ +/* User include file for libfec + * Copyright 2004, Phil Karn, KA9Q + * May be used under the terms of the GNU Lesser General Public License (LGPL) + */ + +#ifndef _FEC_H_ +#define _FEC_H_ + +/* r=1/2 k=7 convolutional encoder polynomials + * The NASA-DSN convention is to use V27POLYA inverted, then V27POLYB + * The CCSDS/NASA-GSFC convention is to use V27POLYB, then V27POLYA inverted + */ +#define V27POLYA 0x4f +#define V27POLYB 0x6d + +typedef struct { + unsigned char c0[32]; + unsigned char c1[32]; +} v27_poly_t; + +typedef struct { + unsigned int w[2]; +} v27_decision_t; + +/* State info for instance of r=1/2 k=7 Viterbi decoder + */ +typedef struct { + unsigned int metrics1[64]; /* Path metric buffer 1 */ + unsigned int metrics2[64]; /* Path metric buffer 2 */ + /* Pointers to path metrics, swapped on every bit */ + unsigned int *old_metrics, *new_metrics; + const v27_poly_t *poly; /* Polynomial to use */ + v27_decision_t *decisions; /* Beginning of decisions for block */ + unsigned int decisions_index; /* Index of current decision */ + unsigned int decisions_count; /* Number of decisions in history */ +} v27_t; + +void v27_poly_init(v27_poly_t *poly, const signed char polynomial[2]); + +void v27_init(v27_t *v, v27_decision_t *decisions, unsigned int decisions_count, + const v27_poly_t *poly, unsigned char initial_state); +void v27_update(v27_t *v, const unsigned char *syms, int nbits); +void v27_chainback_fixed(v27_t *v, unsigned char *data, unsigned int nbits, + unsigned char final_state); +void v27_chainback_likely(v27_t *v, unsigned char *data, unsigned int nbits); + +#endif diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/swift_common.h b/src/algorithms/telemetry_decoder/libs/libswiftcnav/swift_common.h new file mode 100644 index 000000000..c673596c7 --- /dev/null +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/swift_common.h @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2012 Swift Navigation Inc. + * Contact: Henry Hallam + * Fergus Noble + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#ifndef LIBSWIFTNAV_COMMON_H +#define LIBSWIFTNAV_COMMON_H + +/** \defgroup common Common definitions + * Common definitions used throughout the library. + * \{ */ + +#define ABS(x) ((x) < 0 ? -(x) : (x)) +#define MIN(x, y) (((x) < (y)) ? (x) : (y)) +#define MAX(x, y) (((x) > (y)) ? (x) : (y)) +#define CLAMP_DIFF(a,b) (MAX((a),(b)) - (b)) + +#include +#include +#include + +#ifndef COMMON_INT_TYPES +#define COMMON_INT_TYPES + +/** \defgroup common_inttypes Integer types + * Specified-width integer type definitions for shorter and nicer code. + * + * These should be used in preference to unspecified width types such as + * `int` which can lead to portability issues between different platforms. + * \{ */ + +/** Signed 8-bit integer. */ +typedef int8_t s8; +/** Signed 16-bit integer. */ +typedef int16_t s16; +/** Signed 32-bit integer. */ +typedef int32_t s32; +/** Signed 64-bit integer. */ +typedef int64_t s64; +/** Unsigned 8-bit integer. */ +typedef uint8_t u8; +/** Unsigned 16-bit integer. */ +typedef uint16_t u16; +/** Unsigned 32-bit integer. */ +typedef uint32_t u32; +/** Unsigned 64-bit integer. */ +typedef uint64_t u64; + +#endif + +/** \} */ + +/** \} */ + +#endif /* LIBSWIFTNAV_COMMON_H */ + diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c new file mode 100644 index 000000000..33ea04594 --- /dev/null +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c @@ -0,0 +1,216 @@ +/* K=7 r=1/2 Viterbi decoder in portable C + * Copyright Feb 2004, Phil Karn, KA9Q + * May be used under the terms of the GNU Lesser General Public License (LGPL) + */ + +#include + +#include "fec.h" + +static inline int parity(int x) +{ + x ^= x >> 16; + x ^= x >> 8; + x ^= x >> 4; + x &= 0xf; + return (0x6996 >> x) & 1; +} + +/** Initialize a v27_poly_t struct for use with a v27_t decoder. + * + * \param poly Structure to initialize. + * \param polynomial Byte array representing the desired polynomials. + */ +void v27_poly_init(v27_poly_t *poly, const signed char polynomial[2]) +{ + int state; + + for(state = 0; state < 32; state++) { + poly->c0[state] = (polynomial[0] < 0) ^ parity((2*state) & abs(polynomial[0])) ? 255 : 0; + poly->c1[state] = (polynomial[1] < 0) ^ parity((2*state) & abs(polynomial[1])) ? 255 : 0; + } +} + +/** Initialize a v27_t struct for Viterbi decoding. + * + * \param v Structure to initialize + * \param decisions Array of v27_decision_t structs, capacity = decisions_count. + * Must remain valid as long as v is used. + * \param decisions_count Size of decisions array. Equal to the number of bit + * decisions kept in history. + * \param poly Struct describing the polynomials to use. Must remain valid as + * long as v is used. May be shared between multiple decoders. + * \param initial_state Initial state of the decoder shift register. Usually zero. + */ +void v27_init(v27_t *v, v27_decision_t *decisions, unsigned int decisions_count, + const v27_poly_t *poly, unsigned char initial_state) +{ + int i; + + v->old_metrics = v->metrics1; + v->new_metrics = v->metrics2; + v->poly = poly; + v->decisions = decisions; + v->decisions_index = 0; + v->decisions_count = decisions_count; + + for(i = 0; i < 64; i++) + v->old_metrics[i] = 63; + + v->old_metrics[initial_state & 63] = 0; /* Bias known start state */ +} + +/* C-language butterfly */ +#define BFLY(i) {\ +unsigned int metric,m0,m1,decision;\ + metric = (v->poly->c0[i] ^ sym0) + (v->poly->c1[i] ^ sym1);\ + m0 = v->old_metrics[i] + metric;\ + m1 = v->old_metrics[i+32] + (510 - metric);\ + decision = (signed int)(m0-m1) > 0;\ + v->new_metrics[2*i] = decision ? m1 : m0;\ + d->w[i/16] |= decision << ((2*i)&31);\ + m0 -= (metric+metric-510);\ + m1 += (metric+metric-510);\ + decision = (signed int)(m0-m1) > 0;\ + v->new_metrics[2*i+1] = decision ? m1 : m0;\ + d->w[i/16] |= decision << ((2*i+1)&31);\ +} + +/** Update a v27_t decoder with a block of symbols. + * + * \param v Structure to update. + * \param syms Array of symbols to use. Must contain two symbols per bit. + * 0xff = strong 1, 0x00 = strong 0. + * \param nbits Number of bits corresponding to the provided symbols. + */ +void v27_update(v27_t *v, const unsigned char *syms, int nbits) +{ + unsigned char sym0, sym1; + unsigned int *tmp; + int normalize = 0; + + while(nbits--) { + v27_decision_t *d = &v->decisions[v->decisions_index]; + + d->w[0] = d->w[1] = 0; + sym0 = *syms++; + sym1 = *syms++; + + BFLY(0); + BFLY(1); + BFLY(2); + BFLY(3); + BFLY(4); + BFLY(5); + BFLY(6); + BFLY(7); + BFLY(8); + BFLY(9); + BFLY(10); + BFLY(11); + BFLY(12); + BFLY(13); + BFLY(14); + BFLY(15); + BFLY(16); + BFLY(17); + BFLY(18); + BFLY(19); + BFLY(20); + BFLY(21); + BFLY(22); + BFLY(23); + BFLY(24); + BFLY(25); + BFLY(26); + BFLY(27); + BFLY(28); + BFLY(29); + BFLY(30); + BFLY(31); + + /* Normalize metrics if they are nearing overflow */ + if(v->new_metrics[0] > (1<<30)) { + int i; + unsigned int minmetric = 1<<31; + + for(i=0; i<64; i++) { + if(v->new_metrics[i] < minmetric) + minmetric = v->new_metrics[i]; + } + + for(i=0; i<64; i++) + v->new_metrics[i] -= minmetric; + + normalize += minmetric; + } + + /* Advance decision index */ + if(++v->decisions_index >= v->decisions_count) + v->decisions_index = 0; + + /* Swap pointers to old and new metrics */ + tmp = v->old_metrics; + v->old_metrics = v->new_metrics; + v->new_metrics = tmp; + } +} + +/** Retrieve the most likely output bit sequence with known final state from + * a v27_t decoder. + * + * \param v Structure to use. + * \param data Array used to store output bits, capacity = nbits. + * \param nbits Number of bits to retrieve. + * \param final_state Known final state of the decoder shift register. + */ +void v27_chainback_fixed(v27_t *v, unsigned char *data, unsigned int nbits, + unsigned char final_state) +{ + int k; + unsigned int decisions_index = v->decisions_index; + + final_state %= 64; + final_state <<= 2; + + while(nbits-- != 0) { + + /* Decrement decision index */ + decisions_index = (decisions_index == 0) ? + v->decisions_count-1 : decisions_index-1; + + v27_decision_t *d = &v->decisions[decisions_index]; + k = (d->w[(final_state>>2)/32] >> ((final_state>>2)%32)) & 1; + /* The store into data[] only needs to be done every 8 bits. + * But this avoids a conditional branch, and the writes will + * combine in the cache anyway + */ + data[nbits>>3] = final_state = (final_state >> 1) | (k << 7); + } +} + +/** Retrieve the most likely output bit sequence with unknown final state from + * a v27_t decoder. + * + * \param v Structure to use. + * \param data Array used to store output bits, capacity = nbits. + * \param nbits Number of bits to retrieve. + */ +void v27_chainback_likely(v27_t *v, unsigned char *data, unsigned int nbits) +{ + /* Determine state with minimum metric */ + + int i; + unsigned int best_metric = 0xffffffff; + unsigned char best_state = 0; + for(i=0; i<64; i++) + { + if(v->new_metrics[i] < best_metric) + { + best_metric = v->new_metrics[i]; + best_state = i; + } + } + + v27_chainback_fixed(v, data, nbits, best_state); +} diff --git a/src/core/receiver/CMakeLists.txt b/src/core/receiver/CMakeLists.txt index a443b8a60..e06c2c4f0 100644 --- a/src/core/receiver/CMakeLists.txt +++ b/src/core/receiver/CMakeLists.txt @@ -65,6 +65,7 @@ include_directories( ${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/adapters ${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/gnuradio_blocks ${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/libs + ${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/libs/libswiftcnav ${CMAKE_SOURCE_DIR}/src/algorithms/observables/adapters ${CMAKE_SOURCE_DIR}/src/algorithms/observables/gnuradio_blocks ${CMAKE_SOURCE_DIR}/src/algorithms/PVT/adapters @@ -141,6 +142,8 @@ target_link_libraries(gnss_rx ${Boost_LIBRARIES} tracking_lib tracking_adapters channel_adapters + telemetry_decoder_libswiftcnav + telemetry_decoder_lib telemetry_decoder_adapters obs_adapters pvt_adapters diff --git a/src/core/system_parameters/GPS_L2C.h b/src/core/system_parameters/GPS_L2C.h index 4fd8374a2..c6c703952 100644 --- a/src/core/system_parameters/GPS_L2C.h +++ b/src/core/system_parameters/GPS_L2C.h @@ -95,12 +95,14 @@ const int32_t GPS_L2C_M_INIT_REG[115] = // NAVIGATION MESSAGE FIELDS POSITIONS (from IS-GPS-200E Appendix III) #define GPS_CNAV_PREAMBLE {1, 0, 0, 0, 1, 0, 1, 1} +#define GPS_CNAV_PREAMBLE_STR "10001011" +#define GPS_CNAV_INV_PREAMBLE_STR "01110100" const int GPS_L2_CNAV_DATA_PAGE_BITS = 300; //!< GPS L2 CNAV page length, including preamble and CRC [bits] const int GPS_L2_SYMBOLS_PER_BIT = 2; const int GPS_L2_SAMPLES_PER_SYMBOL = 1; +const int GPS_L2_CNAV_DATA_PAGE_SYMBOLS = 600; const int GPS_L2_CNAV_DATA_PAGE_DURATION_S = 12; -const int GPS_L2_CNAV_DATA_PAGE_BITS_EXTENDED_BYTES = 304; //!< GPS L2 CNAV page length, including preamble and CRC [bits] // common to all messages const std::vector > CNAV_PRN( { {9,6} } ); @@ -127,15 +129,15 @@ const std::vector > CNAV_A_DOT({{108,25}}); const double CNAV_A_DOT_LSB = TWO_N21; const std::vector > CNAV_DELTA_N0({{133,17}}); -const double CNAV_DELTA_N0_LSB = TWO_N44; +const double CNAV_DELTA_N0_LSB = TWO_N44*GPS_L2_PI; //semi-circles to radians const std::vector > CNAV_DELTA_N0_DOT({{150,23}}); -const double CNAV_DELTA_N0_DOT_LSB = TWO_N57; +const double CNAV_DELTA_N0_DOT_LSB = TWO_N57*GPS_L2_PI;//semi-circles to radians const std::vector > CNAV_M0({{173,33}}); -const double CNAV_M0_LSB = TWO_N32; +const double CNAV_M0_LSB = TWO_N32*GPS_L2_PI;//semi-circles to radians const std::vector > CNAV_E_ECCENTRICITY({{206,33}}); const double CNAV_E_ECCENTRICITY_LSB = TWO_N34; const std::vector > CNAV_OMEGA({{239,33}}); -const double CNAV_OMEGA_LSB = TWO_N32; +const double CNAV_OMEGA_LSB = TWO_N32*GPS_L2_PI;//semi-circles to radians const std::vector > CNAV_INTEGRITY_FLAG({{272,1}}); const std::vector > CNAV_L2_PHASING_FLAG({{273,1}}); @@ -144,13 +146,13 @@ const std::vector > CNAV_L2_PHASING_FLAG({{273,1}}); const std::vector > CNAV_TOE2({{39,11}}); const double CNAV_TOE2_LSB = 300.0; const std::vector > CNAV_OMEGA0({{50,33}}); -const double CNAV_OMEGA0_LSB = TWO_N32; +const double CNAV_OMEGA0_LSB = TWO_N32*GPS_L2_PI;//semi-circles to radians const std::vector > CNAV_I0({{83,33}}); -const double CNAV_I0_LSB = TWO_N32; +const double CNAV_I0_LSB = TWO_N32*GPS_L2_PI;//semi-circles to radians const std::vector > CNAV_DELTA_OMEGA_DOT({{116,17}}); //Relative to REF = -2.6 x 10-9 semi-circles/second. -const double CNAV_DELTA_OMEGA_DOT_LSB = TWO_N44; +const double CNAV_DELTA_OMEGA_DOT_LSB = TWO_N44*GPS_L2_PI;//semi-circles to radians const std::vector > CNAV_I0_DOT({{133,15}}); -const double CNAV_I0_DOT_LSB = TWO_N44; +const double CNAV_I0_DOT_LSB = TWO_N44*GPS_L2_PI;//semi-circles to radians const std::vector > CNAV_CIS({{148,16}}); const double CNAV_CIS_LSB = TWO_N30; const std::vector > CNAV_CIC({{164,16}}); @@ -211,6 +213,28 @@ const double CNAV_BETA3_LSB = TWO_P16; const std::vector > CNAV_WNOP({{257,8}}); +// MESSAGE TYPE 33 (CLOCK and UTC) + +const std::vector > CNAV_A0({{128,16}}); +const double CNAV_A0_LSB = TWO_N35; +const std::vector > CNAV_A1({{144,13}}); +const double CNAV_A1_LSB = TWO_N51; +const std::vector > CNAV_A2({{157,7}}); +const double CNAV_A2_LSB = TWO_N68; +const std::vector > CNAV_DELTA_TLS({{164,8}}); +const double CNAV_DELTA_TLS_LSB = 1; +const std::vector > CNAV_TOT({{172,16}}); +const double CNAV_TOT_LSB = TWO_P4; +const std::vector > CNAV_WN_OT({{188,13}}); +const double CNAV_WN_OT_LSB = 1; +const std::vector > CNAV_WN_LSF({{201,13}}); +const double CNAV_WN_LSF_LSB = 1; +const std::vector > CNAV_DN({{214,4}}); +const double CNAV_DN_LSB = 1; +const std::vector > CNAV_DELTA_TLSF({{218,8}}); +const double CNAV_DELTA_TLSF_LSB = 1; + + // TODO: Add more frames (Almanac, etc...) diff --git a/src/core/system_parameters/MATH_CONSTANTS.h b/src/core/system_parameters/MATH_CONSTANTS.h index 7fc35d683..1ed316ed5 100644 --- a/src/core/system_parameters/MATH_CONSTANTS.h +++ b/src/core/system_parameters/MATH_CONSTANTS.h @@ -86,6 +86,7 @@ const double TWO_N55 = (2.775557561562891e-017); //!< 2^-55 const double TWO_N57 = (6.938893903907228e-18); //!< 2^-57 const double TWO_N59 = (1.73472347597681e-018); //!< 2^-59 const double TWO_N60 = (8.673617379884036e-19); //!< 2^-60 +const double TWO_N68 = (3.388131789017201e-21); //!< 2^-68 const double PI_TWO_N19 = (5.992112452678286e-006); //!< Pi*2^-19 diff --git a/src/core/system_parameters/galileo_ephemeris.h b/src/core/system_parameters/galileo_ephemeris.h index 39d48eeef..a6dcc515d 100644 --- a/src/core/system_parameters/galileo_ephemeris.h +++ b/src/core/system_parameters/galileo_ephemeris.h @@ -119,7 +119,7 @@ public: void serialize(Archive& archive, const unsigned int version) { using boost::serialization::make_nvp; - + if(version){}; archive & make_nvp("i_satellite_PRN",i_satellite_PRN); archive & make_nvp("M0_1", M0_1); archive & make_nvp("e_1", e_1); diff --git a/src/core/system_parameters/gps_cnav_ephemeris.cc b/src/core/system_parameters/gps_cnav_ephemeris.cc index 310980151..a248fb66a 100644 --- a/src/core/system_parameters/gps_cnav_ephemeris.cc +++ b/src/core/system_parameters/gps_cnav_ephemeris.cc @@ -177,7 +177,7 @@ double Gps_CNAV_Ephemeris::sv_clock_relativistic_term(double transmitTime) } -void Gps_CNAV_Ephemeris::satellitePosition(double transmitTime) +double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime) { double tk; double a; @@ -271,4 +271,15 @@ void Gps_CNAV_Ephemeris::satellitePosition(double transmitTime) d_satvel_X = - Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega); d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega); d_satvel_Z = d_satpos_Y * sin(i); + + // Time from ephemeris reference clock + tk = check_t(transmitTime - d_Toc); + + double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk; + + /* relativity correction */ + dtr_s -= 2.0 * sqrt(GM * a) * d_e_eccentricity * sin(E) / (GPS_L2_C_m_s * GPS_L2_C_m_s); + + return dtr_s; + } diff --git a/src/core/system_parameters/gps_cnav_ephemeris.h b/src/core/system_parameters/gps_cnav_ephemeris.h index 46e94cb5d..dd0ea9a9b 100644 --- a/src/core/system_parameters/gps_cnav_ephemeris.h +++ b/src/core/system_parameters/gps_cnav_ephemeris.h @@ -153,7 +153,8 @@ public: archive & make_nvp("d_IDOT", d_IDOT); //!< Rate of Inclination Angle [semi-circles/s] archive & make_nvp("i_GPS_week", i_GPS_week); //!< GPS week number, aka WN [week] archive & make_nvp("d_TGD", d_TGD); //!< Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] - + archive & make_nvp("d_DELTA_A", d_DELTA_A); + archive & make_nvp("d_A_DOT", d_A_DOT); archive & make_nvp("d_A_f0", d_A_f0); //!< Coefficient 0 of code phase offset model [s] archive & make_nvp("d_A_f1", d_A_f1); //!< Coefficient 1 of code phase offset model [s/s] @@ -168,7 +169,7 @@ public: * \brief Compute the ECEF SV coordinates and ECEF velocity * Implementation of Table 20-IV (IS-GPS-200E) */ - void satellitePosition(double transmitTime); + double satellitePosition(double transmitTime); /*! * \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction diff --git a/src/core/system_parameters/gps_cnav_navigation_message.cc b/src/core/system_parameters/gps_cnav_navigation_message.cc index dcfc582c8..5b3652387 100644 --- a/src/core/system_parameters/gps_cnav_navigation_message.cc +++ b/src/core/system_parameters/gps_cnav_navigation_message.cc @@ -71,13 +71,6 @@ Gps_CNAV_Navigation_Message::Gps_CNAV_Navigation_Message() } -void Gps_CNAV_Navigation_Message::print_gps_word_bytes(unsigned int GPS_word) -{ - std::cout << " Word ="; - std::cout << std::bitset<32>(GPS_word); - std::cout << std::endl; -} - bool Gps_CNAV_Navigation_Message::read_navigation_bool(std::bitset bits, const std::vector> parameter) { @@ -174,24 +167,8 @@ signed long int Gps_CNAV_Navigation_Message::read_navigation_signed(std::bitset< } -void Gps_CNAV_Navigation_Message::decode_page(std::vector data) +void Gps_CNAV_Navigation_Message::decode_page(std::bitset data_bits) { - std::bitset data_bits; - - try - { - for(int i = 0; i < GPS_L2_CNAV_DATA_PAGE_BITS; i++) - { - data_bits[i] = static_cast(data[GPS_L2_CNAV_DATA_PAGE_BITS - i - 1]); - } - - } - catch(std::exception &e) - { - std::cout << "Exception converting to bitset " << e.what() << std::endl; - return; - } - int PRN; int page_type; @@ -210,7 +187,7 @@ void Gps_CNAV_Navigation_Message::decode_page(std::vector data) page_type = static_cast(read_navigation_unsigned(data_bits, CNAV_MSG_TYPE)); - std::cout << "PRN=" << PRN << " TOW=" << d_TOW << " alert_flag=" << alert_flag << " page_type= " << page_type << std::endl; + //std::cout << "PRN=" << PRN << " TOW=" << d_TOW << " alert_flag=" << alert_flag << " page_type= " << page_type << std::endl; switch(page_type) { case 10: // Ephemeris 1/2 @@ -231,7 +208,7 @@ void Gps_CNAV_Navigation_Message::decode_page(std::vector data) ephemeris_record.d_DELTA_DOT_N = ephemeris_record.d_DELTA_DOT_N * CNAV_DELTA_N0_DOT_LSB; ephemeris_record.d_M_0 = static_cast(read_navigation_signed(data_bits, CNAV_M0)); ephemeris_record.d_M_0 = ephemeris_record.d_M_0 * CNAV_M0_LSB; - ephemeris_record.d_e_eccentricity = static_cast(read_navigation_signed(data_bits, CNAV_E_ECCENTRICITY)); + ephemeris_record.d_e_eccentricity = static_cast(read_navigation_unsigned(data_bits, CNAV_E_ECCENTRICITY)); ephemeris_record.d_e_eccentricity = ephemeris_record.d_e_eccentricity * CNAV_E_ECCENTRICITY_LSB; ephemeris_record.d_OMEGA = static_cast(read_navigation_signed(data_bits, CNAV_OMEGA)); ephemeris_record.d_OMEGA = ephemeris_record.d_OMEGA * CNAV_OMEGA_LSB; @@ -309,6 +286,46 @@ void Gps_CNAV_Navigation_Message::decode_page(std::vector data) iono_record.d_beta3 = iono_record.d_beta3 * CNAV_BETA3_LSB; b_flag_iono_valid = true; break; + case 33: // (CLOCK & UTC) + ephemeris_record.d_Top = static_cast(read_navigation_unsigned(data_bits, CNAV_TOP1)); + ephemeris_record.d_Top = ephemeris_record.d_Top * CNAV_TOP1_LSB; + ephemeris_record.d_Toc = static_cast(read_navigation_unsigned(data_bits, CNAV_TOC)); + ephemeris_record.d_Toc = ephemeris_record.d_Toc * CNAV_TOC_LSB; + ephemeris_record.d_A_f0 = static_cast(read_navigation_signed(data_bits, CNAV_AF0)); + ephemeris_record.d_A_f0 = ephemeris_record.d_A_f0 * CNAV_AF0_LSB; + ephemeris_record.d_A_f1 = static_cast(read_navigation_signed(data_bits, CNAV_AF1)); + ephemeris_record.d_A_f1 = ephemeris_record.d_A_f1 * CNAV_AF1_LSB; + ephemeris_record.d_A_f2 = static_cast(read_navigation_signed(data_bits, CNAV_AF2)); + ephemeris_record.d_A_f2 = ephemeris_record.d_A_f2 * CNAV_AF2_LSB; + + + utc_model_record.d_A0 = static_cast(read_navigation_signed(data_bits, CNAV_A0)); + utc_model_record.d_A0 = utc_model_record.d_A0 * CNAV_A0_LSB; + utc_model_record.d_A1 = static_cast(read_navigation_signed(data_bits, CNAV_A1)); + utc_model_record.d_A1 = utc_model_record.d_A1 * CNAV_A1_LSB; + utc_model_record.d_A2 = static_cast(read_navigation_signed(data_bits, CNAV_A2)); + utc_model_record.d_A2 = utc_model_record.d_A2 * CNAV_A2_LSB; + + + utc_model_record.d_DeltaT_LS = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_TLS)); + utc_model_record.d_DeltaT_LS = utc_model_record.d_DeltaT_LS * CNAV_DELTA_TLS_LSB; + + utc_model_record.d_t_OT = static_cast(read_navigation_signed(data_bits, CNAV_TOT)); + utc_model_record.d_t_OT = utc_model_record.d_t_OT * CNAV_TOT_LSB; + + utc_model_record.i_WN_T = static_cast(read_navigation_signed(data_bits, CNAV_WN_OT)); + utc_model_record.i_WN_T = utc_model_record.i_WN_T * CNAV_WN_OT_LSB; + + utc_model_record.i_WN_LSF = static_cast(read_navigation_signed(data_bits, CNAV_WN_LSF)); + utc_model_record.i_WN_LSF = utc_model_record.i_WN_LSF * CNAV_WN_LSF_LSB; + + utc_model_record.i_DN = static_cast(read_navigation_signed(data_bits, CNAV_DN)); + utc_model_record.i_DN = utc_model_record.i_DN * CNAV_DN_LSB; + + utc_model_record.d_DeltaT_LSF = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_TLSF)); + utc_model_record.d_DeltaT_LSF = utc_model_record.d_DeltaT_LSF * CNAV_DELTA_TLSF_LSB; + b_flag_utc_valid = true; + break; default: break; } @@ -319,7 +336,7 @@ bool Gps_CNAV_Navigation_Message::have_new_ephemeris() //Check if we have a new { if (b_flag_ephemeris_1 == true and b_flag_ephemeris_2 == true) { - if (ephemeris_record.d_Toe1 == ephemeris_record.d_Toe2) + if (ephemeris_record.d_Toe1 == ephemeris_record.d_Toe2)// and ephemeris_record.d_Toe1==ephemeris_record.d_Toc) { //if all ephemeris pages have the same TOE, then they belong to the same block // std::cout << "Ephemeris (1, 2) have been received and belong to the same batch" << std::endl; @@ -365,6 +382,19 @@ Gps_CNAV_Iono Gps_CNAV_Navigation_Message::get_iono() } +bool Gps_CNAV_Navigation_Message::have_new_utc_model() //Check if we have a new iono data stored in the galileo navigation class +{ + if (b_flag_utc_valid == true) + { + b_flag_utc_valid = false;// clear the flag + return true; + } + else + { + return false; + } +} + Gps_CNAV_Utc_Model Gps_CNAV_Navigation_Message::get_utc_model() { return utc_model_record; diff --git a/src/core/system_parameters/gps_cnav_navigation_message.h b/src/core/system_parameters/gps_cnav_navigation_message.h index 0fe71123a..9ab370a1a 100644 --- a/src/core/system_parameters/gps_cnav_navigation_message.h +++ b/src/core/system_parameters/gps_cnav_navigation_message.h @@ -58,7 +58,6 @@ private: unsigned long int read_navigation_unsigned(std::bitset bits, const std::vector> parameter); signed long int read_navigation_signed(std::bitset bits, const std::vector> parameter); bool read_navigation_bool(std::bitset bits, const std::vector> parameter); - void print_gps_word_bytes(unsigned int GPS_word); Gps_CNAV_Ephemeris ephemeris_record; Gps_CNAV_Iono iono_record; @@ -68,7 +67,8 @@ public: double d_TOW; bool b_flag_ephemeris_1; bool b_flag_ephemeris_2; - bool b_flag_iono_valid; //!< If set, it indicates that the ionospheric parameters are filled (page 18 has arrived and decoded) + bool b_flag_iono_valid; //!< If set, it indicates that the ionospheric parameters are filled and are not yet readed by the get_iono + bool b_flag_utc_valid; //!< If set, it indicates that the utc parameters are filled and are not yet readed by the get_utc_model std::map satelliteBlock; //!< Map that stores to which block the PRN belongs http://www.navcen.uscg.gov/?Do=constellationStatus @@ -89,13 +89,13 @@ public: // public functions void reset(); - void decode_page(std::vector data); + void decode_page(std::bitset data_bits); /*! * \brief Obtain a GPS SV Ephemeris class filled with current SV data */ Gps_CNAV_Ephemeris get_ephemeris(); /*! - * \brief Check if we have a new iono record stored in the galileo navigation class + * \brief Check if we have a new iono record stored in the GPS ephemeris class */ bool have_new_iono(); /*! @@ -109,7 +109,12 @@ public: Gps_CNAV_Utc_Model get_utc_model(); /*! - * \brief Check if we have a new ephemeris stored in the galileo navigation class + * \briefCheck if we have a new GPS UTC model record stored in the GPS ephemeris class + */ + bool have_new_utc_model(); + + /*! + * \brief Check if we have a new ephemeris stored in the GPS ephemeris class */ bool have_new_ephemeris(); diff --git a/src/core/system_parameters/gps_cnav_utc_model.cc b/src/core/system_parameters/gps_cnav_utc_model.cc index 748b0c306..adf343c21 100644 --- a/src/core/system_parameters/gps_cnav_utc_model.cc +++ b/src/core/system_parameters/gps_cnav_utc_model.cc @@ -34,6 +34,7 @@ Gps_CNAV_Utc_Model::Gps_CNAV_Utc_Model() { valid = false; + d_A2 = 0; d_A1 = 0; d_A0 = 0; d_t_OT = 0; @@ -43,3 +44,75 @@ Gps_CNAV_Utc_Model::Gps_CNAV_Utc_Model() i_DN = 0; d_DeltaT_LSF = 0; } + +double Gps_CNAV_Utc_Model::utc_time(double gpstime_corrected, int i_GPS_week) +{ + double t_utc; + double t_utc_daytime; + double Delta_t_UTC = d_DeltaT_LS + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * static_cast(i_GPS_week - i_WN_T)); + + //std::cout<<"d_DeltaT_LS="<(i_GPS_week - i_WN_T)); + Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * tmp_d + d_A2*tmp_d*tmp_d; + t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400); + } + + double secondsOfWeekBeforeToday = 86400 * floor(gpstime_corrected / 86400); + t_utc = secondsOfWeekBeforeToday + t_utc_daytime; + return t_utc; +} diff --git a/src/core/system_parameters/gps_cnav_utc_model.h b/src/core/system_parameters/gps_cnav_utc_model.h index 80ff61eb7..da50b7ffe 100644 --- a/src/core/system_parameters/gps_cnav_utc_model.h +++ b/src/core/system_parameters/gps_cnav_utc_model.h @@ -34,7 +34,7 @@ #include "boost/assign.hpp" #include - +#include /*! * \brief This class is a storage for the GPS UTC MODEL data as described in in IS-GPS-200H @@ -46,6 +46,7 @@ class Gps_CNAV_Utc_Model public: bool valid; // UTC parameters + double d_A2; //!< 2nd order term of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200H) [s/s] double d_A1; //!< 1st order term of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200H) [s/s] double d_A0; //!< Constant of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200H) [s] double d_t_OT; //!< Reference time for UTC data (reference 20.3.4.5 and 20.3.3.5.2.4 IS-GPS-200H) [s] @@ -60,6 +61,12 @@ public: */ Gps_CNAV_Utc_Model(); + /*! + * \brief Computes the Coordinated Universal Time (UTC) and + * returns it in [s] (IS-GPS-200E, 20.3.3.5.2.4 + 30.3.3.6.2) + */ + double utc_time(double gpstime_corrected, int i_GPS_week); + template /* * \brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the ephemeris data on disk file. diff --git a/src/core/system_parameters/gps_utc_model.cc b/src/core/system_parameters/gps_utc_model.cc index 2548b5ab9..59a385c1a 100644 --- a/src/core/system_parameters/gps_utc_model.cc +++ b/src/core/system_parameters/gps_utc_model.cc @@ -29,6 +29,7 @@ */ #include "gps_utc_model.h" +#include #include Gps_Utc_Model::Gps_Utc_Model() @@ -50,6 +51,11 @@ double Gps_Utc_Model::utc_time(double gpstime_corrected, int i_GPS_week) double t_utc_daytime; double Delta_t_UTC = d_DeltaT_LS + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * static_cast(i_GPS_week - i_WN_T)); + //std::cout<<"d_DeltaT_LS="< Date: Sat, 25 Mar 2017 18:10:16 +0100 Subject: [PATCH 3/8] Remove convolutional.h fron libswiftcnav --- .../telemetry_decoder/libs/{libswiftcnav => }/convolutional.h | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/algorithms/telemetry_decoder/libs/{libswiftcnav => }/convolutional.h (100%) diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/convolutional.h b/src/algorithms/telemetry_decoder/libs/convolutional.h similarity index 100% rename from src/algorithms/telemetry_decoder/libs/libswiftcnav/convolutional.h rename to src/algorithms/telemetry_decoder/libs/convolutional.h From f83b01f65fe9bf13bca53a18d66cdee1882b2a80 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 25 Mar 2017 18:11:36 +0100 Subject: [PATCH 4/8] Add missing include for fmod --- src/core/system_parameters/gps_cnav_utc_model.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core/system_parameters/gps_cnav_utc_model.cc b/src/core/system_parameters/gps_cnav_utc_model.cc index adf343c21..e4380ba9d 100644 --- a/src/core/system_parameters/gps_cnav_utc_model.cc +++ b/src/core/system_parameters/gps_cnav_utc_model.cc @@ -29,7 +29,7 @@ */ #include "gps_cnav_utc_model.h" - +#include Gps_CNAV_Utc_Model::Gps_CNAV_Utc_Model() { From 4477da0f755ba1367260b462e4581c28fa8f7f4b Mon Sep 17 00:00:00 2001 From: Administrator Date: Tue, 28 Mar 2017 17:40:17 +0200 Subject: [PATCH 5/8] Fix C compilation errors in c11 compilers --- .../libs/libswiftcnav/bits.c | 29 +++++++++++-------- .../libs/libswiftcnav/cnav_msg.c | 7 +++-- .../telemetry_decoder/libs/libswiftcnav/edc.c | 6 ++-- 3 files changed, 26 insertions(+), 16 deletions(-) diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c index b8d4ff832..1c7045e4c 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c @@ -53,8 +53,8 @@ u8 parity(u32 x) u32 getbitu(const u8 *buff, u32 pos, u8 len) { u32 bits = 0; - - for (u32 i = pos; i < pos + len; i++) { + u32 i=0; + for ( i= pos; i < pos + len; i++) { bits = (bits << 1) + ((buff[i/8] >> (7 - i%8)) & 1u); } @@ -98,9 +98,9 @@ void setbitu(u8 *buff, u32 pos, u32 len, u32 data) u32 mask = 1u << (len - 1); if (len <= 0 || 32 < len) - return; - - for (u32 i = pos; i < pos + len; i++, mask >>= 1) { + return; + u32 i=0; + for (i = pos; i < pos + len; i++, mask >>= 1) { if (data & mask) buff[i/8] |= 1u << (7 - i % 8); else @@ -156,7 +156,8 @@ void bitshl(void *buf, u32 size, u32 shift) } else { /* Create an accumulator: it will hold a value of two consecutive bytes */ u32 acc = *src++; - for (u32 i = 0; i < full_bytes; ++i) { + u32 i=0; + for (i = 0; i < full_bytes; ++i) { acc = (acc << CHAR_BIT) | *src++; *dst++ = acc >> byte_shift; } @@ -188,8 +189,8 @@ void bitcopy(void *dst, u32 dst_index, const void *src, u32 src_index, { u32 limit1 = count / 32; u32 limit2 = count % 32; - - for (u32 idx = 0; idx < limit1; ++idx) { + u32 idx=0; + for (idx = 0; idx < limit1; ++idx) { u32 tmp = getbitu(src, src_index, 32); setbitu(dst, dst_index, 32, tmp); src_index += 32; @@ -212,7 +213,8 @@ void bitcopy(void *dst, u32 dst_index, const void *src, u32 src_index, u8 count_bits_u64(u64 v, u8 bv) { u8 r = 0; - for (int i = 0; i < 16; i++) +int i =0; + for (i = 0; i < 16; i++) r += bitn[(v >> (i*4)) & 0xf]; return bv == 1 ? r : 64 - r; } @@ -228,7 +230,8 @@ u8 count_bits_u64(u64 v, u8 bv) u8 count_bits_u32(u32 v, u8 bv) { u8 r = 0; - for (int i = 0; i < 8; i++) + int i=0; + for (i = 0; i < 8; i++) r += bitn[(v >> (i*4)) & 0xf]; return bv == 1 ? r : 32 - r; } @@ -244,7 +247,8 @@ u8 count_bits_u32(u32 v, u8 bv) u8 count_bits_u16(u16 v, u8 bv) { u8 r = 0; - for (int i = 0; i < 4; i++) +int i =0; + for (i= 0; i < 4; i++) r += bitn[(v >> (i*4)) & 0xf]; return bv == 1 ? r : 16 - r; } @@ -260,7 +264,8 @@ u8 count_bits_u16(u16 v, u8 bv) u8 count_bits_u8(u8 v, u8 bv) { u8 r = 0; - for (int i = 0; i < 2; i++) +int i=0; + for (i = 0; i < 2; i++) r += bitn[(v >> (i*4)) & 0xf]; return bv == 1 ? r : 8 - r; } diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c index e8c733ebd..5051a381b 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c @@ -108,7 +108,9 @@ static void _cnav_rescan_preamble(cnav_v27_part_t *part) part->preamble_seen = false; if (part->n_decoded > GPS_CNAV_PREAMBLE_LENGTH + 1) { - for (size_t i = 1, j = part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH; + size_t i=0; + size_t j=0; + for (i = 1, j = part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH; i < j; ++i) { u32 c = getbitu(part->decoded, i, GPS_CNAV_PREAMBLE_LENGTH); if (GPS_CNAV_PREAMBLE1 == c || GPS_CNAV_PREAMBLE2 == c) { @@ -259,7 +261,8 @@ static void _cnav_add_symbol(cnav_v27_part_t *part, u8 ch) */ static void _cnav_msg_invert(cnav_v27_part_t *part) { - for (size_t i = 0; i < sizeof(part->decoded); i++) { + size_t i = 0; + for (i = 0; i < sizeof(part->decoded); i++) { part->decoded[i] ^= 0xFFu; } } diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c index 280c68c7e..0d1d0e369 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c @@ -72,7 +72,8 @@ static const u32 crc24qtab[256] = { */ u32 crc24q(const u8 *buf, u32 len, u32 crc) { - for (u32 i = 0; i < len; i++) + u32 i=0; + for ( i= 0; i < len; i++) crc = ((crc << 8) & 0xFFFFFF) ^ crc24qtab[((crc >> 16) ^ buf[i]) & 0xff]; return crc; } @@ -98,7 +99,8 @@ u32 crc24q_bits(u32 crc, const u8 *buf, u32 n_bits, bool invert) u8 b = 0; u32 shift = 8 - n_bits % 8; - for (u32 i = 0; i < n_bits / 8; ++i) { + u32 i=0; + for ( i= 0; i < n_bits / 8; ++i) { acc = (acc << 8) | *buf++; if (invert) { acc ^= 0xFFu; From 21e61ed2623f30e661e3f375441935bab7aea8f2 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 28 Mar 2017 19:08:59 +0200 Subject: [PATCH 6/8] Improve file headers --- .../libs/libswiftcnav/bits.c | 31 ++++++++++++---- .../libs/libswiftcnav/bits.h | 31 ++++++++++++---- .../libs/libswiftcnav/cnav_msg.c | 33 +++++++++++++---- .../libs/libswiftcnav/cnav_msg.h | 32 +++++++++++++---- .../telemetry_decoder/libs/libswiftcnav/edc.c | 31 ++++++++++++---- .../telemetry_decoder/libs/libswiftcnav/edc.h | 32 +++++++++++++---- .../telemetry_decoder/libs/libswiftcnav/fec.h | 36 ++++++++++++++++--- .../libs/libswiftcnav/swift_common.h | 33 +++++++++++++---- .../libs/libswiftcnav/viterbi27.c | 34 +++++++++++++++--- 9 files changed, 242 insertions(+), 51 deletions(-) diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c index b8d4ff832..0826c6e7f 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c @@ -1,13 +1,32 @@ -/* +/*! + * \file bits.c + * \author Fergus Noble + * + * ------------------------------------------------------------------------- + * This file was originally borrowed from libswiftnav + * , + * a portable C library implementing GNSS related functions and algorithms, + * and then modified by J. Arribas and C. Fernandez + * * Copyright (C) 2013, 2016 Swift Navigation Inc. * Contact: Fergus Noble * - * This source is subject to the license found in the file 'LICENSE' which must - * be be distributed together with this source. All other rights reserved. + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, - * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + * This file is part of GNSS-SDR. + * + * This file is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as + * published by the Free Software Foundation, version 3. + * + * 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 + * Lesser General Lesser Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . */ #include "bits.h" diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.h b/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.h index 96ae12b7d..cd7b1c959 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.h +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.h @@ -1,13 +1,32 @@ -/* +/*! + * \file bits.h + * \author Fergus Noble + * + * ------------------------------------------------------------------------- + * This file was originally borrowed from libswiftnav + * , + * a portable C library implementing GNSS related functions and algorithms, + * and then modified by J. Arribas and C. Fernandez + * * Copyright (C) 2013, 2016 Swift Navigation Inc. * Contact: Fergus Noble * - * This source is subject to the license found in the file 'LICENSE' which must - * be be distributed together with this source. All other rights reserved. + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, - * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + * This file is part of GNSS-SDR. + * + * This file is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as + * published by the Free Software Foundation, version 3. + * + * 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 + * Lesser General Lesser Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . */ #ifndef LIBSWIFTNAV_BITS_H diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c index e8c733ebd..b438c207d 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c @@ -1,15 +1,36 @@ -/* +/*! + * \file cnav_msg.c + * \author Valeri Atamaniouk + * + * ------------------------------------------------------------------------- + * This file was originally borrowed from libswiftnav + * , + * a portable C library implementing GNSS related functions and algorithms, + * and then modified by J. Arribas and C. Fernandez + * * Copyright (C) 2016 Swift Navigation Inc. * Contact: Valeri Atamaniouk * - * This source is subject to the license found in the file 'LICENSE' which must - * be be distributed together with this source. All other rights reserved. + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, - * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + * This file is part of GNSS-SDR. + * + * This file is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as + * published by the Free Software Foundation, version 3. + * + * 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 + * Lesser General Lesser Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . */ + + #include "edc.h" #include "bits.h" #include "cnav_msg.h" diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h index 684e381c5..dba1c8d6b 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h @@ -1,15 +1,35 @@ -/* +/*! + * \file cnav_msg.h + * \author Valeri Atamaniouk + * + * ------------------------------------------------------------------------- + * This file was originally borrowed from libswiftnav + * , + * a portable C library implementing GNSS related functions and algorithms, + * and then modified by J. Arribas and C. Fernandez + * * Copyright (C) 2016 Swift Navigation Inc. * Contact: Valeri Atamaniouk * - * This source is subject to the license found in the file 'LICENSE' which must - * be be distributed together with this source. All other rights reserved. + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, - * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + * This file is part of GNSS-SDR. + * + * This file is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as + * published by the Free Software Foundation, version 3. + * + * 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 + * Lesser General Lesser Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . */ + #ifndef LIBSWIFTNAV_CNAV_MSG_H #define LIBSWIFTNAV_CNAV_MSG_H diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c index 280c68c7e..422c8a05e 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c @@ -1,13 +1,32 @@ -/* +/*! + * \file edc.c + * \author Fergus Noble + * + * ------------------------------------------------------------------------- + * This file was originally borrowed from libswiftnav + * , + * a portable C library implementing GNSS related functions and algorithms, + * and then modified by J. Arribas and C. Fernandez + * * Copyright (C) 2010 Swift Navigation Inc. * Contact: Fergus Noble * - * This source is subject to the license found in the file 'LICENSE' which must - * be be distributed together with this source. All other rights reserved. + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, - * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + * This file is part of GNSS-SDR. + * + * This file is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as + * published by the Free Software Foundation, version 3. + * + * 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 + * Lesser General Lesser Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . */ #include "edc.h" diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.h b/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.h index f6d6c28ff..32a7d67ae 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.h +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.h @@ -1,15 +1,35 @@ -/* +/*! + * \file edc.h + * \author Fergus Noble + * + * ------------------------------------------------------------------------- + * This file was originally borrowed from libswiftnav + * , + * a portable C library implementing GNSS related functions and algorithms, + * and then modified by J. Arribas and C. Fernandez + * * Copyright (C) 2010 Swift Navigation Inc. * Contact: Fergus Noble * - * This source is subject to the license found in the file 'LICENSE' which must - * be be distributed together with this source. All other rights reserved. + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, - * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + * This file is part of GNSS-SDR. + * + * This file is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as + * published by the Free Software Foundation, version 3. + * + * 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 + * Lesser General Lesser Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . */ + #ifndef LIBSWIFTNAV_EDC_H #define LIBSWIFTNAV_EDC_H diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h b/src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h index 296358d2d..8842f49bf 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h @@ -1,10 +1,36 @@ -/* User include file for libfec - * Copyright 2004, Phil Karn, KA9Q - * May be used under the terms of the GNU Lesser General Public License (LGPL) +/*! + * \file fec.h + * \author Phil Karn, KA9Q + * + * ------------------------------------------------------------------------- + * This file was originally borrowed from libswiftnav + * , + * a portable C library implementing GNSS related functions and algorithms, + * and then modified by J. Arribas and C. Fernandez + * + * Copyright (C) 2004, Phil Karn, KA9Q + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * This file is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as + * published by the Free Software Foundation, version 3. + * + * 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 + * Lesser General Lesser Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . */ -#ifndef _FEC_H_ -#define _FEC_H_ + +#ifndef LIBSWIFTNAV_FEC_H_ +#define LIBSWIFTNAV_FEC_H_ /* r=1/2 k=7 convolutional encoder polynomials * The NASA-DSN convention is to use V27POLYA inverted, then V27POLYB diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/swift_common.h b/src/algorithms/telemetry_decoder/libs/libswiftcnav/swift_common.h index c673596c7..abfc25492 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/swift_common.h +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/swift_common.h @@ -1,16 +1,37 @@ -/* +/*! + * \file swift_common.h + * \author Henry Hallam + * Fergus Noble + * + * ------------------------------------------------------------------------- + * This file was originally borrowed from libswiftnav + * , + * a portable C library implementing GNSS related functions and algorithms, + * and then modified by J. Arribas and C. Fernandez + * * Copyright (C) 2012 Swift Navigation Inc. * Contact: Henry Hallam * Fergus Noble * - * This source is subject to the license found in the file 'LICENSE' which must - * be be distributed together with this source. All other rights reserved. + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver * - * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, - * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + * This file is part of GNSS-SDR. + * + * This file is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as + * published by the Free Software Foundation, version 3. + * + * 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 + * Lesser General Lesser Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . */ + #ifndef LIBSWIFTNAV_COMMON_H #define LIBSWIFTNAV_COMMON_H diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c index 33ea04594..c2227a2d8 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c @@ -1,10 +1,36 @@ -/* K=7 r=1/2 Viterbi decoder in portable C - * Copyright Feb 2004, Phil Karn, KA9Q - * May be used under the terms of the GNU Lesser General Public License (LGPL) +/*! + * \file viterbi27.c + * \author Phil Karn, KA9Q + * \brief K=7 r=1/2 Viterbi decoder in portable C + * + * ------------------------------------------------------------------------- + * This file was originally borrowed from libswiftnav + * , + * a portable C library implementing GNSS related functions and algorithms, + * and then modified by J. Arribas and C. Fernandez + * + * Copyright (C) 2004, Phil Karn, KA9Q + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * This file is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as + * published by the Free Software Foundation, version 3. + * + * 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 + * Lesser General Lesser Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . */ -#include +#include #include "fec.h" static inline int parity(int x) From cff63b378a4e18abb0c5c437cc8d3d3781803790 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 28 Mar 2017 19:32:42 +0200 Subject: [PATCH 7/8] Adapt to gnss-sdr coding style --- .../libs/libswiftcnav/bits.c | 175 +++---- .../libs/libswiftcnav/cnav_msg.c | 457 ++++++++++-------- .../libs/libswiftcnav/cnav_msg.h | 50 +- .../telemetry_decoder/libs/libswiftcnav/edc.c | 109 +++-- .../telemetry_decoder/libs/libswiftcnav/fec.h | 22 +- .../libs/libswiftcnav/viterbi27.c | 252 +++++----- 6 files changed, 561 insertions(+), 504 deletions(-) diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c index fc1243789..33edf91f8 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c @@ -52,11 +52,11 @@ static const u8 bitn[16] = {0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4}; */ u8 parity(u32 x) { - x ^= x >> 16; - x ^= x >> 8; - x ^= x >> 4; - x &= 0xF; - return (0x6996 >> x) & 1; + x ^= x >> 16; + x ^= x >> 8; + x ^= x >> 4; + x &= 0xF; + return (0x6996 >> x) & 1; } @@ -72,11 +72,12 @@ u8 parity(u32 x) u32 getbitu(const u8 *buff, u32 pos, u8 len) { u32 bits = 0; - u32 i=0; - for ( i= pos; i < pos + len; i++) { - bits = (bits << 1) + - ((buff[i/8] >> (7 - i%8)) & 1u); - } + u32 i=0; + for (i = pos; i < pos + len; i++) + { + bits = (bits << 1) + + ((buff[i/8] >> (7 - i%8)) & 1u); + } return bits; } @@ -114,17 +115,18 @@ s32 getbits(const u8 *buff, u32 pos, u8 len) */ void setbitu(u8 *buff, u32 pos, u32 len, u32 data) { - u32 mask = 1u << (len - 1); + u32 mask = 1u << (len - 1); - if (len <= 0 || 32 < len) - return; - u32 i=0; - for (i = pos; i < pos + len; i++, mask >>= 1) { - if (data & mask) - buff[i/8] |= 1u << (7 - i % 8); - else - buff[i/8] &= ~(1u << (7 - i % 8)); - } + if (len <= 0 || 32 < len) + return; + u32 i = 0; + for (i = pos; i < pos + len; i++, mask >>= 1) + { + if (data & mask) + buff[i/8] |= 1u << (7 - i % 8); + else + buff[i/8] &= ~(1u << (7 - i % 8)); + } } /** Set bit field in buffer from a signed integer. @@ -138,7 +140,7 @@ void setbitu(u8 *buff, u32 pos, u32 len, u32 data) */ void setbits(u8 *buff, u32 pos, u32 len, s32 data) { - setbitu(buff, pos, len, (u32)data); + setbitu(buff, pos, len, (u32)data); } /** @@ -153,38 +155,43 @@ void setbits(u8 *buff, u32 pos, u32 len, s32 data) */ void bitshl(void *buf, u32 size, u32 shift) { - if (shift > size * CHAR_BIT) { - /* Quick check: if the shift is larger, than the buffer, zero the data */ - memset(buf, 0, size); - return; - } + if (shift > size * CHAR_BIT) + { + /* Quick check: if the shift is larger, than the buffer, zero the data */ + memset(buf, 0, size); + return; + } - unsigned char *dst = buf; /* Destination byte. */ - const unsigned char *src = dst + shift / CHAR_BIT; /* First source byte, - * possibly incomplete. */ + unsigned char *dst = buf; /* Destination byte. */ + const unsigned char *src = dst + shift / CHAR_BIT; /* First source byte, possibly incomplete. */ - u32 copy_bits = size * CHAR_BIT - shift; /* Number of bits to move */ - u32 byte_shift = copy_bits % CHAR_BIT; /* Shift of data */ - u32 full_bytes = copy_bits / CHAR_BIT; /* Number of bytes to move */ + u32 copy_bits = size * CHAR_BIT - shift; /* Number of bits to move */ + u32 byte_shift = copy_bits % CHAR_BIT; /* Shift of data */ + u32 full_bytes = copy_bits / CHAR_BIT; /* Number of bytes to move */ - if (0 == byte_shift) { - /* When moving data in character boundaries, use built-in functions: move - * data, and then zero the tail. */ - memmove(dst, src, full_bytes); - memset(dst + full_bytes, 0, size - full_bytes); - } else { - /* Create an accumulator: it will hold a value of two consecutive bytes */ - u32 acc = *src++; - u32 i=0; - for (i = 0; i < full_bytes; ++i) { - acc = (acc << CHAR_BIT) | *src++; - *dst++ = acc >> byte_shift; - } - *dst++ = acc << CHAR_BIT >> byte_shift; - if (full_bytes + 1 < size) { - memset(dst, 0, size - full_bytes - 1); - } - } + if (0 == byte_shift) + { + /* When moving data in character boundaries, use built-in functions: move + * data, and then zero the tail. */ + memmove(dst, src, full_bytes); + memset(dst + full_bytes, 0, size - full_bytes); + } + else + { + /* Create an accumulator: it will hold a value of two consecutive bytes */ + u32 acc = *src++; + u32 i = 0; + for (i = 0; i < full_bytes; ++i) + { + acc = (acc << CHAR_BIT) | *src++; + *dst++ = acc >> byte_shift; + } + *dst++ = acc << CHAR_BIT >> byte_shift; + if (full_bytes + 1 < size) + { + memset(dst, 0, size - full_bytes - 1); + } + } } /** @@ -206,19 +213,21 @@ void bitshl(void *buf, u32 size, u32 shift) void bitcopy(void *dst, u32 dst_index, const void *src, u32 src_index, u32 count) { - u32 limit1 = count / 32; - u32 limit2 = count % 32; - u32 idx=0; - for (idx = 0; idx < limit1; ++idx) { - u32 tmp = getbitu(src, src_index, 32); - setbitu(dst, dst_index, 32, tmp); - src_index += 32; - dst_index += 32; - } - if (0 != limit2) { - u32 tmp = getbitu(src, src_index, limit2); - setbitu(dst, dst_index, limit2, tmp); - } + u32 limit1 = count / 32; + u32 limit2 = count % 32; + u32 idx = 0; + for (idx = 0; idx < limit1; ++idx) + { + u32 tmp = getbitu(src, src_index, 32); + setbitu(dst, dst_index, 32, tmp); + src_index += 32; + dst_index += 32; + } + if (0 != limit2) + { + u32 tmp = getbitu(src, src_index, limit2); + setbitu(dst, dst_index, limit2, tmp); + } } /** @@ -231,11 +240,11 @@ void bitcopy(void *dst, u32 dst_index, const void *src, u32 src_index, */ u8 count_bits_u64(u64 v, u8 bv) { - u8 r = 0; -int i =0; - for (i = 0; i < 16; i++) - r += bitn[(v >> (i*4)) & 0xf]; - return bv == 1 ? r : 64 - r; + u8 r = 0; + int i = 0; + for (i = 0; i < 16; i++) + r += bitn[(v >> (i*4)) & 0xf]; + return bv == 1 ? r : 64 - r; } /** @@ -248,11 +257,11 @@ int i =0; */ u8 count_bits_u32(u32 v, u8 bv) { - u8 r = 0; - int i=0; - for (i = 0; i < 8; i++) - r += bitn[(v >> (i*4)) & 0xf]; - return bv == 1 ? r : 32 - r; + u8 r = 0; + int i = 0; + for (i = 0; i < 8; i++) + r += bitn[(v >> (i*4)) & 0xf]; + return bv == 1 ? r : 32 - r; } /** @@ -265,11 +274,11 @@ u8 count_bits_u32(u32 v, u8 bv) */ u8 count_bits_u16(u16 v, u8 bv) { - u8 r = 0; -int i =0; - for (i= 0; i < 4; i++) - r += bitn[(v >> (i*4)) & 0xf]; - return bv == 1 ? r : 16 - r; + u8 r = 0; + int i = 0; + for (i= 0; i < 4; i++) + r += bitn[(v >> (i*4)) & 0xf]; + return bv == 1 ? r : 16 - r; } /** @@ -282,11 +291,11 @@ int i =0; */ u8 count_bits_u8(u8 v, u8 bv) { - u8 r = 0; -int i=0; - for (i = 0; i < 2; i++) - r += bitn[(v >> (i*4)) & 0xf]; - return bv == 1 ? r : 8 - r; + u8 r = 0; + int i = 0; + for (i = 0; i < 2; i++) + r += bitn[(v >> (i*4)) & 0xf]; + return bv == 1 ? r : 8 - r; } /** \} */ diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c index 5e33c4ba9..bbb9a92f6 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c @@ -84,12 +84,13 @@ */ static u32 _cnav_compute_crc(cnav_v27_part_t *part) { - u32 crc = crc24q_bits(0, part->decoded, GPS_CNAV_MSG_DATA_LENGTH, - part->invert); + u32 crc = crc24q_bits(0, part->decoded, GPS_CNAV_MSG_DATA_LENGTH, + part->invert); - return crc; + return crc; } + /** * Extracts CRC-24Q from a CNAV message buffer. * CRC-24Q value is the last 24 bits from 300 bits message buffer. @@ -102,14 +103,16 @@ static u32 _cnav_compute_crc(cnav_v27_part_t *part) */ static u32 _cnav_extract_crc(const cnav_v27_part_t *part) { - u32 crc = getbitu(part->decoded, GPS_CNAV_MSG_DATA_LENGTH, - GPS_CNAV_MSG_CRC_LENGTH); - if (part->invert) { - crc ^= 0xFFFFFF; - } - return crc; + u32 crc = getbitu(part->decoded, GPS_CNAV_MSG_DATA_LENGTH, + GPS_CNAV_MSG_CRC_LENGTH); + if (part->invert) + { + crc ^= 0xFFFFFF; + } + return crc; } + /** * Helper to rescan for preamble in the received buffer. * Occasionally there could be a false lock on message contents if it the @@ -126,31 +129,35 @@ static u32 _cnav_extract_crc(const cnav_v27_part_t *part) */ static void _cnav_rescan_preamble(cnav_v27_part_t *part) { - part->preamble_seen = false; + part->preamble_seen = false; - if (part->n_decoded > GPS_CNAV_PREAMBLE_LENGTH + 1) { - size_t i=0; - size_t j=0; - for (i = 1, j = part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH; - i < j; ++i) { - u32 c = getbitu(part->decoded, i, GPS_CNAV_PREAMBLE_LENGTH); - if (GPS_CNAV_PREAMBLE1 == c || GPS_CNAV_PREAMBLE2 == c) { - part->preamble_seen = true; - part->invert = (GPS_CNAV_PREAMBLE2 == c); - /* We shift the accumulated bits to the beginning of the buffer */ - bitshl(part->decoded, sizeof(part->decoded), i); - part->n_decoded -= i; - break; - } - } - } - if (!part->preamble_seen && part->n_decoded >= GPS_CNAV_PREAMBLE_LENGTH) { - bitshl(part->decoded, sizeof(part->decoded), - part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH + 1); - part->n_decoded = GPS_CNAV_PREAMBLE_LENGTH - 1; - } + if (part->n_decoded > GPS_CNAV_PREAMBLE_LENGTH + 1) + { + size_t i = 0; + size_t j = 0; + for (i = 1, j = part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH; i < j; ++i) + { + u32 c = getbitu(part->decoded, i, GPS_CNAV_PREAMBLE_LENGTH); + if (GPS_CNAV_PREAMBLE1 == c || GPS_CNAV_PREAMBLE2 == c) + { + part->preamble_seen = true; + part->invert = (GPS_CNAV_PREAMBLE2 == c); + /* We shift the accumulated bits to the beginning of the buffer */ + bitshl(part->decoded, sizeof(part->decoded), i); + part->n_decoded -= i; + break; + } + } + } + if (!part->preamble_seen && part->n_decoded >= GPS_CNAV_PREAMBLE_LENGTH) + { + bitshl(part->decoded, sizeof(part->decoded), + part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH + 1); + part->n_decoded = GPS_CNAV_PREAMBLE_LENGTH - 1; + } } + /** * Feed a symbol into Viterbi decoder instance. * @@ -167,110 +174,126 @@ static void _cnav_rescan_preamble(cnav_v27_part_t *part) */ static void _cnav_add_symbol(cnav_v27_part_t *part, u8 ch) { - part->symbols[part->n_symbols++] = ch; + part->symbols[part->n_symbols++] = ch; - if (part->init) { - /* Initial step - load more symbols without decoding. */ - if (part->n_symbols < (GPS_L2C_V27_INIT_BITS + GPS_L2C_V27_DECODE_BITS) * 2) { - return; - } - part->init = false; - } - else if (part->n_symbols < GPS_L2C_V27_DECODE_BITS * 2) { - /* Wait until decoding block is accumulated */ - return; - } - - /* Feed accumulated symbols into the buffer, reset the number of accumulated - * symbols. */ - v27_update(&part->dec, part->symbols, part->n_symbols / 2); - part->n_symbols = 0; - - /* Decode N+M bits, where: - * - N - Number of bits to put into decoded buffer - * - M - Number of bits in the tail to ignore. - */ - unsigned char tmp_bits[ (GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS + - CHAR_BIT - 1) / CHAR_BIT]; - - v27_chainback_likely(&part->dec, tmp_bits, - GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS); - - /* Read decoded bits and add them to the decoded buffer */ - bitcopy(part->decoded, part->n_decoded, tmp_bits, 0, GPS_L2C_V27_DECODE_BITS); - part->n_decoded += GPS_L2C_V27_DECODE_BITS; - - /* Depending on the decoder state, one of the following actions are - * possible: - * - If no message lock - * - If no preamble seen - look for preamble - * - If preamble seen - collect 300 bits - * - If 300 bits are collected - verify CRC - * - If CRC is OK - message lock is acquired - * - If CRC fails - rescan for preamble - * - If found - continue collecting 300 bits - * - If not found - continue preamble wait - * - If message lock - * - If 300 bits collected, compute CRC - * - If CRC is OK, message can be decoded - * - If CRC is not OK, discard data - */ - - bool retry = true; - while (retry) { - retry = false; - - if (!part->preamble_seen) { - /* Rescan for preamble if possible. The first bit is ignored. */ - _cnav_rescan_preamble(part); - } - if (part->preamble_seen && GPS_CNAV_MSG_LENGTH <= part->n_decoded) { - - /* We have collected 300 bits starting from message preamble. Now try - * to compute CRC-24Q */ - u32 crc = _cnav_compute_crc(part); - u32 crc2 = _cnav_extract_crc(part); - - if (part->message_lock) { - /* We have message lock */ - part->crc_ok = (crc == crc2); - if (part->crc_ok) { - /* Reset message lock counter */ - part->n_crc_fail = 0; - } else { - /* Increment message lock counter */ - part->n_crc_fail++; - if (part->n_crc_fail > GPS_CNAV_LOCK_MAX_CRC_FAILS) { - /* CRC has failed too many times - drop the lock. */ - part->n_crc_fail = 0; - part->message_lock = false; - part->preamble_seen = false; - /* Try to find a new preamble, reuse data from buffer. */ - retry = true; - } + if (part->init) + { + /* Initial step - load more symbols without decoding. */ + if (part->n_symbols < (GPS_L2C_V27_INIT_BITS + GPS_L2C_V27_DECODE_BITS) * 2) + { + return; + } + part->init = false; + } + else if (part->n_symbols < GPS_L2C_V27_DECODE_BITS * 2) + { + /* Wait until decoding block is accumulated */ + return; } - } else if (crc == crc2) { - /* CRC match - message can be decoded */ - part->message_lock = true; - part->crc_ok = true; - part->n_crc_fail = 0; - } else { - /* There is no message lock and the CRC check fails. Assume there is - * false positive lock - rescan for preamble. */ - part->crc_ok = false; - part->preamble_seen = false; - /* CRC mismatch - try to re-scan for preamble */ - retry = true; - } - } - else - { - /* No preamble or preamble and less than 300 bits decoded */ - } - } + /* Feed accumulated symbols into the buffer, reset the number of accumulated + * symbols. */ + v27_update(&part->dec, part->symbols, part->n_symbols / 2); + part->n_symbols = 0; + + /* Decode N+M bits, where: + * - N - Number of bits to put into decoded buffer + * - M - Number of bits in the tail to ignore. + */ + unsigned char tmp_bits[ (GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS + + CHAR_BIT - 1) / CHAR_BIT]; + + v27_chainback_likely(&part->dec, tmp_bits, + GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS); + + /* Read decoded bits and add them to the decoded buffer */ + bitcopy(part->decoded, part->n_decoded, tmp_bits, 0, GPS_L2C_V27_DECODE_BITS); + part->n_decoded += GPS_L2C_V27_DECODE_BITS; + + /* Depending on the decoder state, one of the following actions are + * possible: + * - If no message lock + * - If no preamble seen - look for preamble + * - If preamble seen - collect 300 bits + * - If 300 bits are collected - verify CRC + * - If CRC is OK - message lock is acquired + * - If CRC fails - rescan for preamble + * - If found - continue collecting 300 bits + * - If not found - continue preamble wait + * - If message lock + * - If 300 bits collected, compute CRC + * - If CRC is OK, message can be decoded + * - If CRC is not OK, discard data + */ + + bool retry = true; + while (retry) + { + retry = false; + + if (!part->preamble_seen) + { + /* Rescan for preamble if possible. The first bit is ignored. */ + _cnav_rescan_preamble(part); + } + if (part->preamble_seen && GPS_CNAV_MSG_LENGTH <= part->n_decoded) + { + + /* We have collected 300 bits starting from message preamble. Now try + * to compute CRC-24Q */ + u32 crc = _cnav_compute_crc(part); + u32 crc2 = _cnav_extract_crc(part); + + if (part->message_lock) + { + /* We have message lock */ + part->crc_ok = (crc == crc2); + if (part->crc_ok) + { + /* Reset message lock counter */ + part->n_crc_fail = 0; + } + else + { + /* Increment message lock counter */ + part->n_crc_fail++; + if (part->n_crc_fail > GPS_CNAV_LOCK_MAX_CRC_FAILS) + { + /* CRC has failed too many times - drop the lock. */ + part->n_crc_fail = 0; + part->message_lock = false; + part->preamble_seen = false; + /* Try to find a new preamble, reuse data from buffer. */ + retry = true; + } + } + } + else if (crc == crc2) + { + /* CRC match - message can be decoded */ + part->message_lock = true; + part->crc_ok = true; + part->n_crc_fail = 0; + } + else + { + /* There is no message lock and the CRC check fails. Assume there is + * false positive lock - rescan for preamble. */ + part->crc_ok = false; + part->preamble_seen = false; + + /* CRC mismatch - try to re-scan for preamble */ + retry = true; + } + } + else + { + /* No preamble or preamble and less than 300 bits decoded */ + } + } } + /** * Invert message bits in the buffer. * @@ -282,12 +305,14 @@ static void _cnav_add_symbol(cnav_v27_part_t *part, u8 ch) */ static void _cnav_msg_invert(cnav_v27_part_t *part) { - size_t i = 0; - for (i = 0; i < sizeof(part->decoded); i++) { - part->decoded[i] ^= 0xFFu; - } + size_t i = 0; + for (i = 0; i < sizeof(part->decoded); i++) + { + part->decoded[i] ^= 0xFFu; + } } + /** * Performs CNAV message decoding. * @@ -310,39 +335,45 @@ static void _cnav_msg_invert(cnav_v27_part_t *part) */ static bool _cnav_msg_decode(cnav_v27_part_t *part, cnav_msg_t *msg, u32 *delay) { - bool res = false; - if (GPS_CNAV_MSG_LENGTH <= part->n_decoded) { - if (part->crc_ok) { - /* CRC is OK */ - if (part->invert) { - _cnav_msg_invert(part); - } + bool res = false; + if (GPS_CNAV_MSG_LENGTH <= part->n_decoded) + { + if (part->crc_ok) + { + /* CRC is OK */ + if (part->invert) + { + _cnav_msg_invert(part); + } - msg->prn = getbitu(part->decoded, 8, 6); - msg->msg_id = getbitu(part->decoded, 14, 6); - msg->tow = getbitu(part->decoded, 20, 17); - msg->alert = getbitu(part->decoded, 37, 1) ? true : false; + msg->prn = getbitu(part->decoded, 8, 6); + msg->msg_id = getbitu(part->decoded, 14, 6); + msg->tow = getbitu(part->decoded, 20, 17); + msg->alert = getbitu(part->decoded, 37, 1) ? true : false; - /* copy RAW message for GNSS-SDR */ - memcpy(msg->raw_msg,part->decoded,GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS); + /* copy RAW message for GNSS-SDR */ + memcpy(msg->raw_msg,part->decoded,GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS); - *delay = (part->n_decoded - GPS_CNAV_MSG_LENGTH + GPS_L2C_V27_DELAY_BITS) - * 2 + part->n_symbols; + *delay = (part->n_decoded - GPS_CNAV_MSG_LENGTH + GPS_L2C_V27_DELAY_BITS) * 2 + part->n_symbols; - if (part->invert) { - _cnav_msg_invert(part); - } - res = true; - } else { - /* CRC mismatch - no decoding */ - } - bitshl(part->decoded, sizeof(part->decoded), GPS_CNAV_MSG_LENGTH); - part->n_decoded -= GPS_CNAV_MSG_LENGTH; - } + if (part->invert) + { + _cnav_msg_invert(part); + } + res = true; + } + else + { + /* CRC mismatch - no decoding */ + } + bitshl(part->decoded, sizeof(part->decoded), GPS_CNAV_MSG_LENGTH); + part->n_decoded -= GPS_CNAV_MSG_LENGTH; + } - return res; + return res; } + /** * Initialize CNAV decoder. * @@ -355,20 +386,20 @@ static bool _cnav_msg_decode(cnav_v27_part_t *part, cnav_msg_t *msg, u32 *delay) */ void cnav_msg_decoder_init(cnav_msg_decoder_t *dec) { - memset(dec, 0, sizeof(*dec)); - v27_init(&dec->part1.dec, - dec->part1.decisions, - GPS_L2_V27_HISTORY_LENGTH_BITS, - cnav_msg_decoder_get_poly(), - 0); - v27_init(&dec->part2.dec, - dec->part2.decisions, - GPS_L2_V27_HISTORY_LENGTH_BITS, - cnav_msg_decoder_get_poly(), - 0); - dec->part1.init = true; - dec->part2.init = true; - _cnav_add_symbol(&dec->part2, 0x80); + memset(dec, 0, sizeof(*dec)); + v27_init(&dec->part1.dec, + dec->part1.decisions, + GPS_L2_V27_HISTORY_LENGTH_BITS, + cnav_msg_decoder_get_poly(), + 0); + v27_init(&dec->part2.dec, + dec->part2.decisions, + GPS_L2_V27_HISTORY_LENGTH_BITS, + cnav_msg_decoder_get_poly(), + 0); + dec->part1.init = true; + dec->part2.init = true; + _cnav_add_symbol(&dec->part2, 0x80); } /** @@ -394,29 +425,32 @@ void cnav_msg_decoder_init(cnav_msg_decoder_t *dec) * \retval false More data is required. */ bool cnav_msg_decoder_add_symbol(cnav_msg_decoder_t *dec, - u8 symbol, - cnav_msg_t *msg, - u32 *pdelay) + u8 symbol, + cnav_msg_t *msg, + u32 *pdelay) { - _cnav_add_symbol(&dec->part1, symbol); - _cnav_add_symbol(&dec->part2, symbol); + _cnav_add_symbol(&dec->part1, symbol); + _cnav_add_symbol(&dec->part2, symbol); - if (dec->part1.message_lock) { - /* Flush data in decoder. */ - dec->part2.n_decoded = 0; - dec->part2.n_symbols = 0; - return _cnav_msg_decode(&dec->part1, msg, pdelay); - } - if (dec->part2.message_lock) { - /* Flush data in decoder. */ - dec->part1.n_decoded = 0; - dec->part1.n_symbols = 0; - return _cnav_msg_decode(&dec->part2, msg, pdelay); - } + if (dec->part1.message_lock) + { + /* Flush data in decoder. */ + dec->part2.n_decoded = 0; + dec->part2.n_symbols = 0; + return _cnav_msg_decode(&dec->part1, msg, pdelay); + } + if (dec->part2.message_lock) + { + /* Flush data in decoder. */ + dec->part1.n_decoded = 0; + dec->part1.n_symbols = 0; + return _cnav_msg_decode(&dec->part2, msg, pdelay); + } - return false; + return false; } + /** * Provides a singleton polynomial object. * @@ -429,27 +463,28 @@ bool cnav_msg_decoder_add_symbol(cnav_msg_decoder_t *dec, */ const v27_poly_t *cnav_msg_decoder_get_poly(void) { - static v27_poly_t instance; - static bool initialized = false; + static v27_poly_t instance; + static bool initialized = false; - if (!initialized) { - /* Coefficients for polynomial object */ - const signed char coeffs[2] = { GPS_L2C_V27_POLY_A, GPS_L2C_V27_POLY_B }; + if (!initialized) + { + /* Coefficients for polynomial object */ + const signed char coeffs[2] = { GPS_L2C_V27_POLY_A, GPS_L2C_V27_POLY_B }; - /* Racing condition handling: the data can be potential initialized more - * than once in case multiple threads request concurrent access. However, - * nature of the v27_poly_init() function and data alignment ensure that - * the data returned from the earlier finished call is consistent and can - * be used even when re-initialization is happening. - * - * Other possible approaches are: - * - Replace late initialization with an explicit call. - * - Use POSIX synchronization objects like pthread_once_t. - */ - v27_poly_init(&instance, coeffs); - initialized = true; - } - return &instance; + /* Racing condition handling: the data can be potential initialized more + * than once in case multiple threads request concurrent access. However, + * nature of the v27_poly_init() function and data alignment ensure that + * the data returned from the earlier finished call is consistent and can + * be used even when re-initialization is happening. + * + * Other possible approaches are: + * - Replace late initialization with an explicit call. + * - Use POSIX synchronization objects like pthread_once_t. + */ + v27_poly_init(&instance, coeffs); + initialized = true; + } + return &instance; } /** \} */ diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h index dba1c8d6b..8b8141358 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h @@ -62,11 +62,11 @@ */ typedef struct { - u8 prn; /**< SV PRN. 0..31 */ - u8 msg_id; /**< Message id. 0..31 */ - u32 tow; /**< GPS ToW in 6-second units. Multiply to 6 to get seconds. */ - bool alert; /**< CNAV message alert flag */ - u8 raw_msg[GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS]; /**< RAW MSG for GNSS-SDR */ + u8 prn; /**< SV PRN. 0..31 */ + u8 msg_id; /**< Message id. 0..31 */ + u32 tow; /**< GPS ToW in 6-second units. Multiply to 6 to get seconds. */ + bool alert; /**< CNAV message alert flag */ + u8 raw_msg[GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS]; /**< RAW MSG for GNSS-SDR */ } cnav_msg_t; /** @@ -76,24 +76,24 @@ typedef struct * @sa cnav_msg_decoder_t */ typedef struct { - v27_t dec; /**< Viterbi block decoder object */ - v27_decision_t decisions[GPS_L2_V27_HISTORY_LENGTH_BITS]; - /**< Decision graph */ - unsigned char symbols[(GPS_L2C_V27_INIT_BITS + GPS_L2C_V27_DECODE_BITS) * 2]; - /**< Symbol buffer */ - size_t n_symbols; /**< Count of symbols in the symbol buffer */ - unsigned char decoded[GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS]; - /**< Decode buffer */ - size_t n_decoded; /**< Number of bits in the decode buffer */ - bool preamble_seen; /**< When true, the decode buffer is aligned on - * preamble. */ - bool invert; /**< When true, indicates the bits are inverted */ - bool message_lock; /**< When true, indicates the message boundary - * is found. */ - bool crc_ok; /**< Flag that the last message had good CRC */ - size_t n_crc_fail; /**< Counter for CRC failures */ - bool init; /**< Initial state flag. When true, initial bits - * do not produce output. */ + v27_t dec; /**< Viterbi block decoder object */ + v27_decision_t decisions[GPS_L2_V27_HISTORY_LENGTH_BITS]; + /**< Decision graph */ + unsigned char symbols[(GPS_L2C_V27_INIT_BITS + GPS_L2C_V27_DECODE_BITS) * 2]; + /**< Symbol buffer */ + size_t n_symbols; /**< Count of symbols in the symbol buffer */ + unsigned char decoded[GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS]; + /**< Decode buffer */ + size_t n_decoded; /**< Number of bits in the decode buffer */ + bool preamble_seen; /**< When true, the decode buffer is aligned on + * preamble. */ + bool invert; /**< When true, indicates the bits are inverted */ + bool message_lock; /**< When true, indicates the message boundary + * is found. */ + bool crc_ok; /**< Flag that the last message had good CRC */ + size_t n_crc_fail; /**< Counter for CRC failures */ + bool init; /**< Initial state flag. When true, initial bits + * do not produce output. */ } cnav_v27_part_t; /** @@ -104,8 +104,8 @@ typedef struct { */ typedef struct { - cnav_v27_part_t part1; /**< Decoder for odd symbol pairs */ - cnav_v27_part_t part2; /**< Decoder for even symbol pairs */ + cnav_v27_part_t part1; /**< Decoder for odd symbol pairs */ + cnav_v27_part_t part2; /**< Decoder for even symbol pairs */ } cnav_msg_decoder_t; const v27_poly_t *cnav_msg_decoder_get_poly(void); diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c index 370304d04..dc5fee6b7 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c @@ -40,38 +40,38 @@ * \{ */ static const u32 crc24qtab[256] = { - 0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17, - 0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E, - 0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E, - 0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7, - 0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE, - 0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7, - 0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077, - 0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E, - 0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5, - 0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC, - 0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C, - 0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375, - 0x15723B, 0x933EC0, 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C, - 0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15, - 0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5, - 0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C, - 0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3, - 0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A, - 0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A, - 0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703, - 0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A, - 0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863, - 0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3, - 0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA, - 0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61, - 0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58, - 0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8, - 0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1, - 0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88, - 0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1, - 0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401, - 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538 + 0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17, + 0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E, + 0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E, + 0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7, + 0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE, + 0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7, + 0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077, + 0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E, + 0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5, + 0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC, + 0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C, + 0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375, + 0x15723B, 0x933EC0, 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C, + 0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15, + 0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5, + 0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C, + 0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3, + 0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A, + 0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A, + 0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703, + 0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A, + 0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863, + 0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3, + 0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA, + 0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61, + 0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58, + 0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8, + 0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1, + 0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88, + 0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1, + 0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401, + 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538 }; /** Calculate Qualcomm 24-bit Cyclical Redundancy Check (CRC-24Q). @@ -91,10 +91,10 @@ static const u32 crc24qtab[256] = { */ u32 crc24q(const u8 *buf, u32 len, u32 crc) { - u32 i=0; - for ( i= 0; i < len; i++) - crc = ((crc << 8) & 0xFFFFFF) ^ crc24qtab[((crc >> 16) ^ buf[i]) & 0xff]; - return crc; + u32 i = 0; + for (i = 0; i < len; i++) + crc = ((crc << 8) & 0xFFFFFF) ^ crc24qtab[((crc >> 16) ^ buf[i]) & 0xff]; + return crc; } /** @@ -114,27 +114,30 @@ u32 crc24q(const u8 *buf, u32 len, u32 crc) */ u32 crc24q_bits(u32 crc, const u8 *buf, u32 n_bits, bool invert) { - u16 acc = 0; - u8 b = 0; - u32 shift = 8 - n_bits % 8; + u16 acc = 0; + u8 b = 0; + u32 shift = 8 - n_bits % 8; - u32 i=0; - for ( i= 0; i < n_bits / 8; ++i) { - acc = (acc << 8) | *buf++; - if (invert) { - acc ^= 0xFFu; - } + u32 i = 0; + for (i = 0; i < n_bits / 8; ++i) + { + acc = (acc << 8) | *buf++; + if (invert) + { + acc ^= 0xFFu; + } + b = (acc >> shift) & 0xFFu; + crc = ((crc << 8) & 0xFFFFFFu) ^ crc24qtab[((crc >> 16) ^ b) & 0xFFu]; + } + acc = (acc << 8) | *buf; + if (invert) + { + acc ^= 0xFFu; + } b = (acc >> shift) & 0xFFu; crc = ((crc << 8) & 0xFFFFFFu) ^ crc24qtab[((crc >> 16) ^ b) & 0xFFu]; - } - acc = (acc << 8) | *buf; - if (invert) { - acc ^= 0xFFu; - } - b = (acc >> shift) & 0xFFu; - crc = ((crc << 8) & 0xFFFFFFu) ^ crc24qtab[((crc >> 16) ^ b) & 0xFFu]; - return crc; + return crc; } diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h b/src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h index 8842f49bf..d5196dcc4 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h @@ -40,25 +40,25 @@ #define V27POLYB 0x6d typedef struct { - unsigned char c0[32]; - unsigned char c1[32]; + unsigned char c0[32]; + unsigned char c1[32]; } v27_poly_t; typedef struct { - unsigned int w[2]; + unsigned int w[2]; } v27_decision_t; /* State info for instance of r=1/2 k=7 Viterbi decoder */ typedef struct { - unsigned int metrics1[64]; /* Path metric buffer 1 */ - unsigned int metrics2[64]; /* Path metric buffer 2 */ - /* Pointers to path metrics, swapped on every bit */ - unsigned int *old_metrics, *new_metrics; - const v27_poly_t *poly; /* Polynomial to use */ - v27_decision_t *decisions; /* Beginning of decisions for block */ - unsigned int decisions_index; /* Index of current decision */ - unsigned int decisions_count; /* Number of decisions in history */ + unsigned int metrics1[64]; /* Path metric buffer 1 */ + unsigned int metrics2[64]; /* Path metric buffer 2 */ + /* Pointers to path metrics, swapped on every bit */ + unsigned int *old_metrics, *new_metrics; + const v27_poly_t *poly; /* Polynomial to use */ + v27_decision_t *decisions; /* Beginning of decisions for block */ + unsigned int decisions_index; /* Index of current decision */ + unsigned int decisions_count; /* Number of decisions in history */ } v27_t; void v27_poly_init(v27_poly_t *poly, const signed char polynomial[2]); diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c index c2227a2d8..958b0e635 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c @@ -35,13 +35,14 @@ static inline int parity(int x) { - x ^= x >> 16; - x ^= x >> 8; - x ^= x >> 4; - x &= 0xf; - return (0x6996 >> x) & 1; + x ^= x >> 16; + x ^= x >> 8; + x ^= x >> 4; + x &= 0xf; + return (0x6996 >> x) & 1; } + /** Initialize a v27_poly_t struct for use with a v27_t decoder. * * \param poly Structure to initialize. @@ -49,14 +50,16 @@ static inline int parity(int x) */ void v27_poly_init(v27_poly_t *poly, const signed char polynomial[2]) { - int state; + int state; - for(state = 0; state < 32; state++) { - poly->c0[state] = (polynomial[0] < 0) ^ parity((2*state) & abs(polynomial[0])) ? 255 : 0; - poly->c1[state] = (polynomial[1] < 0) ^ parity((2*state) & abs(polynomial[1])) ? 255 : 0; - } + for(state = 0; state < 32; state++) + { + poly->c0[state] = (polynomial[0] < 0) ^ parity((2*state) & abs(polynomial[0])) ? 255 : 0; + poly->c1[state] = (polynomial[1] < 0) ^ parity((2*state) & abs(polynomial[1])) ? 255 : 0; + } } + /** Initialize a v27_t struct for Viterbi decoding. * * \param v Structure to initialize @@ -71,35 +74,36 @@ void v27_poly_init(v27_poly_t *poly, const signed char polynomial[2]) void v27_init(v27_t *v, v27_decision_t *decisions, unsigned int decisions_count, const v27_poly_t *poly, unsigned char initial_state) { - int i; + int i; - v->old_metrics = v->metrics1; - v->new_metrics = v->metrics2; - v->poly = poly; - v->decisions = decisions; - v->decisions_index = 0; - v->decisions_count = decisions_count; + v->old_metrics = v->metrics1; + v->new_metrics = v->metrics2; + v->poly = poly; + v->decisions = decisions; + v->decisions_index = 0; + v->decisions_count = decisions_count; - for(i = 0; i < 64; i++) - v->old_metrics[i] = 63; + for(i = 0; i < 64; i++) + v->old_metrics[i] = 63; - v->old_metrics[initial_state & 63] = 0; /* Bias known start state */ + v->old_metrics[initial_state & 63] = 0; /* Bias known start state */ } + /* C-language butterfly */ #define BFLY(i) {\ -unsigned int metric,m0,m1,decision;\ - metric = (v->poly->c0[i] ^ sym0) + (v->poly->c1[i] ^ sym1);\ - m0 = v->old_metrics[i] + metric;\ - m1 = v->old_metrics[i+32] + (510 - metric);\ - decision = (signed int)(m0-m1) > 0;\ - v->new_metrics[2*i] = decision ? m1 : m0;\ - d->w[i/16] |= decision << ((2*i)&31);\ - m0 -= (metric+metric-510);\ - m1 += (metric+metric-510);\ - decision = (signed int)(m0-m1) > 0;\ - v->new_metrics[2*i+1] = decision ? m1 : m0;\ - d->w[i/16] |= decision << ((2*i+1)&31);\ + unsigned int metric,m0,m1,decision;\ + metric = (v->poly->c0[i] ^ sym0) + (v->poly->c1[i] ^ sym1);\ + m0 = v->old_metrics[i] + metric;\ + m1 = v->old_metrics[i+32] + (510 - metric);\ + decision = (signed int)(m0-m1) > 0;\ + v->new_metrics[2*i] = decision ? m1 : m0;\ + d->w[i/16] |= decision << ((2*i)&31);\ + m0 -= (metric+metric-510);\ + m1 += (metric+metric-510);\ + decision = (signed int)(m0-m1) > 0;\ + v->new_metrics[2*i+1] = decision ? m1 : m0;\ + d->w[i/16] |= decision << ((2*i+1)&31);\ } /** Update a v27_t decoder with a block of symbols. @@ -111,77 +115,81 @@ unsigned int metric,m0,m1,decision;\ */ void v27_update(v27_t *v, const unsigned char *syms, int nbits) { - unsigned char sym0, sym1; - unsigned int *tmp; - int normalize = 0; + unsigned char sym0, sym1; + unsigned int *tmp; + int normalize = 0; - while(nbits--) { - v27_decision_t *d = &v->decisions[v->decisions_index]; + while(nbits--) + { + v27_decision_t *d = &v->decisions[v->decisions_index]; - d->w[0] = d->w[1] = 0; - sym0 = *syms++; - sym1 = *syms++; + d->w[0] = d->w[1] = 0; + sym0 = *syms++; + sym1 = *syms++; - BFLY(0); - BFLY(1); - BFLY(2); - BFLY(3); - BFLY(4); - BFLY(5); - BFLY(6); - BFLY(7); - BFLY(8); - BFLY(9); - BFLY(10); - BFLY(11); - BFLY(12); - BFLY(13); - BFLY(14); - BFLY(15); - BFLY(16); - BFLY(17); - BFLY(18); - BFLY(19); - BFLY(20); - BFLY(21); - BFLY(22); - BFLY(23); - BFLY(24); - BFLY(25); - BFLY(26); - BFLY(27); - BFLY(28); - BFLY(29); - BFLY(30); - BFLY(31); + BFLY(0); + BFLY(1); + BFLY(2); + BFLY(3); + BFLY(4); + BFLY(5); + BFLY(6); + BFLY(7); + BFLY(8); + BFLY(9); + BFLY(10); + BFLY(11); + BFLY(12); + BFLY(13); + BFLY(14); + BFLY(15); + BFLY(16); + BFLY(17); + BFLY(18); + BFLY(19); + BFLY(20); + BFLY(21); + BFLY(22); + BFLY(23); + BFLY(24); + BFLY(25); + BFLY(26); + BFLY(27); + BFLY(28); + BFLY(29); + BFLY(30); + BFLY(31); - /* Normalize metrics if they are nearing overflow */ - if(v->new_metrics[0] > (1<<30)) { - int i; - unsigned int minmetric = 1<<31; + /* Normalize metrics if they are nearing overflow */ + if(v->new_metrics[0] > (1 << 30)) + { + int i; + unsigned int minmetric = 1 << 31; - for(i=0; i<64; i++) { - if(v->new_metrics[i] < minmetric) - minmetric = v->new_metrics[i]; - } + for(i = 0; i < 64; i++) + { + if(v->new_metrics[i] < minmetric) + minmetric = v->new_metrics[i]; + } - for(i=0; i<64; i++) - v->new_metrics[i] -= minmetric; + for(i = 0; i < 64; i++) + v->new_metrics[i] -= minmetric; - normalize += minmetric; - } + normalize += minmetric; + } - /* Advance decision index */ - if(++v->decisions_index >= v->decisions_count) - v->decisions_index = 0; + /* Advance decision index */ + if(++v->decisions_index >= v->decisions_count) + v->decisions_index = 0; - /* Swap pointers to old and new metrics */ - tmp = v->old_metrics; - v->old_metrics = v->new_metrics; - v->new_metrics = tmp; - } + /* Swap pointers to old and new metrics */ + tmp = v->old_metrics; + v->old_metrics = v->new_metrics; + v->new_metrics = tmp; + } } + /** Retrieve the most likely output bit sequence with known final state from * a v27_t decoder. * @@ -193,28 +201,30 @@ void v27_update(v27_t *v, const unsigned char *syms, int nbits) void v27_chainback_fixed(v27_t *v, unsigned char *data, unsigned int nbits, unsigned char final_state) { - int k; - unsigned int decisions_index = v->decisions_index; + int k; + unsigned int decisions_index = v->decisions_index; - final_state %= 64; - final_state <<= 2; + final_state %= 64; + final_state <<= 2; - while(nbits-- != 0) { + while(nbits-- != 0) + { - /* Decrement decision index */ - decisions_index = (decisions_index == 0) ? - v->decisions_count-1 : decisions_index-1; + /* Decrement decision index */ + decisions_index = (decisions_index == 0) ? + v->decisions_count-1 : decisions_index-1; - v27_decision_t *d = &v->decisions[decisions_index]; - k = (d->w[(final_state>>2)/32] >> ((final_state>>2)%32)) & 1; - /* The store into data[] only needs to be done every 8 bits. - * But this avoids a conditional branch, and the writes will - * combine in the cache anyway - */ - data[nbits>>3] = final_state = (final_state >> 1) | (k << 7); - } + v27_decision_t *d = &v->decisions[decisions_index]; + k = (d->w[(final_state >> 2) / 32] >> ((final_state >> 2) % 32)) & 1; + /* The store into data[] only needs to be done every 8 bits. + * But this avoids a conditional branch, and the writes will + * combine in the cache anyway + */ + data[nbits >> 3] = final_state = (final_state >> 1) | (k << 7); + } } + /** Retrieve the most likely output bit sequence with unknown final state from * a v27_t decoder. * @@ -224,19 +234,19 @@ void v27_chainback_fixed(v27_t *v, unsigned char *data, unsigned int nbits, */ void v27_chainback_likely(v27_t *v, unsigned char *data, unsigned int nbits) { - /* Determine state with minimum metric */ + /* Determine state with minimum metric */ - int i; - unsigned int best_metric = 0xffffffff; - unsigned char best_state = 0; - for(i=0; i<64; i++) - { - if(v->new_metrics[i] < best_metric) - { - best_metric = v->new_metrics[i]; - best_state = i; - } - } + int i; + unsigned int best_metric = 0xffffffff; + unsigned char best_state = 0; + for(i = 0; i < 64; i++) + { + if(v->new_metrics[i] < best_metric) + { + best_metric = v->new_metrics[i]; + best_state = i; + } + } - v27_chainback_fixed(v, data, nbits, best_state); + v27_chainback_fixed(v, data, nbits, best_state); } From 620a44dad9a49447b89926520b1430b1ded756cf Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Wed, 29 Mar 2017 10:35:50 +0200 Subject: [PATCH 8/8] Fix compilation error for GPU tracking module --- .../gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc index ab56ac1e6..7acfd7c21 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc @@ -438,8 +438,6 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work (int noutput_items __attribu current_synchro_data.Prompt_Q = static_cast((d_correlator_outs[1]).imag()); // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!) current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + old_d_rem_code_phase_samples) / static_cast(d_fs_in); - // This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0 - current_synchro_data.Code_phase_secs = 0; current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;