mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-14 04:00:34 +00:00
Add Advanced Navigation Protocol printer
This commit is contained in:
Carles Fernandez
parent
e09a37ca61
commit
2ccac04003
@ -122,10 +122,35 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration,
|
||||
pvt_output_parameters.rtcm_msg_rate_ms[k] = rtcm_MT1097_rate_ms;
|
||||
}
|
||||
|
||||
// Advanced Nativation Protocol Printer settings
|
||||
pvt_output_parameters.an_output_enabled = configuration->property(role + ".an_output_enabled", pvt_output_parameters.an_output_enabled);
|
||||
pvt_output_parameters.an_dump_devname = configuration->property(role + ".an_dump_devname", default_nmea_dump_devname);
|
||||
if (pvt_output_parameters.an_output_enabled && pvt_output_parameters.flag_nmea_tty_port)
|
||||
{
|
||||
if (pvt_output_parameters.nmea_dump_devname == pvt_output_parameters.an_dump_devname)
|
||||
{
|
||||
std::cerr << "Warning: NMEA an Advanced Nativation printers set to write to the same serial port.\n"
|
||||
<< "Please make sure that PVT.an_dump_devname and PVT.an_dump_devname are different.\n"
|
||||
<< "Shutting down the NMEA serial output.\n";
|
||||
pvt_output_parameters.flag_nmea_tty_port = false;
|
||||
}
|
||||
}
|
||||
if (pvt_output_parameters.an_output_enabled && pvt_output_parameters.flag_rtcm_tty_port)
|
||||
{
|
||||
if (pvt_output_parameters.rtcm_dump_devname == pvt_output_parameters.an_dump_devname)
|
||||
{
|
||||
std::cerr << "Warning: RTCM an Advanced Nativation printers set to write to the same serial port.\n"
|
||||
<< "Please make sure that PVT.an_dump_devname and .rtcm_dump_devname are different.\n"
|
||||
<< "Shutting down the RTCM serial output.\n";
|
||||
pvt_output_parameters.flag_rtcm_tty_port = false;
|
||||
}
|
||||
}
|
||||
|
||||
pvt_output_parameters.kml_rate_ms = bc::lcm(configuration->property(role + ".kml_rate_ms", pvt_output_parameters.kml_rate_ms), pvt_output_parameters.output_rate_ms);
|
||||
pvt_output_parameters.gpx_rate_ms = bc::lcm(configuration->property(role + ".gpx_rate_ms", pvt_output_parameters.gpx_rate_ms), pvt_output_parameters.output_rate_ms);
|
||||
pvt_output_parameters.geojson_rate_ms = bc::lcm(configuration->property(role + ".geojson_rate_ms", pvt_output_parameters.geojson_rate_ms), pvt_output_parameters.output_rate_ms);
|
||||
pvt_output_parameters.nmea_rate_ms = bc::lcm(configuration->property(role + ".nmea_rate_ms", pvt_output_parameters.nmea_rate_ms), pvt_output_parameters.output_rate_ms);
|
||||
pvt_output_parameters.an_rate_ms = bc::lcm(configuration->property(role + ".an_rate_ms", pvt_output_parameters.an_rate_ms), pvt_output_parameters.output_rate_ms);
|
||||
|
||||
// Infer the type of receiver
|
||||
/*
|
||||
|
||||
@ -16,6 +16,7 @@
|
||||
|
||||
#include "rtklib_pvt_gs.h"
|
||||
#include "MATH_CONSTANTS.h"
|
||||
#include "an_packet_printer.h"
|
||||
#include "beidou_dnav_almanac.h"
|
||||
#include "beidou_dnav_ephemeris.h"
|
||||
#include "beidou_dnav_iono.h"
|
||||
@ -411,6 +412,18 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels,
|
||||
d_rx_time = 0.0;
|
||||
d_last_status_print_seg = 0;
|
||||
|
||||
// Initialize AN printer
|
||||
d_an_printer_enabled = conf_.an_output_enabled;
|
||||
d_an_rate_ms = conf_.an_rate_ms;
|
||||
if (d_an_printer_enabled)
|
||||
{
|
||||
d_an_printer = std::make_unique<An_Packet_Printer>(conf_.an_dump_devname);
|
||||
}
|
||||
else
|
||||
{
|
||||
d_an_printer = nullptr;
|
||||
}
|
||||
|
||||
// PVT MONITOR
|
||||
d_flag_monitor_pvt_enabled = conf_.monitor_enabled;
|
||||
if (d_flag_monitor_pvt_enabled)
|
||||
@ -2236,6 +2249,13 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
|
||||
flag_write_RTCM_1045_output,
|
||||
d_enable_rx_clock_correction);
|
||||
}
|
||||
if (d_an_printer_enabled)
|
||||
{
|
||||
if (current_RX_time_ms % d_an_rate_ms == 0)
|
||||
{
|
||||
d_an_printer->print_packet(d_user_pvt_solver.get());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -57,6 +57,7 @@ class Nmea_Printer;
|
||||
class Pvt_Conf;
|
||||
class Rinex_Printer;
|
||||
class Rtcm_Printer;
|
||||
class An_Packet_Printer;
|
||||
class Has_Simple_Printer;
|
||||
class Rtklib_Solver;
|
||||
class rtklib_pvt_gs;
|
||||
@ -174,6 +175,7 @@ private:
|
||||
std::unique_ptr<Monitor_Pvt_Udp_Sink> d_udp_sink_ptr;
|
||||
std::unique_ptr<Monitor_Ephemeris_Udp_Sink> d_eph_udp_sink_ptr;
|
||||
std::unique_ptr<Has_Simple_Printer> d_has_simple_printer;
|
||||
std::unique_ptr<An_Packet_Printer> d_an_printer;
|
||||
|
||||
std::chrono::time_point<std::chrono::system_clock> d_start;
|
||||
std::chrono::time_point<std::chrono::system_clock> d_end;
|
||||
@ -247,6 +249,7 @@ private:
|
||||
int32_t d_gpx_rate_ms;
|
||||
int32_t d_geojson_rate_ms;
|
||||
int32_t d_nmea_rate_ms;
|
||||
int32_t d_an_rate_ms;
|
||||
int32_t d_last_status_print_seg; // for status printer
|
||||
int32_t d_output_rate_ms;
|
||||
int32_t d_display_rate_ms;
|
||||
@ -273,6 +276,7 @@ private:
|
||||
bool d_waiting_obs_block_rx_clock_offset_correction_msg;
|
||||
bool d_enable_rx_clock_correction;
|
||||
bool d_enable_has_messages;
|
||||
bool d_an_printer_enabled;
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -9,6 +9,7 @@ protobuf_generate_cpp(PROTO_SRCS2 PROTO_HDRS2 ${CMAKE_SOURCE_DIR}/docs/protobuf/
|
||||
protobuf_generate_cpp(PROTO_SRCS3 PROTO_HDRS3 ${CMAKE_SOURCE_DIR}/docs/protobuf/galileo_ephemeris.proto)
|
||||
|
||||
set(PVT_LIB_SOURCES
|
||||
an_packet_printer.cc
|
||||
pvt_solution.cc
|
||||
geojson_printer.cc
|
||||
gpx_printer.cc
|
||||
@ -24,6 +25,7 @@ set(PVT_LIB_SOURCES
|
||||
)
|
||||
|
||||
set(PVT_LIB_HEADERS
|
||||
an_packet_printer.h
|
||||
pvt_conf.h
|
||||
pvt_solution.h
|
||||
geojson_printer.h
|
||||
|
||||
291
src/algorithms/PVT/libs/an_packet_printer.cc
Normal file
291
src/algorithms/PVT/libs/an_packet_printer.cc
Normal file
@ -0,0 +1,291 @@
|
||||
/*!
|
||||
* \file an_packet_printer.cc
|
||||
* \brief Implementation of a class that prints PVT solutions in a serial device
|
||||
* following a custom version of the Advanced Navigation Packet Protocol
|
||||
* \author Carles Fernandez-Prades, 2021. cfernandez(at)cttc.es
|
||||
* \author Miguel Angel Gomez Lopez, 2021. gomezlma(at)inta.es
|
||||
*
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2021 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
|
||||
#include "an_packet_printer.h"
|
||||
#include "rtklib_solver.h" // for Rtklib_Solver
|
||||
#include <boost/date_time/posix_time/posix_time.hpp>
|
||||
#include <glog/logging.h> // for DLOG
|
||||
#include <cmath> // for std::sqrt, M_PI
|
||||
#include <fcntl.h> // for fcntl
|
||||
#include <iostream> // for std::cerr
|
||||
#include <termios.h> // values for termios
|
||||
#include <unistd.h> // for write(), read(), close()
|
||||
|
||||
|
||||
An_Packet_Printer::An_Packet_Printer(const std::string& an_dump_devname)
|
||||
{
|
||||
d_an_devname = an_dump_devname;
|
||||
|
||||
d_an_dev_descriptor = init_serial(d_an_devname);
|
||||
if (d_an_dev_descriptor != -1)
|
||||
{
|
||||
DLOG(INFO) << "AN Printer writing on " << d_an_devname;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
An_Packet_Printer::~An_Packet_Printer()
|
||||
{
|
||||
try
|
||||
{
|
||||
close_serial();
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
{
|
||||
std::cerr << e.what() << '\n';
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool An_Packet_Printer::print_packet(const Rtklib_Solver* const pvt_data)
|
||||
{
|
||||
an_packet_t an_packet{};
|
||||
raw_gnss_packet_t raw_packet{};
|
||||
|
||||
update_raw_gnss_packet(&raw_packet, pvt_data);
|
||||
encode_raw_gnss_packet(&raw_packet, &an_packet);
|
||||
|
||||
if (d_an_dev_descriptor != -1)
|
||||
{
|
||||
if (write(d_an_dev_descriptor, &an_packet, sizeof(an_packet)) == -1)
|
||||
{
|
||||
LOG(ERROR) << "Advanced Navigation printer cannot write on serial device " << d_an_devname;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
void An_Packet_Printer::close_serial() const
|
||||
{
|
||||
if (d_an_dev_descriptor != -1)
|
||||
{
|
||||
close(d_an_dev_descriptor);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* @brief update_raw_gnss_packet
|
||||
* @param raw_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
|
||||
{
|
||||
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]));
|
||||
|
||||
_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;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* @brief encode_raw_gnss_packet
|
||||
* @param raw_gnss_packet_t* Pointer to a structure that contains
|
||||
* the information.
|
||||
* @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
|
||||
{
|
||||
_packet->id = 201;
|
||||
_packet->length = RAW_GNSS_PACKET_LENGTH;
|
||||
if (_packet != NULL)
|
||||
{
|
||||
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));
|
||||
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));
|
||||
offset += 8;
|
||||
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->longitude), offset, _packet->data, sizeof(raw_packet->longitude));
|
||||
offset += 8;
|
||||
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->height), offset, _packet->data, sizeof(raw_packet->height));
|
||||
offset += 8;
|
||||
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->velocity[0]), offset, _packet->data, sizeof(raw_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]));
|
||||
offset += 4;
|
||||
LSB_bytes_to_array(reinterpret_cast<uint8_t*>(&raw_packet->velocity[2]), offset, _packet->data, sizeof(raw_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;
|
||||
}
|
||||
an_packet_encode(_packet);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Function to encode an an_packet
|
||||
*/
|
||||
void An_Packet_Printer::an_packet_encode(an_packet_t* an_packet) const
|
||||
{
|
||||
uint16_t crc;
|
||||
an_packet->header[1] = an_packet->id;
|
||||
an_packet->header[2] = an_packet->length;
|
||||
crc = calculate_crc16(an_packet->data, an_packet->length);
|
||||
memcpy(&an_packet->header[3], &crc, sizeof(uint16_t));
|
||||
an_packet->header[0] = calculate_header_lrc(&an_packet->header[1]);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Function to calculate a 4 byte LRC
|
||||
*/
|
||||
uint8_t An_Packet_Printer::calculate_header_lrc(const uint8_t* data) const
|
||||
{
|
||||
return ((data[0] + data[1] + data[2] + data[3]) ^ 0xFF) + 1;
|
||||
}
|
||||
|
||||
|
||||
void An_Packet_Printer::LSB_bytes_to_array(void* _in, int offset, uint8_t* _out, uint8_t var_size) const
|
||||
{
|
||||
switch (var_size)
|
||||
{
|
||||
case 1:
|
||||
{
|
||||
uint8_t* tmp = reinterpret_cast<uint8_t*>(_in);
|
||||
for (int i = 0; i < var_size; i++)
|
||||
{
|
||||
_out[offset + i] = (*tmp >> 8 * i) & 0xFF;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
{
|
||||
uint16_t* tmp = reinterpret_cast<uint16_t*>(_in);
|
||||
for (int i = 0; i < var_size; i++)
|
||||
{
|
||||
_out[offset + i] = (*tmp >> 8 * i) & 0xFF;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 4:
|
||||
{
|
||||
uint32_t* tmp = reinterpret_cast<uint32_t*>(_in);
|
||||
for (int i = 0; i < var_size; i++)
|
||||
{
|
||||
_out[offset + i] = (*tmp >> 8 * i) & 0xFF;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 8:
|
||||
{
|
||||
uint64_t* tmp = reinterpret_cast<uint64_t*>(_in);
|
||||
for (int i = 0; i < var_size; i++)
|
||||
{
|
||||
_out[offset + i] = (*tmp >> 8 * i) & 0xFF;
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Function to calculate the CRC16 of data
|
||||
* CRC16-CCITT
|
||||
* Initial value = 0xFFFF
|
||||
* Polynomial = x^16 + x^12 + x^5 + x^0
|
||||
*/
|
||||
uint16_t An_Packet_Printer::calculate_crc16(const void* data, uint16_t length) const
|
||||
{
|
||||
const uint8_t* bytes = reinterpret_cast<const uint8_t*>(data);
|
||||
uint16_t crc = 0xFFFF;
|
||||
for (uint16_t i = 0; i < length; i++)
|
||||
{
|
||||
crc = static_cast<uint16_t>((crc << 8) ^ d_crc16_table[(crc >> 8) ^ bytes[i]]);
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Opens the serial device and sets the default baud rate for a NMEA transmission (9600,8,N,1)
|
||||
*/
|
||||
int An_Packet_Printer::init_serial(const std::string& serial_device)
|
||||
{
|
||||
int fd = 0;
|
||||
// clang-format off
|
||||
struct termios options{};
|
||||
// clang-format on
|
||||
const int64_t BAUD = B9600; // BAUD = B38400;
|
||||
const int64_t DATABITS = CS8;
|
||||
const int64_t STOPBITS = 0;
|
||||
const int64_t PARITYON = 0;
|
||||
const int64_t PARITY = 0;
|
||||
|
||||
fd = open(serial_device.c_str(), O_RDWR | O_NOCTTY | O_NDELAY | O_CLOEXEC);
|
||||
if (fd == -1)
|
||||
{
|
||||
return fd; // failed to open TTY port
|
||||
}
|
||||
|
||||
if (fcntl(fd, F_SETFL, 0) == -1)
|
||||
{
|
||||
LOG(INFO) << "Error enabling direct I/O"; // clear all flags on descriptor, enable direct I/O
|
||||
}
|
||||
tcgetattr(fd, &options); // read serial port options
|
||||
|
||||
options.c_cflag = BAUD | DATABITS | STOPBITS | PARITYON | PARITY | CLOCAL | CREAD;
|
||||
// enable receiver, set 8 bit data, ignore control lines
|
||||
// options.c_cflag |= (CLOCAL | CREAD | CS8);
|
||||
options.c_iflag = IGNPAR;
|
||||
|
||||
// set the new port options
|
||||
tcsetattr(fd, TCSANOW, &options);
|
||||
return fd;
|
||||
}
|
||||
128
src/algorithms/PVT/libs/an_packet_printer.h
Normal file
128
src/algorithms/PVT/libs/an_packet_printer.h
Normal file
@ -0,0 +1,128 @@
|
||||
/*!
|
||||
* \file an_packet_printer.h
|
||||
* \brief Interface of a class that prints PVT solutions in a serial device
|
||||
* following a custom version of the Advanced Navigation Packet Protocol
|
||||
* \author Carles Fernandez-Prades, 2021. cfernandez(at)cttc.es
|
||||
* \author Miguel Angel Gomez Lopez, 2021. gomezlma(at)inta.es
|
||||
*
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2021 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
|
||||
#ifndef GNSS_SDR_AN_PACKET_PRINTER_H
|
||||
#define GNSS_SDR_AN_PACKET_PRINTER_H
|
||||
|
||||
#include <array>
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
|
||||
/** \addtogroup PVT
|
||||
* \{ */
|
||||
/** \addtogroup PVT_libs
|
||||
* \{ */
|
||||
|
||||
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;
|
||||
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;
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t id;
|
||||
uint8_t length;
|
||||
uint8_t header[5]; // AN_PACKET_HEADER_SIZE
|
||||
uint8_t data[126]; // AN_MAXIMUM_PACKET_SIZE
|
||||
} an_packet_t;
|
||||
|
||||
|
||||
/*!
|
||||
* \brief class that prints PVT solutions in a serial device following a custom
|
||||
* version of the Advanced Navigation Packet Protocol.
|
||||
*/
|
||||
class An_Packet_Printer
|
||||
{
|
||||
public:
|
||||
/*!
|
||||
* \brief Default constructor.
|
||||
*/
|
||||
explicit An_Packet_Printer(const std::string& an_dump_devname);
|
||||
|
||||
/*!
|
||||
* \brief Default destructor.
|
||||
*/
|
||||
~An_Packet_Printer();
|
||||
|
||||
/*!
|
||||
* \brief Print AN packet to the initialized device.
|
||||
*/
|
||||
bool print_packet(const Rtklib_Solver* const pvt_data);
|
||||
|
||||
/*!
|
||||
* \brief Close serial port. Also done in the destructor, this is only
|
||||
* for testing.
|
||||
*/
|
||||
void close_serial() const;
|
||||
|
||||
private:
|
||||
const std::array<uint16_t, 256> d_crc16_table =
|
||||
{
|
||||
0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, 0x1231, 0x0210, 0x3273,
|
||||
0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6, 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485, 0xa56a, 0xb54b, 0x8528,
|
||||
0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, 0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc, 0x48c4, 0x58e5, 0x6886,
|
||||
0x78a7, 0x0840, 0x1861, 0x2802, 0x3823, 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b, 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12, 0xdbfd, 0xcbdc, 0xfbbf,
|
||||
0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41, 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49, 0x7e97, 0x6eb6, 0x5ed5,
|
||||
0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70, 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, 0x1080, 0x00a1, 0x30c2,
|
||||
0x20e3, 0x5004, 0x4025, 0x7046, 0x6067, 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e, 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256, 0xb5ea, 0xa5cb, 0x95a8,
|
||||
0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c, 0x26d3, 0x36f2, 0x0691,
|
||||
0x16b0, 0x6657, 0x7676, 0x4615, 0x5634, 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab, 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, 0xcb7d, 0xdb5c, 0xeb3f,
|
||||
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{});
|
||||
|
||||
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;
|
||||
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 LSB_bytes_to_array(void* _in, int offset, uint8_t* _out, uint8_t var_size) const;
|
||||
|
||||
std::string d_an_devname;
|
||||
int d_an_dev_descriptor; // serial device descriptor (i.e. COM port)
|
||||
};
|
||||
|
||||
/** \} */
|
||||
/** \} */
|
||||
#endif // GNSS_SDR_AN_PACKET_PRINTER_H
|
||||
@ -37,6 +37,7 @@ public:
|
||||
std::string nmea_dump_filename;
|
||||
std::string nmea_dump_devname;
|
||||
std::string rtcm_dump_devname;
|
||||
std::string an_dump_devname;
|
||||
std::string output_path = std::string(".");
|
||||
std::string rinex_output_path = std::string(".");
|
||||
std::string gpx_output_path = std::string(".");
|
||||
@ -59,6 +60,7 @@ public:
|
||||
int32_t nmea_rate_ms = 1000;
|
||||
int32_t rinex_version = 0;
|
||||
int32_t rinexobs_rate_ms = 0;
|
||||
int32_t an_rate_ms = 1000;
|
||||
int32_t max_obs_block_rx_clock_offset_ms = 40;
|
||||
int udp_port = 0;
|
||||
int udp_eph_port = 0;
|
||||
@ -75,6 +77,7 @@ public:
|
||||
bool gpx_output_enabled = true;
|
||||
bool geojson_output_enabled = true;
|
||||
bool nmea_output_file_enabled = true;
|
||||
bool an_output_enabled = false;
|
||||
bool kml_output_enabled = true;
|
||||
bool xml_output_enabled = true;
|
||||
bool rtcm_output_file_enabled = true;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user