mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-14 04:00:34 +00:00
Code Issues Releases Wiki Activity

Add work on AN printer

Browse Source
This commit is contained in:
Carles Fernandez 2021-09-30 15:01:58 +02:00
parent 387d9ad1b1
commit d01f4cb86b
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
3 changed files with 112 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc
View File

@ -2253,7 +2253,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
{ {
if (current_RX_time_ms % d_an_rate_ms == 0) if (current_RX_time_ms % d_an_rate_ms == 0)
{ {
d_an_printer->print_packet(d_user_pvt_solver.get()); d_an_printer->print_packet(d_user_pvt_solver.get(), d_gnss_observables_map);
} }
} }
} }

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

@ -54,13 +54,13 @@ An_Packet_Printer::~An_Packet_Printer()
} }
bool An_Packet_Printer::print_packet(const Rtklib_Solver* const pvt_data) bool An_Packet_Printer::print_packet(const Rtklib_Solver* const pvt_data, const std::map<int, Gnss_Synchro>& gnss_observables_map)
{ {
an_packet_t an_packet{}; an_packet_t an_packet{};
raw_gnss_packet_t raw_packet{}; sdr_gnss_packet_t sdr_gnss_packet{};
update_raw_gnss_packet(&raw_packet, pvt_data); update_sdr_gnss_packet(&sdr_gnss_packet, pvt_data, gnss_observables_map);
encode_raw_gnss_packet(&raw_packet, &an_packet); encode_sdr_gnss_packet(&sdr_gnss_packet, &an_packet);
if (d_an_dev_descriptor != -1) if (d_an_dev_descriptor != -1)
{ {
@ -83,84 +83,112 @@ void An_Packet_Printer::close_serial() const
} }
/* /*
* @brief update_raw_gnss_packet * @brief update_sdr_gnss_packet
* @param raw_gnss_packet_t* Pointer to a structure that contains * @param sdr_gnss_packet_t* Pointer to a structure that contains
* the output information. * the output information.
* @param NavData_t* pointer to input packet with all the information * @param NavData_t* pointer to input packet with all the information
* @reval None * @reval None
*/ */
void An_Packet_Printer::update_raw_gnss_packet(raw_gnss_packet_t* _packet, const Rtklib_Solver* const pvt) const void An_Packet_Printer::update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const Rtklib_Solver* const pvt, const std::map<int, Gnss_Synchro>& gnss_observables_map) const
{ {
const boost::posix_time::ptime time_origin(boost::gregorian::date(1970, 1, 1)); const boost::posix_time::ptime time_origin(boost::gregorian::date(1970, 1, 1));
boost::date_time::time_duration unix_t = pvt->get_position_UTC_time() - time_origin; boost::date_time::time_duration unix_t = pvt->get_position_UTC_time() - time_origin;
_packet->unix_time_stamp = unix_t.total_seconds(); std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter;
_packet->microseconds = unix_t.total_microseconds() - unix_t.total_seconds() * 1e6; uint8_t num_gps_sats = 0;
_packet->latitude = static_cast<double>(pvt->get_latitude()) / 1.0e-7 * (M_PI / 180.0); uint8_t num_gal_sats = 0;
_packet->longitude = static_cast<double>(pvt->get_latitude()) / 1.0e-7 * (M_PI / 180.0); int index = 0;
_packet->height = static_cast<double>(pvt->get_height()) / 100.0;
_packet->velocity[0] = static_cast<float>(pvt->pvt_sol.rr[3]) / 100.0;
_packet->velocity[1] = static_cast<float>(pvt->pvt_sol.rr[4]) / 100.0;
_packet->velocity[2] = static_cast<float>(pvt->pvt_sol.rr[5]) / 100.0;
_packet->position_standard_deviation[0] = std::sqrt(static_cast<float>(pvt->pvt_sol.qr[0]));
_packet->position_standard_deviation[1] = std::sqrt(static_cast<float>(pvt->pvt_sol.qr[1]));
_packet->position_standard_deviation[2] = std::sqrt(static_cast<float>(pvt->pvt_sol.qr[2]));
_packet->status.b.fix_type = 2; //!< 2: 3D fix for (gnss_observables_iter = gnss_observables_map.cbegin();
_packet->status.b.velocity_valid = 0; gnss_observables_iter != gnss_observables_map.cend();
_packet->status.b.time_valid = 0; ++gnss_observables_iter)
_packet->status.b.external_gnss = 0; {
if (gnss_observables_iter->second.Flag_valid_pseudorange)
{
switch (gnss_observables_iter->second.System)
{
case 'G':
num_gps_sats++;
if (index < 6)
{
_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);
index++;
}
break;
case 'E':
num_gal_sats++;
if (index < 6)
{
_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);
index++;
}
break;
default:
break;
}
}
}
_packet->nsvfix = static_cast<uint8_t>(pvt->get_num_valid_observations());
_packet->gps_satellites = num_gps_sats;
_packet->galileo_satellites = num_gal_sats;
_packet->microseconds = static_cast<uint32_t>(unix_t.total_microseconds());
_packet->latitude = static_cast<double>(pvt->get_latitude()) * (M_PI / 180.0);
_packet->longitude = static_cast<double>(pvt->get_longitude()) * (M_PI / 180.0);
_packet->height = static_cast<double>(pvt->get_height());
_packet->velocity[0] = static_cast<float>(pvt->get_rx_vel()[1]);
_packet->velocity[1] = static_cast<float>(pvt->get_rx_vel()[0]);
_packet->velocity[2] = static_cast<float>(-pvt->get_rx_vel()[2]);
uint16_t status = 0;
// Clarify table
_packet->status = status;
} }
/* /*
* @brief encode_raw_gnss_packet * @brief encode_sdr_gnss_packet
* @param raw_gnss_packet_t* Pointer to a structure that contains * @param sdr_gnss_packet_t* Pointer to a structure that contains the data.
* the information.
* @param an_packet_t* pointer to output packet * @param an_packet_t* pointer to output packet
* @reval None * @reval None
*/ */
void An_Packet_Printer::encode_raw_gnss_packet(raw_gnss_packet_t* raw_packet, an_packet_t* _packet) const void An_Packet_Printer::encode_sdr_gnss_packet(sdr_gnss_packet_t* sdr_gnss_packet, an_packet_t* _packet) const
{ {
_packet->id = 201; _packet->id = SDR_GNSS_PACKET_ID;
_packet->length = RAW_GNSS_PACKET_LENGTH; _packet->length = SDR_GNSS_PACKET_LENGTH;
if (_packet != NULL) if (_packet != nullptr)
{ {
uint8_t offset = 0; uint8_t offset = 0;
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->unix_time_stamp), offset, _packet->data, sizeof(raw_packet->unix_time_stamp)); LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->nsvfix), offset, _packet->data, sizeof(sdr_gnss_packet->nsvfix));
offset += 1;
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->gps_satellites), offset, _packet->data, sizeof(sdr_gnss_packet->gps_satellites));
offset += 1;
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->galileo_satellites), offset, _packet->data, sizeof(sdr_gnss_packet->galileo_satellites));
offset += 1;
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->microseconds), offset, _packet->data, sizeof(sdr_gnss_packet->microseconds));
offset += 4; offset += 4;
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->microseconds), offset, _packet->data, sizeof(raw_packet->microseconds)); LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->latitude), offset, _packet->data, sizeof(sdr_gnss_packet->latitude));
offset += 4;
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->latitude), offset, _packet->data, sizeof(raw_packet->latitude));
offset += 8; offset += 8;
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->longitude), offset, _packet->data, sizeof(raw_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 += 8;
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->height), offset, _packet->data, sizeof(raw_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 += 8;
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->velocity[0]), offset, _packet->data, sizeof(raw_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 += 4;
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->velocity[1]), offset, _packet->data, sizeof(raw_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 += 4;
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->velocity[2]), offset, _packet->data, sizeof(raw_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 += 4;
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->position_standard_deviation[0]), offset, _packet->data, sizeof(raw_packet->position_standard_deviation[0])); for (int i = 0; i < 6; i++)
offset += 4; {
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->position_standard_deviation[1]), offset, _packet->data, sizeof(raw_packet->position_standard_deviation[1])); LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->sats[i].prn), offset, _packet->data, sizeof(sdr_gnss_packet->sats[i].prn));
offset += 4; offset += 1;
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->position_standard_deviation[2]), offset, _packet->data, sizeof(raw_packet->position_standard_deviation[2])); LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->sats[i].snr), offset, _packet->data, sizeof(sdr_gnss_packet->sats[i].snr));
offset += 4; offset += 1;
// This could be optimized to add just zeros }
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->reserved[0]), offset, _packet->data, sizeof(raw_packet->reserved[0])); offset += 16;
offset += 4; 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*>(&raw_packet->reserved[1]), offset, _packet->data, sizeof(raw_packet->reserved[1]));
offset += 4;
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->reserved[2]), offset, _packet->data, sizeof(raw_packet->reserved[2]));
offset += 4;
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->reserved[3]), offset, _packet->data, sizeof(raw_packet->reserved[3]));
offset += 4;
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->status.r), offset, _packet->data, sizeof(raw_packet->status));
offset += 2;
} }
an_packet_encode(_packet); an_packet_encode(_packet);
} }
@ -254,7 +282,7 @@ uint16_t An_Packet_Printer::calculate_crc16(const void* data, uint16_t length) c
/* /*
* Opens the serial device and sets the default baud rate for a NMEA transmission (9600,8,N,1) * Opens the serial device and sets the default baud rate for a transmission (115200,8,N,1)
*/ */
int An_Packet_Printer::init_serial(const std::string& serial_device) int An_Packet_Printer::init_serial(const std::string& serial_device)
{ {

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

@ -21,8 +21,10 @@
#ifndef GNSS_SDR_AN_PACKET_PRINTER_H #ifndef GNSS_SDR_AN_PACKET_PRINTER_H
#define GNSS_SDR_AN_PACKET_PRINTER_H #define GNSS_SDR_AN_PACKET_PRINTER_H
#include "gnss_synchro.h"
#include <array> #include <array>
#include <cstdint> #include <cstdint>
#include <map>
#include <string> #include <string>
/** \addtogroup PVT /** \addtogroup PVT
@ -34,26 +36,25 @@ class Rtklib_Solver;
typedef struct typedef struct
{ {
uint32_t unix_time_stamp; uint8_t nsvfix; // number of sats used in PVT fix
uint32_t microseconds; uint8_t gps_satellites; // number of tracked GPS satellites
double latitude; uint8_t galileo_satellites; // number of tracked Galileo satellites
double longitude; uint32_t microseconds; // ??
double height; double latitude; // in [rad]
float velocity[3]; double longitude; // in [rad]
float position_standard_deviation[3]; double height; // in [m]
float reserved[4]; float velocity[3]; // North, East, Down, in [m/s]
union
{
uint16_t r;
struct struct
{ {
uint32_t fix_type : 3; uint8_t prn; // Galileo sats expressed as PRN + 100
uint32_t velocity_valid : 1; uint8_t snr; // in [dB-Hz]
uint32_t time_valid : 1; } sats[6];
uint32_t external_gnss : 1;
} b; float reserved[4];
} status; uint16_t status;
} raw_gnss_packet_t;
} sdr_gnss_packet_t;
typedef struct typedef struct
@ -85,7 +86,7 @@ public:
/*! /*!
* \brief Print AN packet to the initialized device. * \brief Print AN packet to the initialized device.
*/ */
bool print_packet(const Rtklib_Solver* const pvt_data); bool print_packet(const Rtklib_Solver* const pvt_data, const std::map<int, Gnss_Synchro>& gnss_observables_map);
/*! /*!
* \brief Close serial port. Also done in the destructor, this is only * \brief Close serial port. Also done in the destructor, this is only
@ -108,15 +109,16 @@ private:
0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92, 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9, 0x7c26, 0x6c07, 0x5c64, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92, 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9, 0x7c26, 0x6c07, 0x5c64,
0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1, 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0}; 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1, 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0};
const size_t RAW_GNSS_PACKET_LENGTH = sizeof(raw_gnss_packet_t{}); const size_t SDR_GNSS_PACKET_LENGTH = 73;
const uint8_t SDR_GNSS_PACKET_ID = 201;
int init_serial(const std::string& serial_device); int init_serial(const std::string& serial_device);
void update_raw_gnss_packet(raw_gnss_packet_t* _packet, const Rtklib_Solver* const pvt) const; void update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const Rtklib_Solver* const pvt, const std::map<int, Gnss_Synchro>& gnss_observables_map) const;
void encode_gnss_cttc_packet(raw_gnss_packet_t* raw_packet, an_packet_t* _packet) const; void encode_gnss_cttc_packet(sdr_gnss_packet_t* sdr_gnss_packet, an_packet_t* _packet) const;
uint16_t calculate_crc16(const void* data, uint16_t length) const; uint16_t calculate_crc16(const void* data, uint16_t length) const;
uint8_t calculate_header_lrc(const uint8_t* data) const; uint8_t calculate_header_lrc(const uint8_t* data) const;
void an_packet_encode(an_packet_t* an_packet) const; void an_packet_encode(an_packet_t* an_packet) const;
void encode_raw_gnss_packet(raw_gnss_packet_t* raw_packet, an_packet_t* _packet) const; void encode_sdr_gnss_packet(sdr_gnss_packet_t* sdr_gnss_packet, an_packet_t* _packet) const;
void LSB_bytes_to_array(void* _in, int offset, uint8_t* _out, uint8_t var_size) const; void LSB_bytes_to_array(void* _in, int offset, uint8_t* _out, uint8_t var_size) const;
std::string d_an_devname; std::string d_an_devname;