mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-31 11:19:18 +00:00
Code Issues Releases Wiki Activity

Improve code robustness

Browse Source
This commit is contained in:
Carles Fernandez 2021-10-01 14:25:03 +02:00
parent 1f7eefba81
commit 8298062f0e
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
2 changed files with 33 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
src/algorithms/PVT/libs/an_packet_printer.cc
View File

@ -97,6 +97,7 @@ void An_Packet_Printer::update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const
uint8_t num_gal_sats = 0; uint8_t num_gal_sats = 0;
uint32_t microseconds = 0; uint32_t microseconds = 0;
int index = 0; int index = 0;
const int max_reported_sats = *(&_packet->sats + 1) - _packet->sats;
for (gnss_observables_iter = gnss_observables_map.cbegin(); for (gnss_observables_iter = gnss_observables_map.cbegin();
gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter != gnss_observables_map.cend();
@ -108,12 +109,12 @@ void An_Packet_Printer::update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const
{ {
case 'G': case 'G':
num_gps_sats++; num_gps_sats++;
if (index < 6) if (index < max_reported_sats)
{ {
_packet->sats[index].prn = static_cast<uint8_t>(gnss_observables_iter->second.PRN); _packet->sats[index].prn = static_cast<uint8_t>(gnss_observables_iter->second.PRN);
_packet->sats[index].snr = static_cast<uint8_t>(gnss_observables_iter->second.CN0_dB_hz); _packet->sats[index].snr = static_cast<uint8_t>(gnss_observables_iter->second.CN0_dB_hz);
int16_t doppler = 0; int16_t doppler = 0;
double Carrier_Doppler_hz = gnss_observables_iter->second.CN0_dB_hz; double Carrier_Doppler_hz = gnss_observables_iter->second.Carrier_Doppler_hz;
if (Carrier_Doppler_hz > static_cast<double>(std::numeric_limits<int16_t>::max())) if (Carrier_Doppler_hz > static_cast<double>(std::numeric_limits<int16_t>::max()))
{ {
doppler = std::numeric_limits<int16_t>::max(); doppler = std::numeric_limits<int16_t>::max();
@ -134,12 +135,12 @@ void An_Packet_Printer::update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const
break; break;
case 'E': case 'E':
num_gal_sats++; num_gal_sats++;
if (index < 6) if (index < max_reported_sats)
{ {
_packet->sats[index].prn = static_cast<uint8_t>(gnss_observables_iter->second.PRN) + 100; _packet->sats[index].prn = static_cast<uint8_t>(gnss_observables_iter->second.PRN) + 100;
_packet->sats[index].snr = static_cast<uint8_t>(gnss_observables_iter->second.CN0_dB_hz); _packet->sats[index].snr = static_cast<uint8_t>(gnss_observables_iter->second.CN0_dB_hz);
int16_t doppler = 0; int16_t doppler = 0;
double Carrier_Doppler_hz = gnss_observables_iter->second.CN0_dB_hz; double Carrier_Doppler_hz = gnss_observables_iter->second.Carrier_Doppler_hz;
if (Carrier_Doppler_hz > static_cast<double>(std::numeric_limits<int16_t>::max())) if (Carrier_Doppler_hz > static_cast<double>(std::numeric_limits<int16_t>::max()))
{ {
doppler = std::numeric_limits<int16_t>::max(); doppler = std::numeric_limits<int16_t>::max();
@ -176,12 +177,8 @@ void An_Packet_Printer::update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const
_packet->velocity[2] = static_cast<float>(-pvt->get_rx_vel()[2]); _packet->velocity[2] = static_cast<float>(-pvt->get_rx_vel()[2]);
uint16_t status = 0; uint16_t status = 0;
// Set 3D fix // Set 3D fix (bit 0 and 1) / Set Doppler velocity valid (bit 2) / Set Time valid (bit 3)
status = status & 0b00000011; // set bit 0 and 1 status = status & 0b00001111;
// Set Doppler velocity valid
status = status & 0b00000100; // set bit 2
// Set Time valid
status = status & 0b00001000; // set bit 3
_packet->status = status; _packet->status = status;
} }
@ -200,35 +197,36 @@ void An_Packet_Printer::encode_sdr_gnss_packet(sdr_gnss_packet_t* sdr_gnss_packe
{ {
uint8_t offset = 0; uint8_t offset = 0;
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->nsvfix), offset, _packet->data, sizeof(sdr_gnss_packet->nsvfix)); LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->nsvfix), offset, _packet->data, sizeof(sdr_gnss_packet->nsvfix));
offset += 1; offset += sizeof(sdr_gnss_packet->nsvfix);
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->gps_satellites), offset, _packet->data, sizeof(sdr_gnss_packet->gps_satellites)); LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->gps_satellites), offset, _packet->data, sizeof(sdr_gnss_packet->gps_satellites));
offset += 1; offset += sizeof(sdr_gnss_packet->gps_satellites);
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->galileo_satellites), offset, _packet->data, sizeof(sdr_gnss_packet->galileo_satellites)); LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->galileo_satellites), offset, _packet->data, sizeof(sdr_gnss_packet->galileo_satellites));
offset += 1; offset += sizeof(sdr_gnss_packet->galileo_satellites);
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->microseconds), offset, _packet->data, sizeof(sdr_gnss_packet->microseconds)); LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->microseconds), offset, _packet->data, sizeof(sdr_gnss_packet->microseconds));
offset += 4; offset += sizeof(sdr_gnss_packet->microseconds);
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->latitude), offset, _packet->data, sizeof(sdr_gnss_packet->latitude)); LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->latitude), offset, _packet->data, sizeof(sdr_gnss_packet->latitude));
offset += 8; offset += sizeof(sdr_gnss_packet->latitude);
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->longitude), offset, _packet->data, sizeof(sdr_gnss_packet->longitude)); LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->longitude), offset, _packet->data, sizeof(sdr_gnss_packet->longitude));
offset += 8; offset += sizeof(sdr_gnss_packet->longitude);
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->height), offset, _packet->data, sizeof(sdr_gnss_packet->height)); LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->height), offset, _packet->data, sizeof(sdr_gnss_packet->height));
offset += 8; offset += sizeof(sdr_gnss_packet->height);
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->velocity[0]), offset, _packet->data, sizeof(sdr_gnss_packet->velocity[0])); LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->velocity[0]), offset, _packet->data, sizeof(sdr_gnss_packet->velocity[0]));
offset += 4; offset += sizeof(sdr_gnss_packet->velocity[0]);
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->velocity[1]), offset, _packet->data, sizeof(sdr_gnss_packet->velocity[1])); LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->velocity[1]), offset, _packet->data, sizeof(sdr_gnss_packet->velocity[1]));
offset += 4; offset += sizeof(sdr_gnss_packet->velocity[1]);
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->velocity[2]), offset, _packet->data, sizeof(sdr_gnss_packet->velocity[2])); LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->velocity[2]), offset, _packet->data, sizeof(sdr_gnss_packet->velocity[2]));
offset += 4; offset += sizeof(sdr_gnss_packet->velocity[2]);
for (int i = 0; i < 6; i++) for (auto& sat : sdr_gnss_packet->sats)
{ {
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->sats[i].prn), offset, _packet->data, sizeof(sdr_gnss_packet->sats[i].prn)); LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sat.prn), offset, _packet->data, sizeof(sat.prn));
offset += 1; offset += sizeof(sat.prn);
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->sats[i].snr), offset, _packet->data, sizeof(sdr_gnss_packet->sats[i].snr)); LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sat.snr), offset, _packet->data, sizeof(sat.snr));
offset += 1; offset += sizeof(sat.snr);
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->sats[i].doppler), offset, _packet->data, sizeof(sdr_gnss_packet->sats[i].doppler)); LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sat.doppler), offset, _packet->data, sizeof(sat.doppler));
offset += 2; offset += sizeof(sat.doppler);
} }
offset += 4; // reserved
offset = static_cast<uint8_t>(SDR_GNSS_PACKET_LENGTH - sizeof(sdr_gnss_packet->status));
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->status), offset, _packet->data, sizeof(sdr_gnss_packet->status)); LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->status), offset, _packet->data, sizeof(sdr_gnss_packet->status));
} }
an_packet_encode(_packet); an_packet_encode(_packet);
@ -264,7 +262,7 @@ void An_Packet_Printer::LSB_bytes_to_array(void* _in, int offset, uint8_t* _out,
{ {
case 1: case 1:
{ {
uint8_t* tmp = reinterpret_cast<uint8_t*>(_in); auto* tmp = reinterpret_cast<uint8_t*>(_in);
for (int i = 0; i < var_size; i++) for (int i = 0; i < var_size; i++)
{ {
_out[offset + i] = (*tmp >> 8 * i) & 0xFF; _out[offset + i] = (*tmp >> 8 * i) & 0xFF;
@ -273,7 +271,7 @@ void An_Packet_Printer::LSB_bytes_to_array(void* _in, int offset, uint8_t* _out,
} }
case 2: case 2:
{ {
uint16_t* tmp = reinterpret_cast<uint16_t*>(_in); auto* tmp = reinterpret_cast<uint16_t*>(_in);
for (int i = 0; i < var_size; i++) for (int i = 0; i < var_size; i++)
{ {
_out[offset + i] = (*tmp >> 8 * i) & 0xFF; _out[offset + i] = (*tmp >> 8 * i) & 0xFF;
@ -282,7 +280,7 @@ void An_Packet_Printer::LSB_bytes_to_array(void* _in, int offset, uint8_t* _out,
} }
case 4: case 4:
{ {
uint32_t* tmp = reinterpret_cast<uint32_t*>(_in); auto* tmp = reinterpret_cast<uint32_t*>(_in);
for (int i = 0; i < var_size; i++) for (int i = 0; i < var_size; i++)
{ {
_out[offset + i] = (*tmp >> 8 * i) & 0xFF; _out[offset + i] = (*tmp >> 8 * i) & 0xFF;
@ -291,7 +289,7 @@ void An_Packet_Printer::LSB_bytes_to_array(void* _in, int offset, uint8_t* _out,
} }
case 8: case 8:
{ {
uint64_t* tmp = reinterpret_cast<uint64_t*>(_in); auto* tmp = reinterpret_cast<uint64_t*>(_in);
for (int i = 0; i < var_size; i++) for (int i = 0; i < var_size; i++)
{ {
_out[offset + i] = (*tmp >> 8 * i) & 0xFF; _out[offset + i] = (*tmp >> 8 * i) & 0xFF;
@ -312,7 +310,7 @@ void An_Packet_Printer::LSB_bytes_to_array(void* _in, int offset, uint8_t* _out,
*/ */
uint16_t An_Packet_Printer::calculate_crc16(const void* data, uint16_t length) const uint16_t An_Packet_Printer::calculate_crc16(const void* data, uint16_t length) const
{ {
const uint8_t* bytes = reinterpret_cast<const uint8_t*>(data); const auto* bytes = reinterpret_cast<const uint8_t*>(data);
uint16_t crc = 0xFFFF; uint16_t crc = 0xFFFF;
for (uint16_t i = 0; i < length; i++) for (uint16_t i = 0; i < length; i++)
{ {

3
src/algorithms/PVT/libs/an_packet_printer.h
View File

@ -23,6 +23,7 @@
#include "gnss_synchro.h" #include "gnss_synchro.h"
#include <array> #include <array>
#include <cstddef>
#include <cstdint> #include <cstdint>
#include <map> #include <map>
#include <string> #include <string>
@ -49,7 +50,7 @@ typedef struct
{ {
uint8_t prn; // PRN ID. Galileo sats expressed as PRN + 100 uint8_t prn; // PRN ID. Galileo sats expressed as PRN + 100
uint8_t snr; // in [dB-Hz] uint8_t snr; // in [dB-Hz]
int16_t doppler; // in [Hz], saturates at +/- 32767 Hz int16_t doppler; // in [Hz], saturates at +32767 / -32768 Hz
} sats[6]; } sats[6];
uint32_t reserved = 0; uint32_t reserved = 0;