Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
63 changes: 63 additions & 0 deletions Tools/autotest/arducopter.py
Original file line number Diff line number Diff line change
Expand Up @@ -15360,6 +15360,68 @@ def TouchdownGroundEffectAlt(self):
# we are not at the home location - reboot so the next test starts there
self.reboot_sitl()

def BaroGroundEffectRangefinderSwitch(self):
'''EKF height survives a takeoff that switches from the range finder to a baro in ground effect'''
# With EK3_RNG_USE_HGT Copter uses the range finder for height while
# taking off, and ALT_HOLD's takeoff ends as the vehicle leaves the
# ground, so the height source goes back to baro there, still in ground
# effect, where SIM_BARO_GEFF_M makes the baro read low. A baro offset
# learned then is carried through the switch and kept after climbing out
# of the effect; resetting the height to that baro at the switch instead
# drops it by the whole error.
self.set_parameters({
"SIM_BARO_GEFF_M": 3.0,
"SIM_TERRAIN": 0, # flat ground at home, whatever an earlier test left
"RNGFND1_TYPE": 100,
"RNGFND1_MIN": 0,
"RNGFND1_MAX": 10,
"EK3_RNG_USE_HGT": 50,
"PILOT_TKO_ALT_M": 0,
})
self.reboot_sitl()
self.takeoff(10, mode='ALT_HOLD')
self.delay_sim_time(10, reason="collect samples clear of the ground effect")
self.change_mode('LAND')
self.wait_disarmed()

dfreader = self.dfreader_for_current_onboard_log()
armed = False
home_alt = None
true_alt = None
climb = []
high = []
while True:
m = dfreader.recv_match(type=['EV', 'MODE', 'SIM', 'XKF1'])
if m is None:
break
mtype = m.get_type()
if mtype == 'EV':
if m.Id == 10: # LogEvent::ARMED
armed = True
elif mtype == 'MODE':
if armed and m.Mode == 9: # LAND: the takeoff and hover are the part under test
break
elif mtype == 'SIM':
if home_alt is None:
home_alt = m.Alt
true_alt = m.Alt - home_alt
elif m.C == 0 and armed and true_alt is not None:
err = -m.PD - true_alt
if true_alt > 5:
high.append(err)
elif len(high) == 0:
climb.append(err)
if len(climb) < 10 or len(high) < 10:
raise NotAchievedException("insufficient samples (%u up to 5m, %u above it)" % (len(climb), len(high)))
worst_climb = min(climb)
mean_high = sum(high) / len(high)
self.progress("EKF height - truth: worst %+.2f m from arming to 5 m, mean %+.2f m above 5 m"
% (worst_climb, mean_high))
if worst_climb < -1.0:
raise NotAchievedException("EKF height dropped %.2f m below truth in the takeoff" % -worst_climb)
if abs(mean_high) > 0.5:
raise NotAchievedException("EKF height is %+.2f m off truth after climbing out of ground effect" % mean_high)

def _MAV_CMD_CONDITION_YAW(self, command):
self.start_subtest("absolute")
self.takeoff(20, mode='GUIDED')
Expand Down Expand Up @@ -16691,6 +16753,7 @@ def tests1c(self):
self.EK3_AccelBiasZeroVelOptFlow,
self.EK3_ZeroVelFusionNotUsedWithGPS,
self.TakeoffGroundEffectAlt,
self.BaroGroundEffectRangefinderSwitch,
self.TouchdownGroundEffectAlt,
self.StabilityPatch,
self.OBSTACLE_DISTANCE_3D,
Expand Down
14 changes: 11 additions & 3 deletions libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1332,10 +1332,15 @@ void NavEKF3_core::selectHeightForFusion()
activeHgtSource = AP_NavEKF_Source::SourceZ::BARO;
}

// ground effect makes the baro read low while takeoff or touchdown is expected. Fixed wing
// sets those flags for a launch rather than for rotor wash, so it is left as it was
const bool baroInGndEffect = (dal.get_takeoff_expected() || dal.get_touchdown_expected()) && !assume_zero_sideslip();

// if there is new baro data to fuse, calculate filtered baro data required by other processes
if (baroDataToFuse) {
// calculate offset to baro data that enables us to switch to Baro height use during operation
if (activeHgtSource != AP_NavEKF_Source::SourceZ::BARO) {
// calculate offset to baro data that enables us to switch to Baro height use during operation.
// Not from a baro in ground effect: the offset would carry the error into the next switch
if (activeHgtSource != AP_NavEKF_Source::SourceZ::BARO && !baroInGndEffect) {
calcFiltBaroOffset();
}
// filtered baro data used to provide a reference for takeoff
Expand Down Expand Up @@ -1438,7 +1443,10 @@ void NavEKF3_core::selectHeightForFusion()
// detect changes in source and reset height
if ((activeHgtSource != prevHgtSource) && fuseHgtData) {
prevHgtSource = activeHgtSource;
ResetPositionD(-hgtMea);
// a baro in ground effect is no height to reset to, so a switch to it keeps the height held
if (activeHgtSource != AP_NavEKF_Source::SourceZ::BARO || !baroInGndEffect) {
ResetPositionD(-hgtMea);
}
}

// If we haven't fused height data for a while or have bad IMU data, then declare the height data as being timed out
Expand Down
Loading