Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: send RTCM Stream message to over dronecan directly

Browse Source
This commit is contained in:
bugobliterator 2023-05-02 16:03:30 +10:00 committed by Andrew Tridgell
parent a277547248
commit 5026b3d08c
2 changed files with 38 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_GPS/AP_GPS_DroneCAN.cpp
View File

@ -766,7 +766,38 @@ void AP_GPS_DroneCAN::inject_data(const uint8_t *data, uint16_t len)
// don't want to send duplicates
if (_detected_module == 0 ||
_detected_modules[_detected_module].ap_dronecan != _detected_modules[0].ap_dronecan) {
_detected_modules[_detected_module].ap_dronecan->send_RTCMStream(data, len);
if (_rtcm_stream.buf == nullptr) {
// give enough space for a full round from a NTRIP server with all
// constellations
_rtcm_stream.buf = new ByteBuffer(2400);
if (_rtcm_stream.buf == nullptr) {
return;
}
}
_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);
}
}

6
libraries/AP_GPS/AP_GPS_DroneCAN.h
View File

@ -138,5 +138,11 @@ 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);
// GNSS RTCM injection
struct {
uint32_t last_send_ms;
ByteBuffer *buf;
} _rtcm_stream;
};
#endif