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

AP_DDS: Consume covariance implementation in AP_DDS

Browse Source 
* Use float precision as long as possible

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
Ryan Friedman 2023-04-05 01:24:04 -06:00 committed by Andrew Tridgell
parent 1e9403dca6
commit 9e9d487442
1 changed files with 13 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_DDS/AP_DDS_Client.cpp
View File

@ -6,6 +6,7 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_RTC/AP_RTC.h> #include <AP_RTC/AP_RTC.h>
#include <AP_SerialManager/AP_SerialManager.h> #include <AP_SerialManager/AP_SerialManager.h>
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#include "AP_DDS_Client.h" #include "AP_DDS_Client.h"
@ -81,17 +82,13 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t i
case AP_GPS::GPS_OK_FIX_2D: case AP_GPS::GPS_OK_FIX_2D:
case AP_GPS::GPS_OK_FIX_3D: case AP_GPS::GPS_OK_FIX_3D:
msg.status.status = 0; // STATUS_FIX msg.status.status = 0; // STATUS_FIX
msg.position_covariance_type = 1; // COVARIANCE_TYPE_APPROXIMATED
break; break;
case AP_GPS::GPS_OK_FIX_3D_DGPS: case AP_GPS::GPS_OK_FIX_3D_DGPS:
msg.status.status = 1; // STATUS_SBAS_FIX msg.status.status = 1; // STATUS_SBAS_FIX
msg.position_covariance_type = 1; // COVARIANCE_TYPE_APPROXIMATED
break; break;
case AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT: case AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT:
case AP_GPS::GPS_OK_FIX_3D_RTK_FIXED: case AP_GPS::GPS_OK_FIX_3D_RTK_FIXED:
msg.status.status = 2; // STATUS_SBAS_FIX msg.status.status = 2; // STATUS_SBAS_FIX
msg.position_covariance_type = 1; // COVARIANCE_TYPE_APPROXIMATED
// RTK provides "rtk_accuracy" member, should it be used for the covariance?
break; break;
default: default:
//! @todo Can we not just use an enum class and not worry about this condition? //! @todo Can we not just use an enum class and not worry about this condition?
@ -110,17 +107,18 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t i
} }
msg.altitude = alt_cm / 100.0; msg.altitude = alt_cm / 100.0;
// Calculate covariance: https://answers.ros.org/question/10310/calculate-navsatfix-covariance/ // ROS allows double precision, ArduPilot exposes float precision today
// https://github.com/ros-drivers/nmea_navsat_driver/blob/indigo-devel/src/libnmea_navsat_driver/driver.py#L110-L114 Matrix3f cov;
//! @todo This calculation will be moved to AP::gps and fixed in #23259 msg.position_covariance_type = (uint8_t)gps.position_covariance(instance, cov);
//! It is a placeholder for now matching the ROS1 nmea_navsat_driver behavior msg.position_covariance[0] = cov[0][0];
const auto hdop = gps.get_hdop(instance); msg.position_covariance[1] = cov[0][1];
const auto hdopSq = hdop * hdop; msg.position_covariance[2] = cov[0][2];
const auto vdop = gps.get_vdop(instance); msg.position_covariance[3] = cov[1][0];
const auto vdopSq = vdop * vdop; msg.position_covariance[4] = cov[1][1];
msg.position_covariance[0] = hdopSq; msg.position_covariance[5] = cov[1][2];
msg.position_covariance[4] = hdopSq; msg.position_covariance[6] = cov[2][0];
msg.position_covariance[8] = vdopSq; msg.position_covariance[7] = cov[2][1];
msg.position_covariance[8] = cov[2][2];
} }
void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg) void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg)