mirror of https://github.com/ArduPilot/ardupilot
AP_GPS: support RTCMStream for RTCM injection on UAVCAN
This commit is contained in:
parent
7fd3193c09
commit
f5a06e6bd3
|
@ -844,9 +844,13 @@ void AP_GPS::handle_gps_inject(const mavlink_message_t &msg)
|
|||
{
|
||||
mavlink_gps_inject_data_t packet;
|
||||
mavlink_msg_gps_inject_data_decode(&msg, &packet);
|
||||
//TODO: check target
|
||||
|
||||
inject_data(packet.data, packet.len);
|
||||
if (packet.len > sizeof(packet.data)) {
|
||||
// invalid packet
|
||||
return;
|
||||
}
|
||||
|
||||
handle_gps_rtcm_fragment(0, packet.data, packet.len);
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -942,7 +946,7 @@ void AP_GPS::lock_port(uint8_t instance, bool lock)
|
|||
}
|
||||
|
||||
// Inject a packet of raw binary to a GPS
|
||||
void AP_GPS::inject_data(uint8_t *data, uint16_t len)
|
||||
void AP_GPS::inject_data(const uint8_t *data, uint16_t len)
|
||||
{
|
||||
//Support broadcasting to all GPSes.
|
||||
if (_inject_to == GPS_RTK_INJECT_TO_ALL) {
|
||||
|
@ -954,7 +958,7 @@ void AP_GPS::inject_data(uint8_t *data, uint16_t len)
|
|||
}
|
||||
}
|
||||
|
||||
void AP_GPS::inject_data(uint8_t instance, uint8_t *data, uint16_t len)
|
||||
void AP_GPS::inject_data(uint8_t instance, const uint8_t *data, uint16_t len)
|
||||
{
|
||||
if (instance < GPS_MAX_RECEIVERS && drivers[instance] != nullptr) {
|
||||
drivers[instance]->inject_data(data, len);
|
||||
|
@ -1071,21 +1075,13 @@ bool AP_GPS::blend_health_check() const
|
|||
}
|
||||
|
||||
/*
|
||||
re-assemble GPS_RTCM_DATA message
|
||||
re-assemble fragmented RTCM data
|
||||
*/
|
||||
void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t &msg)
|
||||
void AP_GPS::handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_t len)
|
||||
{
|
||||
mavlink_gps_rtcm_data_t packet;
|
||||
mavlink_msg_gps_rtcm_data_decode(&msg, &packet);
|
||||
|
||||
if (packet.len > MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN) {
|
||||
// invalid packet
|
||||
return;
|
||||
}
|
||||
|
||||
if ((packet.flags & 1) == 0) {
|
||||
if ((flags & 1) == 0) {
|
||||
// it is not fragmented, pass direct
|
||||
inject_data(packet.data, packet.len);
|
||||
inject_data(data, len);
|
||||
return;
|
||||
}
|
||||
|
||||
|
@ -1098,8 +1094,8 @@ void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t &msg)
|
|||
}
|
||||
}
|
||||
|
||||
uint8_t fragment = (packet.flags >> 1U) & 0x03;
|
||||
uint8_t sequence = (packet.flags >> 3U) & 0x1F;
|
||||
uint8_t fragment = (flags >> 1U) & 0x03;
|
||||
uint8_t sequence = (flags >> 3U) & 0x1F;
|
||||
|
||||
// see if this fragment is consistent with existing fragments
|
||||
if (rtcm_buffer->fragments_received &&
|
||||
|
@ -1115,15 +1111,15 @@ void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t &msg)
|
|||
rtcm_buffer->fragments_received |= (1U << fragment);
|
||||
|
||||
// copy the data
|
||||
memcpy(&rtcm_buffer->buffer[MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*(uint16_t)fragment], packet.data, packet.len);
|
||||
memcpy(&rtcm_buffer->buffer[MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*(uint16_t)fragment], data, len);
|
||||
|
||||
// when we get a fragment of less than max size then we know the
|
||||
// number of fragments. Note that this means if you want to send a
|
||||
// block of RTCM data of an exact multiple of the buffer size you
|
||||
// need to send a final packet of zero length
|
||||
if (packet.len < MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN) {
|
||||
if (len < MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN) {
|
||||
rtcm_buffer->fragment_count = fragment+1;
|
||||
rtcm_buffer->total_length = (MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*fragment) + packet.len;
|
||||
rtcm_buffer->total_length = (MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*fragment) + len;
|
||||
} else if (rtcm_buffer->fragments_received == 0x0F) {
|
||||
// special case of 4 full fragments
|
||||
rtcm_buffer->fragment_count = 4;
|
||||
|
@ -1140,6 +1136,22 @@ void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t &msg)
|
|||
}
|
||||
}
|
||||
|
||||
/*
|
||||
re-assemble GPS_RTCM_DATA message
|
||||
*/
|
||||
void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t &msg)
|
||||
{
|
||||
mavlink_gps_rtcm_data_t packet;
|
||||
mavlink_msg_gps_rtcm_data_decode(&msg, &packet);
|
||||
|
||||
if (packet.len > sizeof(packet.data)) {
|
||||
// invalid packet
|
||||
return;
|
||||
}
|
||||
|
||||
handle_gps_rtcm_fragment(packet.flags, packet.data, packet.len);
|
||||
}
|
||||
|
||||
void AP_GPS::Write_AP_Logger_Log_Startup_messages()
|
||||
{
|
||||
for (uint8_t instance=0; instance<num_instances; instance++) {
|
||||
|
|
|
@ -436,6 +436,9 @@ public:
|
|||
_force_disable_gps = disable;
|
||||
}
|
||||
|
||||
// handle possibly fragmented RTCM injection data
|
||||
void handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_t len);
|
||||
|
||||
protected:
|
||||
|
||||
// configuration parameters
|
||||
|
@ -546,9 +549,8 @@ private:
|
|||
void handle_gps_inject(const mavlink_message_t &msg);
|
||||
|
||||
//Inject a packet of raw binary to a GPS
|
||||
void inject_data(uint8_t *data, uint16_t len);
|
||||
void inject_data(uint8_t instance, uint8_t *data, uint16_t len);
|
||||
|
||||
void inject_data(const uint8_t *data, uint16_t len);
|
||||
void inject_data(uint8_t instance, const uint8_t *data, uint16_t len);
|
||||
|
||||
// GPS blending and switching
|
||||
Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS]; // Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m)
|
||||
|
|
|
@ -292,4 +292,17 @@ bool AP_GPS_UAVCAN::read(void)
|
|||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
handle RTCM data from MAVLink GPS_RTCM_DATA, forwarding it over MAVLink
|
||||
*/
|
||||
void AP_GPS_UAVCAN::inject_data(const uint8_t *data, uint16_t len)
|
||||
{
|
||||
// we only handle this if we are the first UAVCAN GPS, as we send
|
||||
// the data as broadcast on all UAVCAN devive ports and we don't
|
||||
// want to send duplicates
|
||||
if (_detected_module == 0) {
|
||||
_detected_modules[0].ap_uavcan->send_RTCMStream(data, len);
|
||||
}
|
||||
}
|
||||
|
||||
#endif // HAL_WITH_UAVCAN
|
||||
|
|
|
@ -43,6 +43,8 @@ public:
|
|||
static void handle_fix_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const FixCb &cb);
|
||||
static void handle_aux_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AuxCb &cb);
|
||||
|
||||
void inject_data(const uint8_t *data, uint16_t len) override;
|
||||
|
||||
private:
|
||||
void handle_fix_msg(const FixCb &cb);
|
||||
void handle_aux_msg(const AuxCb &cb);
|
||||
|
|
Loading…
Reference in New Issue