Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: sub update for yaw reset and inFlight checks

Browse Source 
on sub, request final yaw reset after diving 0.5m.
Also update rangefinder tests for sub
This commit is contained in:
Clyde McQueen 2024-06-24 09:47:36 -07:00 committed by Willian Galvani
parent 58db738a0f
commit 1f8ab27996
3 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -55,9 +55,15 @@ void NavEKF3_core::controlMagYawReset()
bool finalResetRequest = false; bool finalResetRequest = false;
bool interimResetRequest = false; bool interimResetRequest = false;
if (flightResetAllowed && !assume_zero_sideslip()) { if (flightResetAllowed && !assume_zero_sideslip()) {
#if APM_BUILD_TYPE(APM_BUILD_ArduSub)
// for sub, we'd like to be far enough away from metal structures like docks and vessels
// diving 0.5m is reasonable for both open water and pools
finalResetRequest = (stateStruct.position.z - posDownAtTakeoff) > EKF3_MAG_FINAL_RESET_ALT_SUB;
#else
// check that we have reached a height where ground magnetic interference effects are insignificant // check that we have reached a height where ground magnetic interference effects are insignificant
// and can perform a final reset of the yaw and field states // and can perform a final reset of the yaw and field states
finalResetRequest = (stateStruct.position.z - posDownAtTakeoff) < -EKF3_MAG_FINAL_RESET_ALT; finalResetRequest = (stateStruct.position.z - posDownAtTakeoff) < -EKF3_MAG_FINAL_RESET_ALT;
#endif
// check for increasing height // check for increasing height
bool hgtIncreasing = (posDownAtLastMagReset-stateStruct.position.z) > 0.5f; bool hgtIncreasing = (posDownAtLastMagReset-stateStruct.position.z) > 0.5f;

12
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

@ -379,6 +379,17 @@ void NavEKF3_core::detectFlight()
} }
if (!onGround) { if (!onGround) {
#if APM_BUILD_TYPE(APM_BUILD_ArduSub)
// If depth has increased since arming, then we definitely are diving
if ((stateStruct.position.z - posDownAtTakeoff) > 1.5f) {
inFlight = true;
}
// If rangefinder has decreased since arming, then we definitely are diving
if ((rangeDataNew.rng - rngAtStartOfFlight) < -0.5f) {
inFlight = true;
}
#else
// If height has increased since exiting on-ground, then we definitely are flying // If height has increased since exiting on-ground, then we definitely are flying
if ((stateStruct.position.z - posDownAtTakeoff) < -1.5f) { if ((stateStruct.position.z - posDownAtTakeoff) < -1.5f) {
inFlight = true; inFlight = true;
@ -388,6 +399,7 @@ void NavEKF3_core::detectFlight()
if ((rangeDataNew.rng - rngAtStartOfFlight) > 0.5f) { if ((rangeDataNew.rng - rngAtStartOfFlight) > 0.5f) {
inFlight = true; inFlight = true;
} }
#endif
// If more than 5 seconds since likely_flying was set // If more than 5 seconds since likely_flying was set
// true, then set inFlight true // true, then set inFlight true

1
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -60,6 +60,7 @@
// mag fusion final reset altitude (using NED frame so altitude is negative) // mag fusion final reset altitude (using NED frame so altitude is negative)
#define EKF3_MAG_FINAL_RESET_ALT 2.5f #define EKF3_MAG_FINAL_RESET_ALT 2.5f
#define EKF3_MAG_FINAL_RESET_ALT_SUB 0.5f
// learning rate for mag biases when using GPS yaw // learning rate for mag biases when using GPS yaw
#define EK3_GPS_MAG_LEARN_RATE 0.005f #define EK3_GPS_MAG_LEARN_RATE 0.005f