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: broaden the acceptance criteria for GPS Yaw measurement

Browse Source
This commit is contained in:
bugobliterator 2024-02-06 10:52:28 +11:00 committed by Randy Mackay
parent eba74e53c7
commit 6fa4259b21
2 changed files with 32 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AP_GPS/GPS_Backend.cpp
View File

@ -326,7 +326,10 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(float reported_heading_deg, const
bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, const float reported_heading_deg, const float reported_distance, const float reported_D) { bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, const float reported_heading_deg, const float reported_distance, const float reported_D) {
constexpr float minimum_antenna_seperation = 0.05; // meters constexpr float minimum_antenna_seperation = 0.05; // meters
constexpr float permitted_error_length_pct = 0.2; // percentage constexpr float permitted_error_length_pct = 0.2; // percentage
#if HAL_LOGGING_ENABLED || AP_AHRS_ENABLED
float min_D = 0.0f;
float max_D = 0.0f;
#endif
bool selectedOffset = false; bool selectedOffset = false;
Vector3f offset; Vector3f offset;
switch (MovingBase::Type(gps.mb_params[interim_state.instance].type.get())) { switch (MovingBase::Type(gps.mb_params[interim_state.instance].type.get())) {
@ -373,10 +376,6 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state,
#if AP_AHRS_ENABLED #if AP_AHRS_ENABLED
{ {
// get lag
float lag = 0.1;
get_lag(lag);
// get vehicle rotation, projected back in time using the gyro // get vehicle rotation, projected back in time using the gyro
// this is not 100% accurate, but it is good enough for // this is not 100% accurate, but it is good enough for
// this test. To do it completely accurately we'd need an // this test. To do it completely accurately we'd need an
@ -385,17 +384,21 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state,
// for this use case // for this use case
const auto &ahrs = AP::ahrs(); const auto &ahrs = AP::ahrs();
const Vector3f &gyro = ahrs.get_gyro(); const Vector3f &gyro = ahrs.get_gyro();
Matrix3f rot_body_to_ned = ahrs.get_rotation_body_to_ned(); Matrix3f rot_body_to_ned_min_lag = ahrs.get_rotation_body_to_ned();
rot_body_to_ned.rotate(gyro * (-lag)); rot_body_to_ned_min_lag.rotate(gyro * -AP_GPS_MB_MIN_LAG);
Matrix3f rot_body_to_ned_max_lag = ahrs.get_rotation_body_to_ned();
rot_body_to_ned_max_lag.rotate(gyro * -AP_GPS_MB_MAX_LAG);
// apply rotation to the offset to get the Z offset in NED // apply rotation to the offset to get the Z offset in NED
const Vector3f antenna_tilt = rot_body_to_ned * offset; const Vector3f antenna_tilt_min_lag = rot_body_to_ned_min_lag * offset;
const float alt_error = reported_D + antenna_tilt.z; const Vector3f antenna_tilt_max_lag = rot_body_to_ned_max_lag * offset;
min_D = MIN(-antenna_tilt_min_lag.z, -antenna_tilt_max_lag.z);
if (fabsf(alt_error) > permitted_error_length_pct * min_dist) { max_D = MAX(-antenna_tilt_min_lag.z, -antenna_tilt_max_lag.z);
min_D -= permitted_error_length_pct * min_dist;
max_D += permitted_error_length_pct * min_dist;
if (reported_D < min_D || reported_D > max_D) {
// the vertical component is out of range, reject it // the vertical component is out of range, reject it
Debug("bad alt_err %.1f > %.1f\n", Debug("bad alt_err %f < %f < %f", (double)min_D, (double)reported_D, (double)max_D);
alt_error, permitted_error_length_pct * min_dist);
goto bad_yaw; goto bad_yaw;
} }
} }
@ -425,16 +428,20 @@ good_yaw:
// @Field: RHD: reported heading,deg // @Field: RHD: reported heading,deg
// @Field: RDist: antenna separation,m // @Field: RDist: antenna separation,m
// @Field: RDown: vertical antenna separation,m // @Field: RDown: vertical antenna separation,m
// @Field: MinCDown: minimum tolerable vertical antenna separation,m
// @Field: MaxCDown: maximum tolerable vertical antenna separation,m
// @Field: OK: 1 if have yaw // @Field: OK: 1 if have yaw
AP::logger().WriteStreaming("GPYW", "TimeUS,Id,RHD,RDist,RDown,OK", AP::logger().WriteStreaming("GPYW", "TimeUS,Id,RHD,RDist,RDown,MinCDown,MaxCDown,OK",
"s#dmm-", "s#dmmmm-",
"F-----", "F-------",
"QBfffB", "QBfffffB",
AP_HAL::micros64(), AP_HAL::micros64(),
state.instance, state.instance,
reported_heading_deg, reported_heading_deg,
reported_distance, reported_distance,
reported_D, reported_D,
min_D,
max_D,
interim_state.have_gps_yaw); interim_state.have_gps_yaw);
#endif #endif

8
libraries/AP_GPS/GPS_Backend.h
View File

@ -34,6 +34,14 @@
#define AP_GPS_DEBUG_LOGGING_ENABLED 0 #define AP_GPS_DEBUG_LOGGING_ENABLED 0
#endif #endif
#ifndef AP_GPS_MB_MIN_LAG
#define AP_GPS_MB_MIN_LAG 0.05f
#endif
#ifndef AP_GPS_MB_MAX_LAG
#define AP_GPS_MB_MAX_LAG 0.25f
#endif
#if AP_GPS_DEBUG_LOGGING_ENABLED #if AP_GPS_DEBUG_LOGGING_ENABLED
#include <AP_HAL/utility/RingBuffer.h> #include <AP_HAL/utility/RingBuffer.h>
#endif #endif