Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_GPS: allow logging of UBX-RXM-RTCM #26806

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
+141 −9
Open

AP_GPS: allow logging of UBX-RXM-RTCM #26806

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
c15503a
AP_GPS: allow logging of UBX-RXM-RTCM
andyp1per Apr 15, 2024
ceda1b6
AP_GPS: prevent de-duping valid RTCM messages
andyp1per Apr 16, 2024
906315e
AP_GPS: add RTKT and RTKR messages
andyp1per Apr 17, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
31 changes: 27 additions & 4 deletions libraries/AP_GPS/AP_GPS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -239,7 +239,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, 6:Enable RTCM full parse even for a single channel, 7:Disable automatic full RTCM parsing when RTCM seen on more than one channel
// @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, 8:Log RTCM data
// @User: Advanced
AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0),

Expand Down Expand Up @@ -1638,7 +1638,14 @@ bool AP_GPS::parse_rtcm_injection(mavlink_channel_t chan, const mavlink_gps_rtcm
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",
// @LoggerMessage: RTKR
// @Description: RTCM parser data
// @Field: TimeUS: microseconds since system startup
// @Field: Chan: RTCM channel data
// @Field: RTCMId: RTCM parser ID
// @Field: Len: RTCM injection packet length
// @Field: CRC: RTCM injection packet CRC
AP::logger().WriteStreaming("RTKR", "TimeUS,Chan,RTCMId,Len,CRC", "s#---", "F----", "QBHHI",
andyp1per marked this conversation as resolved.
Show resolved Hide resolved
AP_HAL::micros64(),
uint8_t(chan),
rtcm.parsers[chan]->get_id(),
Expand All @@ -1648,8 +1655,8 @@ bool AP_GPS::parse_rtcm_injection(mavlink_channel_t chan, const mavlink_gps_rtcm

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
if (rtcm.sent_crc[c] == crc && rtcm.sent_channels[c] != chan) {
// we have already sent this message, duplicates on the same channel are allowed
already_seen = true;
break;
}
Expand All @@ -1658,9 +1665,25 @@ bool AP_GPS::parse_rtcm_injection(mavlink_channel_t chan, const mavlink_gps_rtcm
continue;
}
rtcm.sent_crc[rtcm.sent_idx] = crc;
rtcm.sent_channels[rtcm.sent_idx] = chan;
rtcm.sent_idx = (rtcm.sent_idx+1) % ARRAY_SIZE(rtcm.sent_crc);

if (buf != nullptr && len > 0) {
#if HAL_LOGGING_ENABLED
// @LoggerMessage: RTKT
// @Description: RTCM parser data
// @Field: TimeUS: microseconds since system startup
// @Field: Chan: RTCM channel data
// @Field: RTCMId: RTCM parser ID
// @Field: Len: RTCM injection packet length
// @Field: CRC: RTCM injection packet CRC
AP::logger().WriteStreaming("RTKT", "TimeUS,Chan,RTCMId,Len,CRC", "s#---", "F----", "QBHHI",
andyp1per marked this conversation as resolved.
Show resolved Hide resolved
AP_HAL::micros64(),
uint8_t(chan),
rtcm.parsers[chan]->get_id(),
len,
crc);
#endif
inject_data(buf, len);
}
rtcm.parsers[chan]->reset();
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_GPS/AP_GPS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -631,6 +631,7 @@ class AP_GPS
GPSL5HealthOverride = (1U << 5),
AlwaysRTCMDecode = (1U << 6),
DisableRTCMDecode = (1U << 7),
LogRTCMData = (1U << 8),
};

// check if an option is set
Expand Down Expand Up @@ -802,6 +803,7 @@ class AP_GPS
struct {
RTCM3_Parser *parsers[MAVLINK_COMM_NUM_BUFFERS];
uint32_t sent_crc[32];
mavlink_channel_t sent_channels[32];
uint8_t sent_idx;
uint16_t seen_mav_channels;
} rtcm;
Expand Down
75 changes: 72 additions & 3 deletions libraries/AP_GPS/AP_GPS_UBLOX.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -405,6 +405,17 @@ AP_GPS_UBLOX::_request_next_config(void)
}
#else
_unconfigured_messages & = ~CONFIG_RATE_RAW;
#endif
break;
case STEP_RTCM:
#if UBLOX_RXM_RTCM_LOGGING
if(!option_set(AP_GPS::DriverOptions::LogRTCMData)) {
_unconfigured_messages &= ~CONFIG_RATE_RTCM;
} else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RTCM)) {
_next_message--;
}
#else
_unconfigured_messages & = ~CONFIG_RATE_RTCM;
#endif
break;
case STEP_VERSION:
Expand Down Expand Up @@ -541,7 +552,11 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
config_msg_id = CONFIG_RATE_POSLLH;
break;
case MSG_STATUS:
#if UBLOX_RXM_RTCM_LOGGING
desired_rate = RATE_STATUS;
#else
desired_rate = havePvtMsg ? 0 : RATE_STATUS;
#endif
config_msg_id = CONFIG_RATE_STATUS;
break;
case MSG_SOL:
Expand Down Expand Up @@ -582,9 +597,9 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
return;
}
break;
#if UBLOX_RXM_RAW_LOGGING
case CLASS_RXM:
switch(msg_id) {
#if UBLOX_RXM_RAW_LOGGING
case MSG_RXM_RAW:
desired_rate = gps._raw_data;
config_msg_id = CONFIG_RATE_RAW;
Expand All @@ -593,11 +608,17 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
desired_rate = gps._raw_data;
config_msg_id = CONFIG_RATE_RAW;
break;
#endif // UBLOX_RXM_RAW_LOGGING
#if UBLOX_RXM_RTCM_LOGGING
case MSG_RXM_RTCM:
desired_rate = option_set(AP_GPS::DriverOptions::LogRTCMData) ? 1 : 0;
config_msg_id = CONFIG_RATE_RTCM;
break;
#endif // UBLOX_RXM_RTCM_LOGGING
default:
return;
}
break;
#endif // UBLOX_RXM_RAW_LOGGING
#if UBLOX_TIM_TM2_LOGGING
case CLASS_TIM:
if (msg_id == MSG_TIM_TM2) {
Expand Down Expand Up @@ -975,6 +996,43 @@ void AP_GPS_UBLOX::log_rxm_rawx(const struct ubx_rxm_rawx &raw)
}
#endif // UBLOX_RXM_RAW_LOGGING

