Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: added option to decode RTCM injected data 

this allows for redundent RTCM links (eg. WiFi and SiK links for light
show drones) without causing corruption into the GPS.

If the GPS_DRV_OPTION bit is set then we instantiate a separate RTCM3
decoder per mavlink channel, and only inject when we get a full packet
that passes the RTCM 24 bit CRC
This commit is contained in:
Andrew Tridgell 2024-02-25 10:03:25 +11:00
parent fbfa3e43bc
commit 0f6f738a33
4 changed files with 117 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

92
libraries/AP_GPS/AP_GPS.cpp
View File

@ -55,6 +55,10 @@
#include <AP_Logger/AP_Logger.h>
#include "AP_GPS_FixType.h"
#if AP_GPS_RTCM_DECODE_ENABLED
#include "RTCM3_Parser.h"
#endif
#define GPS_RTK_INJECT_TO_ALL 127
#ifndef GPS_MAX_RATE_MS
#define GPS_MAX_RATE_MS 200 // maximum value of rate_ms (i.e. slowest update rate) is 5hz or 200ms
@ -341,7 +345,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Param: _DRV_OPTIONS
// @DisplayName: driver options
// @Description: Additional backend specific options
// @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL, 5:Override GPS satellite health of L5 band from L1 health
// @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL, 5:Override GPS satellite health of L5 band from L1 health, 6:Enable RTCM full parse even for a single channel, 7:Disable automatic full RTCM parsing when RTCM seen on more than one channel
// @User: Advanced
AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0),
@ -1338,12 +1342,12 @@ void AP_GPS::handle_gps_inject(const mavlink_message_t &msg)
/*
pass along a mavlink message (for MAV type)
*/
void AP_GPS::handle_msg(const mavlink_message_t &msg)
void AP_GPS::handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg)
{
switch (msg.msgid) {
case MAVLINK_MSG_ID_GPS_RTCM_DATA:
// pass data to de-fragmenter
handle_gps_rtcm_data(msg);
handle_gps_rtcm_data(chan, msg);
break;
case MAVLINK_MSG_ID_GPS_INJECT_DATA:
handle_gps_inject(msg);
@ -1676,7 +1680,7 @@ void AP_GPS::handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_
/*
re-assemble GPS_RTCM_DATA message
*/
void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t &msg)
void AP_GPS::handle_gps_rtcm_data(mavlink_channel_t chan, const mavlink_message_t &msg)
{
mavlink_gps_rtcm_data_t packet;
mavlink_msg_gps_rtcm_data_decode(&msg, &packet);
@ -1686,9 +1690,89 @@ void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t &msg)
return;
}
#if AP_GPS_RTCM_DECODE_ENABLED
if (!option_set(DriverOptions::DisableRTCMDecode)) {
const uint16_t mask = (1U << unsigned(chan));
rtcm.seen_mav_channels |= mask;
if (option_set(DriverOptions::AlwaysRTCMDecode) ||
(rtcm.seen_mav_channels & ~mask) != 0) {
/*
we are seeing RTCM on multiple mavlink channels. We will run
the data through a full per-channel RTCM decoder
*/
if (parse_rtcm_injection(chan, packet)) {
return;
}
}
}
#endif
handle_gps_rtcm_fragment(packet.flags, packet.data, packet.len);
}
#if AP_GPS_RTCM_DECODE_ENABLED
/*
fully parse RTCM data coming in from a MAVLink channel, when we have
a full message inject it to the GPS. This approach allows for 2 or
more MAVLink channels to be used for the same RTCM data, allowing
for redundent transports for maximum reliability at the cost of some
extra CPU and a bit of re-assembly lag
*/
bool AP_GPS::parse_rtcm_injection(mavlink_channel_t chan, const mavlink_gps_rtcm_data_t &pkt)
{
if (chan >= MAVLINK_COMM_NUM_BUFFERS) {
return false;
}
if (rtcm.parsers[chan] == nullptr) {
rtcm.parsers[chan] = new RTCM3_Parser();
if (rtcm.parsers[chan] == nullptr) {
return false;
}
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS: RTCM parsing for chan %u", unsigned(chan));
}
for (uint16_t i=0; i<pkt.len; i++) {
if (rtcm.parsers[chan]->read(pkt.data[i])) {
// we have a full message, inject it
const uint8_t *buf = nullptr;
uint16_t len = rtcm.parsers[chan]->get_len(buf);
// see if we have already sent it. This prevents
// duplicates from multiple sources
const uint32_t crc = crc_crc32(0, buf, len);
#if HAL_LOGGING_ENABLED
AP::logger().WriteStreaming("RTCM", "TimeUS,Chan,RTCMId,Len,CRC", "s#---", "F----", "QBHHI",
AP_HAL::micros64(),
uint8_t(chan),
rtcm.parsers[chan]->get_id(),
len,
crc);
#endif
bool already_seen = false;
for (uint8_t c=0; c<ARRAY_SIZE(rtcm.sent_crc); c++) {
if (rtcm.sent_crc[c] == crc) {
// we have already sent this message
already_seen = true;
break;
}
}
if (already_seen) {
continue;
}
rtcm.sent_crc[rtcm.sent_idx] = crc;
rtcm.sent_idx = (rtcm.sent_idx+1) % ARRAY_SIZE(rtcm.sent_crc);
if (buf != nullptr && len > 0) {
inject_data(buf, len);
}
rtcm.parsers[chan]->reset();
}
}
return true;
}
#endif // AP_GPS_RTCM_DECODE_ENABLED
void AP_GPS::Write_AP_Logger_Log_Startup_messages()
{
for (uint8_t instance=0; instance<num_instances; instance++) {

23
libraries/AP_GPS/AP_GPS.h
View File

@ -68,6 +68,7 @@
#endif // GPS_MOVING_BASELINE
class AP_GPS_Backend;
class RTCM3_Parser;
/// @class AP_GPS
/// GPS driver main class
@ -246,7 +247,7 @@ public:
void update(void);
// Pass mavlink data to message handlers (for MAV type)
void handle_msg(const mavlink_message_t &msg);
void handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg);
#if HAL_MSP_GPS_ENABLED
void handle_msp(const MSP::msp_gps_data_message_t &pkt);
#endif
@ -624,7 +625,9 @@ protected:
UBX_Use115200 = (1U << 2U),
UAVCAN_MBUseDedicatedBus = (1 << 3U),
HeightEllipsoid = (1U << 4),
GPSL5HealthOverride = (1U << 5)
GPSL5HealthOverride = (1U << 5),
AlwaysRTCMDecode = (1U << 6),
DisableRTCMDecode = (1U << 7),
};
// check if an option is set
@ -729,7 +732,7 @@ private:
} rtcm_stats;
// re-assemble GPS_RTCM_DATA message
void handle_gps_rtcm_data(const mavlink_message_t &msg);
void handle_gps_rtcm_data(mavlink_channel_t chan, const mavlink_message_t &msg);
void handle_gps_inject(const mavlink_message_t &msg);
//Inject a packet of raw binary to a GPS
@ -787,6 +790,20 @@ private:
// logging support
void Write_GPS(uint8_t instance);
#if AP_GPS_RTCM_DECODE_ENABLED
/*
per mavlink channel RTCM decoder, enabled with RTCM decode
option in GPS_DRV_OPTIONS
*/
struct {
RTCM3_Parser *parsers[MAVLINK_COMM_NUM_BUFFERS];
uint32_t sent_crc[32];
uint8_t sent_idx;
uint16_t seen_mav_channels;
} rtcm;
bool parse_rtcm_injection(mavlink_channel_t chan, const mavlink_gps_rtcm_data_t &pkt);
#endif
};
namespace AP {

4
libraries/AP_GPS/AP_GPS_config.h
View File

@ -62,3 +62,7 @@
#ifndef AP_GPS_UBLOX_ENABLED
#define AP_GPS_UBLOX_ENABLED AP_GPS_BACKEND_DEFAULT_ENABLED
#endif
#ifndef AP_GPS_RTCM_DECODE_ENABLED
#define AP_GPS_RTCM_DECODE_ENABLED BOARD_FLASH_SIZE > 1024
#endif

7
libraries/AP_GPS/RTCM3_Parser.h
View File

@ -19,7 +19,10 @@
#pragma once
#include <stdint.h>
#define RTCM3_MAX_PACKET_LEN 300
// maximum packet length with MAVLink GPS_RTCM_DATA is 4*180 as we
// handle up to 4 fragments
#define RTCM3_MAX_PACKET_LEN 720
class RTCM3_Parser {
public:
// process one byte, return true if packet found
@ -40,7 +43,7 @@ public:
private:
const uint8_t RTCMv3_PREAMBLE = 0xD3;
// raw packet, we shouldn't need over 300 bytes for the MB configs we use
// raw packet, we shouldn't need over 600 bytes for the MB configs we use
uint8_t pkt[RTCM3_MAX_PACKET_LEN];
// number of bytes in pkt[]