mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-14 04:00:34 +00:00
Add work on AN printer
This commit is contained in:
Carles Fernandez
parent
387d9ad1b1
commit
d01f4cb86b
@ -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)
|
||||
{
|
||||
d_an_printer->print_packet(d_user_pvt_solver.get());
|
||||
d_an_printer->print_packet(d_user_pvt_solver.get(), d_gnss_observables_map);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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{};
|
||||
raw_gnss_packet_t raw_packet{};
|
||||
sdr_gnss_packet_t sdr_gnss_packet{};
|
||||
|
||||
update_raw_gnss_packet(&raw_packet, pvt_data);
|
||||
encode_raw_gnss_packet(&raw_packet, &an_packet);
|
||||
update_sdr_gnss_packet(&sdr_gnss_packet, pvt_data, gnss_observables_map);
|
||||
encode_sdr_gnss_packet(&sdr_gnss_packet, &an_packet);
|
||||
|
||||
if (d_an_dev_descriptor != -1)
|
||||
{
|
||||
@ -83,84 +83,112 @@ void An_Packet_Printer::close_serial() const
|
||||
}
|
||||
|
||||
/*
|
||||
* @brief update_raw_gnss_packet
|
||||
* @param raw_gnss_packet_t* Pointer to a structure that contains
|
||||
* @brief update_sdr_gnss_packet
|
||||
* @param sdr_gnss_packet_t* Pointer to a structure that contains
|
||||
* the output information.
|
||||
* @param NavData_t* pointer to input packet with all the information
|
||||
* @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));
|
||||
boost::date_time::time_duration unix_t = pvt->get_position_UTC_time() - time_origin;
|
||||
|
||||
_packet->unix_time_stamp = unix_t.total_seconds();
|
||||
_packet->microseconds = unix_t.total_microseconds() - unix_t.total_seconds() * 1e6;
|
||||
_packet->latitude = static_cast<double>(pvt->get_latitude()) / 1.0e-7 * (M_PI / 180.0);
|
||||
_packet->longitude = static_cast<double>(pvt->get_latitude()) / 1.0e-7 * (M_PI / 180.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]));
|
||||
std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter;
|
||||
uint8_t num_gps_sats = 0;
|
||||
uint8_t num_gal_sats = 0;
|
||||
int index = 0;
|
||||
|
||||
_packet->status.b.fix_type = 2; //!< 2: 3D fix
|
||||
_packet->status.b.velocity_valid = 0;
|
||||
_packet->status.b.time_valid = 0;
|
||||
_packet->status.b.external_gnss = 0;
|
||||
for (gnss_observables_iter = gnss_observables_map.cbegin();
|
||||
gnss_observables_iter != gnss_observables_map.cend();
|
||||
++gnss_observables_iter)
|
||||
{
|
||||
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
|
||||
* @param raw_gnss_packet_t* Pointer to a structure that contains
|
||||
* the information.
|
||||
* @brief encode_sdr_gnss_packet
|
||||
* @param sdr_gnss_packet_t* Pointer to a structure that contains the data.
|
||||
* @param an_packet_t* pointer to output packet
|
||||
* @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->length = RAW_GNSS_PACKET_LENGTH;
|
||||
if (_packet != NULL)
|
||||
_packet->id = SDR_GNSS_PACKET_ID;
|
||||
_packet->length = SDR_GNSS_PACKET_LENGTH;
|
||||
if (_packet != nullptr)
|
||||
{
|
||||
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;
|
||||
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->microseconds), offset, _packet->data, sizeof(raw_packet->microseconds));
|
||||
offset += 4;
|
||||
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->latitude), offset, _packet->data, sizeof(raw_packet->latitude));
|
||||
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->latitude), offset, _packet->data, sizeof(sdr_gnss_packet->latitude));
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->position_standard_deviation[0]), offset, _packet->data, sizeof(raw_packet->position_standard_deviation[0]));
|
||||
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]));
|
||||
offset += 4;
|
||||
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->position_standard_deviation[2]), offset, _packet->data, sizeof(raw_packet->position_standard_deviation[2]));
|
||||
offset += 4;
|
||||
// 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 += 4;
|
||||
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;
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
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 += 1;
|
||||
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 += 1;
|
||||
}
|
||||
offset += 16;
|
||||
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&sdr_gnss_packet->status), offset, _packet->data, sizeof(sdr_gnss_packet->status));
|
||||
}
|
||||
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)
|
||||
{
|
||||
|
||||
@ -21,8 +21,10 @@
|
||||
#ifndef GNSS_SDR_AN_PACKET_PRINTER_H
|
||||
#define GNSS_SDR_AN_PACKET_PRINTER_H
|
||||
|
||||
#include "gnss_synchro.h"
|
||||
#include <array>
|
||||
#include <cstdint>
|
||||
#include <map>
|
||||
#include <string>
|
||||
|
||||
/** \addtogroup PVT
|
||||
@ -34,26 +36,25 @@ class Rtklib_Solver;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint32_t unix_time_stamp;
|
||||
uint32_t microseconds;
|
||||
double latitude;
|
||||
double longitude;
|
||||
double height;
|
||||
float velocity[3];
|
||||
float position_standard_deviation[3];
|
||||
float reserved[4];
|
||||
union
|
||||
{
|
||||
uint16_t r;
|
||||
uint8_t nsvfix; // number of sats used in PVT fix
|
||||
uint8_t gps_satellites; // number of tracked GPS satellites
|
||||
uint8_t galileo_satellites; // number of tracked Galileo satellites
|
||||
uint32_t microseconds; // ??
|
||||
double latitude; // in [rad]
|
||||
double longitude; // in [rad]
|
||||
double height; // in [m]
|
||||
float velocity[3]; // North, East, Down, in [m/s]
|
||||
|
||||
struct
|
||||
{
|
||||
uint32_t fix_type : 3;
|
||||
uint32_t velocity_valid : 1;
|
||||
uint32_t time_valid : 1;
|
||||
uint32_t external_gnss : 1;
|
||||
} b;
|
||||
} status;
|
||||
} raw_gnss_packet_t;
|
||||
uint8_t prn; // Galileo sats expressed as PRN + 100
|
||||
uint8_t snr; // in [dB-Hz]
|
||||
} sats[6];
|
||||
|
||||
float reserved[4];
|
||||
uint16_t status;
|
||||
|
||||
} sdr_gnss_packet_t;
|
||||
|
||||
|
||||
typedef struct
|
||||
@ -85,7 +86,7 @@ public:
|
||||
/*!
|
||||
* \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
|
||||
@ -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,
|
||||
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);
|
||||
void update_raw_gnss_packet(raw_gnss_packet_t* _packet, const Rtklib_Solver* const pvt) const;
|
||||
void encode_gnss_cttc_packet(raw_gnss_packet_t* raw_packet, an_packet_t* _packet) 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(sdr_gnss_packet_t* sdr_gnss_packet, an_packet_t* _packet) const;
|
||||
uint16_t calculate_crc16(const void* data, uint16_t length) const;
|
||||
uint8_t calculate_header_lrc(const uint8_t* data) 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;
|
||||
|
||||
std::string d_an_devname;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user