#if UBLOX_RXM_RTCM_LOGGING
void AP_GPS_UBLOX::log_rxm_rtcm(const struct ubx_rxm_rtcm &rtcm)
{
#if HAL_LOGGING_ENABLED
andyp1per marked this conversation as resolved.
Show resolved Hide resolved
if (!should_log()) {
return;
}

uint64_t now = AP_HAL::micros64();

const struct log_GPS_RTCM header = {
LOG_PACKET_HEADER_INIT(LOG_GPS_RTCM_MSG),
time_us : now,
version : rtcm.version,
flags : rtcm.flags,
refStation : rtcm.refStation,
msgType : rtcm.msgType
};
AP::logger().WriteBlock(&header, sizeof(header));
#endif
}

void AP_GPS_UBLOX::log_status(const struct ubx_nav_status &status)
{
#if HAL_LOGGING_ENABLED
andyp1per marked this conversation as resolved.
Show resolved Hide resolved
if (!should_log()) {
return;
}

AP::logger().WriteStreaming("UBX3", "TimeUS,iTOW,fixType,fixStat,diffStat,res,fixTime,uptime", "s-------", "F-------", "QIBBBBII",
AP_HAL::micros64(),
status.itow, status.fix_type, status.fix_status, status.differential_status,
status.res, status.time_to_first_fix, status.uptime);
#endif
}
#endif // UBLOX_RXM_RTCM_LOGGING

