mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-09 03:20:01 +00:00
Code Issues Releases Wiki Activity

Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next

Browse Source
This commit is contained in:
Carles Fernandez 2018-07-12 07:51:37 +02:00
commit df3a76f516
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
3 changed files with 173 additions and 148 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

295
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
View File

@ -83,26 +83,20 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
} }
} }
d_stat = 0; d_stat = 0;
d_symbol_accumulator = 0;
d_symbol_accumulator_counter = 0;
d_frame_bit_index = 0;
d_flag_frame_sync = false; d_flag_frame_sync = false;
d_GPS_frame_4bytes = 0;
d_prev_GPS_frame_4bytes = 0; d_prev_GPS_frame_4bytes = 0;
d_flag_parity = false;
d_TOW_at_Preamble_ms = 0; d_TOW_at_Preamble_ms = 0;
flag_TOW_set = false; flag_TOW_set = false;
d_flag_preamble = false; d_flag_preamble = false;
d_flag_new_tow_available = false; d_flag_new_tow_available = false;
d_word_number = 0;
d_channel = 0; d_channel = 0;
flag_PLL_180_deg_phase_locked = false; flag_PLL_180_deg_phase_locked = false;
d_preamble_time_samples = 0; d_preamble_time_samples = 0;
d_TOW_at_current_symbol_ms = 0; d_TOW_at_current_symbol_ms = 0;
d_symbol_history.resize(GPS_CA_PREAMBLE_LENGTH_SYMBOLS + 1); // Change fixed buffer size d_symbol_history.resize(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); // Change fixed buffer size
d_symbol_history.clear(); // Clear all the elements in the buffer d_symbol_history.clear(); // Clear all the elements in the buffer
d_make_correlation = true; d_crc_error_synchronization_counter = 0;
d_symbol_counter_corr = 0; d_current_subframe_symbol = 0;
} }
@ -150,9 +144,10 @@ bool gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(unsigned int gpsword)
void gps_l1_ca_telemetry_decoder_cc::set_satellite(const Gnss_Satellite &satellite) void gps_l1_ca_telemetry_decoder_cc::set_satellite(const Gnss_Satellite &satellite)
{ {
d_nav.reset();
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
DLOG(INFO) << "Setting decoder Finite State Machine to satellite " << d_satellite; DLOG(INFO) << "Setting decoder Finite State Machine to satellite " << d_satellite;
d_GPS_FSM.i_satellite_PRN = d_satellite.get_PRN(); d_nav.i_satellite_PRN = d_satellite.get_PRN();
DLOG(INFO) << "Navigation Satellite set to " << d_satellite; DLOG(INFO) << "Navigation Satellite set to " << d_satellite;
} }
@ -160,7 +155,7 @@ void gps_l1_ca_telemetry_decoder_cc::set_satellite(const Gnss_Satellite &satelli
void gps_l1_ca_telemetry_decoder_cc::set_channel(int channel) void gps_l1_ca_telemetry_decoder_cc::set_channel(int channel)
{ {
d_channel = channel; d_channel = channel;
d_GPS_FSM.i_channel_ID = channel; d_nav.i_channel_ID = channel;
DLOG(INFO) << "Navigation channel set to " << channel; DLOG(INFO) << "Navigation channel set to " << channel;
// ############# ENABLE DATA FILE LOG ################# // ############# ENABLE DATA FILE LOG #################
if (d_dump == true) if (d_dump == true)
@ -186,10 +181,128 @@ void gps_l1_ca_telemetry_decoder_cc::set_channel(int channel)
} }
bool gps_l1_ca_telemetry_decoder_cc::decode_subframe()
{
char subframe[GPS_SUBFRAME_LENGTH];
int symbol_accumulator_counter = 0;
int frame_bit_index = 0;
int word_index = 0;
uint32_t GPS_frame_4bytes = 0;
float symbol_accumulator = 0;
bool subframe_synchro_confirmation = false;
bool CRC_ok = true;
for (int n = 0; n < GPS_SUBFRAME_MS; n++)
{
//******* SYMBOL TO BIT *******
// extended correlation to bit period is enabled in tracking!
symbol_accumulator += d_subframe_symbols[n]; // accumulate the input value in d_symbol_accumulator
symbol_accumulator_counter++;
if (symbol_accumulator_counter == 20)
{
//symbol to bit
if (symbol_accumulator > 0) GPS_frame_4bytes += 1; //insert the telemetry bit in LSB
symbol_accumulator = 0;
symbol_accumulator_counter = 0;
//******* bits to words ******
frame_bit_index++;
if (frame_bit_index == 30)
{
frame_bit_index = 0;
// parity check
// Each word in wordbuff is composed of:
// Bits 0 to 29 = the GPS data word
// Bits 30 to 31 = 2 LSBs of the GPS word ahead.
// prepare the extended frame [-2 -1 0 ... 30]
if (d_prev_GPS_frame_4bytes & 0x00000001)
{
GPS_frame_4bytes = GPS_frame_4bytes | 0x40000000;
}
if (d_prev_GPS_frame_4bytes & 0x00000002)
{
GPS_frame_4bytes = GPS_frame_4bytes | 0x80000000;
}
/* Check that the 2 most recently logged words pass parity. Have to first
invert the data bits according to bit 30 of the previous word. */
if (GPS_frame_4bytes & 0x40000000)
{
GPS_frame_4bytes ^= 0x3FFFFFC0; // invert the data bits (using XOR)
}
if (gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(GPS_frame_4bytes))
{
subframe_synchro_confirmation = true;
}
else
{
//std::cout << "word invalid sat " << this->d_satellite << std::endl;
CRC_ok = false;
//break;
}
//add word to subframe
// insert the word in the correct position of the subframe
std::memcpy(&subframe[word_index * GPS_WORD_LENGTH], &GPS_frame_4bytes, sizeof(uint32_t));
word_index++;
d_prev_GPS_frame_4bytes = GPS_frame_4bytes; // save the actual frame
GPS_frame_4bytes = 0;
}
else
{
GPS_frame_4bytes <<= 1; //shift 1 bit left the telemetry word
}
}
}
//decode subframe
// NEW GPS SUBFRAME HAS ARRIVED!
if (CRC_ok)
{
int subframe_ID = d_nav.subframe_decoder(subframe); //decode the subframe
std::cout << "New GPS NAV message received in channel " << this->d_channel << ": "
<< "subframe "
<< subframe_ID << " from satellite "
<< Gnss_Satellite(std::string("GPS"), d_nav.i_satellite_PRN) << std::endl;
switch (subframe_ID)
{
case 3: //we have a new set of ephemeris data for the current SV
if (d_nav.satellite_validation() == true)
{
// get ephemeris object for this SV (mandatory)
std::shared_ptr<Gps_Ephemeris> tmp_obj = std::make_shared<Gps_Ephemeris>(d_nav.get_ephemeris());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
break;
case 4: // Possible IONOSPHERE and UTC model update (page 18)
if (d_nav.flag_iono_valid == true)
{
std::shared_ptr<Gps_Iono> tmp_obj = std::make_shared<Gps_Iono>(d_nav.get_iono());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_nav.flag_utc_model_valid == true)
{
std::shared_ptr<Gps_Utc_Model> tmp_obj = std::make_shared<Gps_Utc_Model>(d_nav.get_utc_model());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
break;
case 5:
// get almanac (if available)
//TODO: implement almanac reader in navigation_message
break;
default:
break;
}
d_flag_new_tow_available = true;
}
return subframe_synchro_confirmation;
}
int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{ {
int corr_value = 0;
int preamble_diff_ms = 0; int preamble_diff_ms = 0;
Gnss_Synchro **out = reinterpret_cast<Gnss_Synchro **>(&output_items[0]); // Get the output buffer pointer Gnss_Synchro **out = reinterpret_cast<Gnss_Synchro **>(&output_items[0]); // Get the output buffer pointer
@ -198,15 +311,25 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
Gnss_Synchro current_symbol; //structure to save the synchronization information and send the output object to the next block Gnss_Synchro current_symbol; //structure to save the synchronization information and send the output object to the next block
//1. Copy the current tracking output //1. Copy the current tracking output
current_symbol = in[0][0]; current_symbol = in[0][0];
//record the oldest subframe symbol before inserting a new symbol into the circular buffer
if (d_current_subframe_symbol < GPS_SUBFRAME_MS and d_symbol_history.size() > 0)
{
d_subframe_symbols[d_current_subframe_symbol] = d_symbol_history.at(0).Prompt_I;
d_current_subframe_symbol++;
}
d_symbol_history.push_back(current_symbol); //add new symbol to the symbol queue d_symbol_history.push_back(current_symbol); //add new symbol to the symbol queue
consume_each(1); consume_each(1);
unsigned int required_symbols = GPS_CA_PREAMBLE_LENGTH_SYMBOLS;
d_flag_preamble = false; d_flag_preamble = false;
if ((d_symbol_history.size() > required_symbols) and (d_make_correlation or !d_flag_frame_sync))
{
//******* preamble correlation ******** //******* preamble correlation ********
int corr_value = 0;
if ((d_symbol_history.size() == GPS_CA_PREAMBLE_LENGTH_SYMBOLS)) // and (d_make_correlation or !d_flag_frame_sync))
{
// std::cout << "-------\n";
for (unsigned int i = 0; i < GPS_CA_PREAMBLE_LENGTH_SYMBOLS; i++) for (unsigned int i = 0; i < GPS_CA_PREAMBLE_LENGTH_SYMBOLS; i++)
{ {
if (d_symbol_history.at(i).Flag_valid_symbol_output == true) if (d_symbol_history.at(i).Flag_valid_symbol_output == true)
@ -221,38 +344,27 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
} }
} }
} }
if (std::abs(corr_value) >= GPS_CA_PREAMBLE_LENGTH_SYMBOLS)
{
d_symbol_counter_corr++;
}
} }
//******* frame sync ****************** //******* frame sync ******************
if (std::abs(corr_value) == GPS_CA_PREAMBLE_LENGTH_SYMBOLS) if (std::abs(corr_value) == GPS_CA_PREAMBLE_LENGTH_SYMBOLS)
{ {
//TODO: Rewrite with state machine //TODO: Rewrite with state machine
if (d_stat == 0) if (d_stat == 0)
{ {
d_GPS_FSM.Event_gps_word_preamble();
//record the preamble sample stamp //record the preamble sample stamp
d_preamble_time_samples = d_symbol_history.at(0).Tracking_sample_counter; // record the preamble sample stamp d_preamble_time_samples = d_symbol_history.at(0).Tracking_sample_counter; // record the preamble sample stamp
DLOG(INFO) << "Preamble detection for SAT " << this->d_satellite << "d_symbol_history.at(0).Tracking_sample_counter=" << d_symbol_history.at(0).Tracking_sample_counter; DLOG(INFO) << "Preamble detection for SAT " << this->d_satellite << "d_symbol_history.at(0).Tracking_sample_counter=" << d_symbol_history.at(0).Tracking_sample_counter;
//sync the symbol to bits integrator
d_symbol_accumulator = 0;
d_symbol_accumulator_counter = 0;
d_frame_bit_index = 0;
d_stat = 1; // enter into frame pre-detection status d_stat = 1; // enter into frame pre-detection status
} }
else if (d_stat == 1) //check 6 seconds of preamble separation else if (d_stat == 1) //check 6 seconds of preamble separation
{ {
preamble_diff_ms = std::round(((static_cast<double>(d_symbol_history.at(0).Tracking_sample_counter) - d_preamble_time_samples) / static_cast<double>(d_symbol_history.at(0).fs)) * 1000.0); preamble_diff_ms = std::round(((static_cast<double>(d_symbol_history.at(0).Tracking_sample_counter) - d_preamble_time_samples) / static_cast<double>(d_symbol_history.at(0).fs)) * 1000.0);
if (std::abs(preamble_diff_ms - GPS_SUBFRAME_MS) < 1) if (std::abs(preamble_diff_ms - GPS_SUBFRAME_MS) % GPS_SUBFRAME_MS == 0)
{ {
DLOG(INFO) << "Preamble confirmation for SAT " << this->d_satellite; DLOG(INFO) << "Preamble confirmation for SAT " << this->d_satellite;
d_GPS_FSM.Event_gps_word_preamble();
d_flag_preamble = true; d_flag_preamble = true;
d_make_correlation = false;
d_symbol_counter_corr = 0;
d_preamble_time_samples = d_symbol_history.at(0).Tracking_sample_counter; // record the PRN start sample index associated to the preamble d_preamble_time_samples = d_symbol_history.at(0).Tracking_sample_counter; // record the PRN start sample index associated to the preamble
if (!d_flag_frame_sync) if (!d_flag_frame_sync)
{ {
@ -269,131 +381,46 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at " DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at "
<< static_cast<double>(d_preamble_time_samples) / static_cast<double>(d_symbol_history.at(0).fs) << " [s]"; << static_cast<double>(d_preamble_time_samples) / static_cast<double>(d_symbol_history.at(0).fs) << " [s]";
} }
}
} //try to decode the subframe:
} if (decode_subframe() == false)
else
{ {
d_symbol_counter_corr++; d_crc_error_synchronization_counter++;
if (d_symbol_counter_corr > (GPS_SUBFRAME_MS - GPS_CA_TELEMETRY_SYMBOLS_PER_BIT)) if (d_crc_error_synchronization_counter > 3)
{ {
d_make_correlation = true; DLOG(INFO) << "TOO MANY CRC ERRORS: Lost of frame sync SAT " << this->d_satellite << std::endl;
}
if (d_stat == 1)
{
preamble_diff_ms = round(((static_cast<double>(d_symbol_history.at(0).Tracking_sample_counter) - static_cast<double>(d_preamble_time_samples)) / static_cast<double>(d_symbol_history.at(0).fs)) * 1000.0);
if (preamble_diff_ms > GPS_SUBFRAME_MS + 1)
{
DLOG(INFO) << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff_ms;
d_stat = 0; //lost of frame sync d_stat = 0; //lost of frame sync
d_flag_frame_sync = false; d_flag_frame_sync = false;
flag_TOW_set = false; flag_TOW_set = false;
d_make_correlation = true; d_crc_error_synchronization_counter = 0;
d_symbol_counter_corr = 0;
} }
} }
d_current_subframe_symbol = 0;
} }
//******* SYMBOL TO BIT *******
if (d_symbol_history.at(0).Flag_valid_symbol_output == true)
{
// extended correlation to bit period is enabled in tracking!
d_symbol_accumulator += d_symbol_history.at(0).Prompt_I; // accumulate the input value in d_symbol_accumulator
d_symbol_accumulator_counter += d_symbol_history.at(0).correlation_length_ms;
} }
if (d_symbol_accumulator_counter >= 20)
{
if (d_symbol_accumulator > 0)
{ //symbol to bit
d_GPS_frame_4bytes += 1; //insert the telemetry bit in LSB
}
d_symbol_accumulator = 0;
d_symbol_accumulator_counter = 0;
//******* bits to words ******
d_frame_bit_index++;
if (d_frame_bit_index == 30)
{
d_frame_bit_index = 0;
// parity check
// Each word in wordbuff is composed of:
// Bits 0 to 29 = the GPS data word
// Bits 30 to 31 = 2 LSBs of the GPS word ahead.
// prepare the extended frame [-2 -1 0 ... 30]
if (d_prev_GPS_frame_4bytes & 0x00000001)
{
d_GPS_frame_4bytes = d_GPS_frame_4bytes | 0x40000000;
}
if (d_prev_GPS_frame_4bytes & 0x00000002)
{
d_GPS_frame_4bytes = d_GPS_frame_4bytes | 0x80000000;
}
/* Check that the 2 most recently logged words pass parity. Have to first
invert the data bits according to bit 30 of the previous word. */
if (d_GPS_frame_4bytes & 0x40000000)
{
d_GPS_frame_4bytes ^= 0x3FFFFFC0; // invert the data bits (using XOR)
}
if (gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(d_GPS_frame_4bytes))
{
memcpy(&d_GPS_FSM.d_GPS_frame_4bytes, &d_GPS_frame_4bytes, sizeof(char) * 4);
//d_GPS_FSM.d_preamble_time_ms = d_preamble_time_seconds * 1000.0;
d_GPS_FSM.Event_gps_word_valid();
// send TLM data to PVT using asynchronous message queues
if (d_GPS_FSM.d_flag_new_subframe == true)
{
switch (d_GPS_FSM.d_subframe_ID)
{
case 3: //we have a new set of ephemeris data for the current SV
if (d_GPS_FSM.d_nav.satellite_validation() == true)
{
// get ephemeris object for this SV (mandatory)
std::shared_ptr<Gps_Ephemeris> tmp_obj = std::make_shared<Gps_Ephemeris>(d_GPS_FSM.d_nav.get_ephemeris());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
break;
case 4: // Possible IONOSPHERE and UTC model update (page 18)
if (d_GPS_FSM.d_nav.flag_iono_valid == true)
{
std::shared_ptr<Gps_Iono> tmp_obj = std::make_shared<Gps_Iono>(d_GPS_FSM.d_nav.get_iono());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_GPS_FSM.d_nav.flag_utc_model_valid == true)
{
std::shared_ptr<Gps_Utc_Model> tmp_obj = std::make_shared<Gps_Utc_Model>(d_GPS_FSM.d_nav.get_utc_model());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
break;
case 5:
// get almanac (if available)
//TODO: implement almanac reader in navigation_message
break;
default:
break;
}
d_GPS_FSM.clear_flag_new_subframe();
d_flag_new_tow_available = true;
}
d_flag_parity = true;
} }
else else
{ {
d_GPS_FSM.Event_gps_word_invalid(); if (d_stat == 1)
d_flag_parity = false;
}
d_prev_GPS_frame_4bytes = d_GPS_frame_4bytes; // save the actual frame
d_GPS_frame_4bytes = d_GPS_frame_4bytes & 0;
}
else
{ {
d_GPS_frame_4bytes <<= 1; //shift 1 bit left the telemetry word preamble_diff_ms = round(((static_cast<double>(d_symbol_history.at(0).Tracking_sample_counter) - static_cast<double>(d_preamble_time_samples)) / static_cast<double>(d_symbol_history.at(0).fs)) * 1000.0);
if (preamble_diff_ms > GPS_SUBFRAME_MS)
{
DLOG(INFO) << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff_ms;
// std::cout << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff_ms << std::endl;
d_stat = 0; //lost of frame sync
d_flag_frame_sync = false;
flag_TOW_set = false;
d_current_subframe_symbol = 0;
d_crc_error_synchronization_counter = 0;
}
} }
} }
//2. Add the telemetry decoder information //2. Add the telemetry decoder information
if (this->d_flag_preamble == true and d_flag_new_tow_available == true) if (this->d_flag_preamble == true and d_flag_new_tow_available == true)
{ {
d_TOW_at_current_symbol_ms = static_cast<unsigned int>(d_GPS_FSM.d_nav.d_TOW) * 1000 + GPS_L1_CA_CODE_PERIOD_MS + GPS_CA_PREAMBLE_DURATION_MS; d_TOW_at_current_symbol_ms = static_cast<unsigned int>(d_nav.d_TOW) * 1000 + GPS_L1_CA_CODE_PERIOD_MS + GPS_CA_PREAMBLE_DURATION_MS;
d_TOW_at_Preamble_ms = d_TOW_at_current_symbol_ms; d_TOW_at_Preamble_ms = d_TOW_at_current_symbol_ms;
flag_TOW_set = true; flag_TOW_set = true;
d_flag_new_tow_available = false; d_flag_new_tow_available = false;

24
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h
View File

@ -72,7 +72,9 @@ private:
bool gps_word_parityCheck(unsigned int gpsword); bool gps_word_parityCheck(unsigned int gpsword);
// class private vars bool decode_subframe();
bool new_decoder();
int d_crc_error_synchronization_counter;
int *d_preambles_symbols; int *d_preambles_symbols;
unsigned int d_stat; unsigned int d_stat;
@ -80,26 +82,22 @@ private:
// symbols // symbols
boost::circular_buffer<Gnss_Synchro> d_symbol_history; boost::circular_buffer<Gnss_Synchro> d_symbol_history;
float d_subframe_symbols[GPS_SUBFRAME_MS]; //symbols per subframe
double d_symbol_accumulator; int d_current_subframe_symbol;
short int d_symbol_accumulator_counter; //double d_symbol_accumulator;
//short int d_symbol_accumulator_counter;
// symbol counting
bool d_make_correlation;
unsigned int d_symbol_counter_corr;
//bits and frame //bits and frame
unsigned short int d_frame_bit_index; //unsigned short int d_frame_bit_index;
unsigned int d_GPS_frame_4bytes; //unsigned int d_GPS_frame_4bytes;
unsigned int d_prev_GPS_frame_4bytes; unsigned int d_prev_GPS_frame_4bytes;
bool d_flag_parity; //bool d_flag_parity;
bool d_flag_preamble; bool d_flag_preamble;
bool d_flag_new_tow_available; bool d_flag_new_tow_available;
int d_word_number; //int d_word_number;
// navigation message vars // navigation message vars
Gps_Navigation_Message d_nav; Gps_Navigation_Message d_nav;
GpsL1CaSubframeFsm d_GPS_FSM;
bool d_dump; bool d_dump;
Gnss_Satellite d_satellite; Gnss_Satellite d_satellite;

2
src/core/receiver/gnss_flowgraph.cc
View File

@ -418,7 +418,7 @@ void GNSSFlowgraph::connect()
} }
if (sat == 0) if (sat == 0)
{ {
channels_.at(i)->set_signal(search_next_signal(gnss_signal, false)); //channels_.at(i)->set_signal(search_next_signal(gnss_signal, false));
} }
else else
{ {