mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-09 01:13:57 -04:00
AP_NavEKF3: don't limit baro change when we are in fixed wing mode
This commit is contained in:
parent
ea6724aca5
commit
dc9435a88d
@ -737,7 +737,7 @@ void NavEKF3_core::readBaroData()
|
|||||||
|
|
||||||
// If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
|
// If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
|
||||||
// This prevents negative baro disturbances due to rotor wash ground interaction corrupting the EKF altitude during initial ascent
|
// This prevents negative baro disturbances due to rotor wash ground interaction corrupting the EKF altitude during initial ascent
|
||||||
if (dal.get_takeoff_expected()) {
|
if (dal.get_takeoff_expected() && !assume_zero_sideslip()) {
|
||||||
baroDataNew.hgt = MAX(baroDataNew.hgt, meaHgtAtTakeOff);
|
baroDataNew.hgt = MAX(baroDataNew.hgt, meaHgtAtTakeOff);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1182,7 +1182,7 @@ void NavEKF3_core::selectHeightForFusion()
|
|||||||
}
|
}
|
||||||
// If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
|
// If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
|
||||||
// This prevents negative baro disturbances due to rotor wash ground interaction corrupting the EKF altitude during initial ascent
|
// This prevents negative baro disturbances due to rotor wash ground interaction corrupting the EKF altitude during initial ascent
|
||||||
if (motorsArmed && dal.get_takeoff_expected()) {
|
if (motorsArmed && dal.get_takeoff_expected() && !assume_zero_sideslip()) {
|
||||||
hgtMea = MAX(hgtMea, meaHgtAtTakeOff);
|
hgtMea = MAX(hgtMea, meaHgtAtTakeOff);
|
||||||
}
|
}
|
||||||
velPosObs[5] = -hgtMea;
|
velPosObs[5] = -hgtMea;
|
||||||
|
Loading…
Reference in New Issue
Block a user