void AP_GPS_UBLOX::unexpected_message(void)
{
Debug("Unexpected message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
Expand Down Expand Up @@ -1471,6 +1529,13 @@ AP_GPS_UBLOX::_parse_gps(void)
}
#endif // UBLOX_RXM_RAW_LOGGING

#if UBLOX_RXM_RTCM_LOGGING
if (_class == CLASS_RXM && _msg_id == MSG_RXM_RTCM && option_set(AP_GPS::DriverOptions::LogRTCMData)) {
log_rxm_rtcm(_buffer.rxm_rtcm);
return false;
}
#endif // UBLOX_RXM_RTCM_LOGGING

#if UBLOX_TIM_TM2_LOGGING
if ((_class == CLASS_TIM) && (_msg_id == MSG_TIM_TM2) && (_payload_length == 28)) {
log_tim_tm2();
Expand Down Expand Up @@ -1516,6 +1581,9 @@ AP_GPS_UBLOX::_parse_gps(void)
Debug("MSG_STATUS fix_status=%u fix_type=%u",
_buffer.status.fix_status,
_buffer.status.fix_type);
#if UBLOX_RXM_RTCM_LOGGING
log_status(_buffer.status);
#endif
_check_new_itow(_buffer.status.itow);
if (havePvtMsg) {
_unconfigured_messages |= CONFIG_RATE_STATUS;
Expand Down Expand Up @@ -2170,7 +2238,8 @@ static const char *reasons[] = {"navigation rate",
"TIM TM2",
"F9",
"M10",
"L5 Enable Disable"};
"L5 Enable Disable",
"rtcm rate"};

static_assert((1 << ARRAY_SIZE(reasons)) == CONFIG_LAST, "UBLOX: Missing configuration description");

Expand Down
23 changes: 21 additions & 2 deletions libraries/AP_GPS/AP_GPS_UBLOX.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@
#define UBLOX_SET_BINARY_460800 "\265\142\006\001\003\000\001\006\001\022\117$PUBX,41,1,0023,0001,460800,0*11\r\n"

#define UBLOX_RXM_RAW_LOGGING 1
#define UBLOX_RXM_RTCM_LOGGING 1
#define UBLOX_MAX_RXM_RAW_SATS 22
#define UBLOX_MAX_RXM_RAWX_SATS 32
#define UBLOX_MAX_EXTENSIONS 8
Expand Down Expand Up @@ -104,13 +105,14 @@
#define CONFIG_F9 (1<<19)
#define CONFIG_M10 (1<<20)
#define CONFIG_L5 (1<<21)
#define CONFIG_LAST (1<<22) // this must always be the last bit
#define CONFIG_RATE_RTCM (1<<22)
#define CONFIG_LAST (1<<23) // this must always be the last bit

#define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED)

#define CONFIG_ALL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_SOL | CONFIG_RATE_VELNED \
| CONFIG_RATE_DOP | CONFIG_RATE_MON_HW | CONFIG_RATE_MON_HW2 | CONFIG_RATE_RAW | CONFIG_VERSION \
| CONFIG_NAV_SETTINGS | CONFIG_GNSS | CONFIG_SBAS)
| CONFIG_NAV_SETTINGS | CONFIG_GNSS | CONFIG_SBAS | CONFIG_RATE_RTCM)

//Configuration Sub-Sections
#define SAVE_CFG_IO (1<<0)
Expand Down Expand Up @@ -571,6 +573,16 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
};
#endif

#if UBLOX_RXM_RTCM_LOGGING
struct PACKED ubx_rxm_rtcm {
uint8_t version;
uint8_t flags;
uint16_t reserved1;
uint16_t refStation;
uint16_t msgType;
};
#endif

