diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 285381b9ebc5..8c2f64456da4 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -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') @@ -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, diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 1f4ca90127e0..78aa33c974d9 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -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 @@ -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