From a7e0c565dc477979e444706f3a88e85be69fefd6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 18 Apr 2021 12:51:22 +1000 Subject: [PATCH] AP_GPS: populate extension fields in GPS2_RAW --- libraries/AP_GPS/AP_GPS.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 3ac6cc17e2..60e3246eb4 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1285,6 +1285,12 @@ void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan) } const Location &loc = location(1); + float hacc = 0.0f; + float vacc = 0.0f; + float sacc = 0.0f; + horizontal_accuracy(1, hacc); + vertical_accuracy(1, vacc); + speed_accuracy(1, sacc); mavlink_msg_gps2_raw_send( chan, last_fix_time_ms(1)*(uint64_t)1000, @@ -1299,7 +1305,12 @@ void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan) num_sats(1), state[1].rtk_num_sats, state[1].rtk_age_ms, - gps_yaw_cdeg(1)); + gps_yaw_cdeg(1), + 0, // TODO: Elipsoid height in mm + hacc * 1000, // one-sigma standard deviation in mm + vacc * 1000, // one-sigma standard deviation in mm + sacc * 1000, // one-sigma standard deviation in mm/s + 0); // TODO one-sigma heading accuracy standard deviation } #endif // GPS_MAX_RECEIVERS