struct PACKED ubx_ack_ack {
uint8_t clsID;
uint8_t msgID;
Expand Down Expand Up @@ -630,6 +642,9 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
#if UBLOX_RXM_RAW_LOGGING
ubx_rxm_raw rxm_raw;
ubx_rxm_rawx rxm_rawx;
#endif
#if UBLOX_RXM_RTCM_LOGGING
ubx_rxm_rtcm rxm_rtcm;
#endif
ubx_ack_ack ack;
ubx_ack_nack nack;
Expand Down Expand Up @@ -686,6 +701,7 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
MSG_NAV_SVINFO = 0x30,
MSG_RXM_RAW = 0x10,
MSG_RXM_RAWX = 0x15,
MSG_RXM_RTCM = 0x32,
MSG_TIM_TM2 = 0x03
};
enum ubx_gnss_identifier {
Expand Down Expand Up @@ -748,6 +764,7 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
STEP_MON_HW2,
STEP_RAW,
STEP_RAWX,
STEP_RTCM,
STEP_VERSION,
STEP_RTK_MOVBASE, // setup moving baseline
STEP_TIM_TM2,
Expand Down Expand Up @@ -843,6 +860,8 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
void log_tim_tm2(void);
void log_rxm_raw(const struct ubx_rxm_raw &raw);
void log_rxm_rawx(const struct ubx_rxm_rawx &raw);
void log_rxm_rtcm(const struct ubx_rxm_rtcm &rtcm);
void log_status(const struct ubx_nav_status &status);

#if GPS_MOVING_BASELINE
// see if we should use uart2 for moving baseline config
Expand Down
19 changes: 19 additions & 0 deletions libraries/AP_GPS/LogStructure.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
LOG_GPS_RAW_MSG, \
LOG_GPS_RAWH_MSG, \
LOG_GPS_RAWS_MSG, \
LOG_GPS_RTCM_MSG, \
LOG_GPS_UBX1_MSG, \
LOG_GPS_UBX2_MSG, \
LOG_IDS_FROM_GPS_SBP
Expand Down Expand Up @@ -203,6 +204,22 @@ struct PACKED log_GPS_RAWS {
uint8_t trkStat;
};

// @LoggerMessage: GRTK
// @Description: RTCM input status
// @Field: TimeUS: Time since system startup
// @Field: ver: Message version (0x02 for this version)
// @Field: flgs: RTCM input status flags (see graphic below)
// @Field: ref: Reference station ID
// @Field: msgType: Message type
struct PACKED log_GPS_RTCM {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t version;
uint8_t flags;
uint16_t refStation;
uint16_t msgType;
};

#define LOG_STRUCTURE_FROM_GPS \
{ LOG_GPS_MSG, sizeof(log_GPS), \
"GPS", "QBBIHBcLLeffffB", "TimeUS,I,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,Yaw,U", "s#-s-S-DUmnhnh-", "F--C-0BGGB000--" , true }, \
Expand All @@ -218,4 +235,6 @@ struct PACKED log_GPS_RAWS {
"GRXH", "QdHbBB", "TimeUS,rcvTime,week,leapS,numMeas,recStat", "s-----", "F-----" , true }, \
{ LOG_GPS_RAWS_MSG, sizeof(log_GPS_RAWS), \
"GRXS", "QddfBBBHBBBBB", "TimeUS,prMes,cpMes,doMes,gnss,sv,freq,lock,cno,prD,cpD,doD,trk", "s------------", "F------------" , true }, \
{ LOG_GPS_RTCM_MSG, sizeof(log_GPS_RTCM), \
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Needs gating

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

See how HAL_NAVEKF2_AVAILABLE does things

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

libraries/AP_NavEKF2/LogStructure.h

"GRTK", "QBBHH", "TimeUS,ver,flgs,ref,msgType", "s----", "F----" , true }, \
LOG_STRUCTURE_FROM_GPS_SBP
Loading