aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_GPS: fixed RTCM injection for DroneCAN GPS

Browse Source 
need to check broadcast() return
This commit is contained in:
Andrew Tridgell 2023-06-06 13:49:10 +10:00
parent 49389ccce6
commit 531b12e272
2 changed files with 36 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
libraries/AP_GPS/AP_GPS_DroneCAN.cpp
View File

@ -699,6 +699,9 @@ bool AP_GPS_DroneCAN::read(void)
}
WITH_SEMAPHORE(sem);
send_rtcm();
if (_new_data) {
_new_data = false;
@ -755,6 +758,37 @@ bool AP_GPS_DroneCAN::is_configured(void) const
return (status_flags & ARDUPILOT_GNSS_STATUS_STATUS_ARMABLE) != 0;
}
/*
send pending RTCM data
*/
void AP_GPS_DroneCAN::send_rtcm(void)
{
if (_rtcm_stream.buf == nullptr) {
return;
}
WITH_SEMAPHORE(sem);
const uint32_t now = AP_HAL::native_millis();
if (now - _rtcm_stream.last_send_ms < 20) {
// don't send more than 50 per second
return;
}
uint32_t outlen = 0;
const uint8_t *ptr = _rtcm_stream.buf->readptr(outlen);
if (ptr == nullptr || outlen == 0) {
return;
}
uavcan_equipment_gnss_RTCMStream msg {};
outlen = MIN(outlen, sizeof(msg.data.data));
msg.protocol_id = UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_PROTOCOL_ID_RTCM3;
memcpy(msg.data.data, ptr, outlen);
msg.data.len = outlen;
if (_detected_modules[_detected_module].ap_dronecan->rtcm_stream.broadcast(msg)) {
_rtcm_stream.buf->advance(outlen);
_rtcm_stream.last_send_ms = now;
}
}
/*
handle RTCM data from MAVLink GPS_RTCM_DATA, forwarding it over MAVLink
*/
@ -775,29 +809,7 @@ void AP_GPS_DroneCAN::inject_data(const uint8_t *data, uint16_t len)
}
}
_rtcm_stream.buf->write(data, len);
const uint32_t now = AP_HAL::native_millis();
if (now - _rtcm_stream.last_send_ms < 20) {
// don't send more than 50 per second
return;
}
_rtcm_stream.last_send_ms = now;
uavcan_equipment_gnss_RTCMStream msg;
uint32_t outlen = _rtcm_stream.buf->available();
if (outlen > 128) {
outlen = 128;
}
msg.protocol_id = UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_PROTOCOL_ID_RTCM3;
for (uint8_t i=0; i<outlen; i++) {
uint8_t b;
if (!_rtcm_stream.buf->read_byte(&b)) {
return;
}
msg.data.data[i] = b;
}
msg.data.len = outlen;
_detected_modules[_detected_module].ap_dronecan->rtcm_stream.broadcast(msg);
send_rtcm();
}
}

1
libraries/AP_GPS/AP_GPS_DroneCAN.h
View File

@ -138,6 +138,7 @@ private:
bool handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan, const uint8_t node_id, const char* name, int32_t &value);
bool handle_param_get_set_response_float(AP_DroneCAN* ap_dronecan, const uint8_t node_id, const char* name, float &value);
void handle_param_save_response(AP_DroneCAN* ap_dronecan, const uint8_t node_id, bool success);
void send_rtcm(void);
// GNSS RTCM injection
struct {