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

AP_GPS: Add covariance output to AP_GPS matching ROS NavSatFix

Browse Source 
* Zero every iteration in case GPS quality degrades or fix is lost
* Use float precision for now

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
Ryan Friedman 2023-04-03 23:39:40 -06:00 committed by Andrew Tridgell
parent 06d9a08d16
commit 1e9403dca6
2 changed files with 30 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_GPS/AP_GPS.cpp
View File

@ -525,6 +525,25 @@ bool AP_GPS::vertical_accuracy(uint8_t instance, float &vacc) const
return false;
}
AP_GPS::CovarianceType AP_GPS::position_covariance(const uint8_t instance, Matrix3f& cov) const
{
AP_GPS::CovarianceType cov_type = AP_GPS::CovarianceType::UNKNOWN;
cov.zero();
float hacc, vacc;
if (horizontal_accuracy(instance, hacc) && vertical_accuracy(instance, vacc))
{
cov_type = AP_GPS::CovarianceType::DIAGONAL_KNOWN;
const auto hacc_variance = hacc * hacc;
cov[0][0] = hacc_variance;
cov[1][1] = hacc_variance;
cov[2][2] = vacc * vacc;
}
// There may be a receiver that implements hdop+vdop but not accuracy
// If so, there could be a condition here to attempt to calculate it
return cov_type;
}
/**
convert GPS week and milliseconds to unix epoch in milliseconds

11
libraries/AP_GPS/AP_GPS.h
View File

@ -20,6 +20,7 @@
#include <AP_Common/Location.h>
#include <AP_Param/AP_Param.h>
#include "GPS_detect_state.h"
#include <AP_Math/AP_Math.h>
#include <AP_MSP/msp.h>
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
#include <SITL/SIM_GPS.h>
@ -168,6 +169,14 @@ public:
GPS_ROLE_MB_ROVER,
};
// GPS Covariance Types matching ROS2 sensor_msgs/msg/NavSatFix
enum class CovarianceType : uint8_t {
UNKNOWN = 0, ///< The GPS does not support any accuracy metrics
APPROXIMATED = 1, ///< The accuracy is approximated through metrics such as HDOP/VDOP
DIAGONAL_KNOWN = 2, ///< The diagonal (east, north, up) components of covariance are reported by the GPS
KNOWN = 3, ///< The full covariance array is reported by the GPS
};
/*
The GPS_State structure is filled in by the backend driver as it
parses each message from the GPS.
@ -327,6 +336,8 @@ public:
return vertical_accuracy(primary_instance, vacc);
}
CovarianceType position_covariance(const uint8_t instance, Matrix3f& cov) const WARN_IF_UNUSED;
// 3D velocity in NED format
const Vector3f &velocity(uint8_t instance) const {
return state[instance].velocity;