Skip to content
Open
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
233 changes: 233 additions & 0 deletions Tools/autotest/arducopter.py
Original file line number Diff line number Diff line change
Expand Up @@ -17737,6 +17737,238 @@ def BaroGroundEffectRangefinderSwitch(self):
if abs(mean_high) > 0.5:
raise NotAchievedException("EKF height is %+.2f m off truth after climbing out of ground effect" % mean_high)

def assert_ekf_height_tracks_climb(self, climb_m, timeout=90):
'''check the EKF height estimate follows climb_m of real altitude'''
gnd_alt = self.get_altitude(altitude_source="SIM_STATE.alt")
start_z = self.assert_receive_message('LOCAL_POSITION_NED', timeout=10).z
# wait on the simulated altitude rather than any estimate-derived
# altitude, because the estimate is the thing under test
self.wait_altitude(gnd_alt + climb_m, gnd_alt + climb_m + 30,
altitude_source="SIM_STATE.alt", timeout=timeout)
true_climb = self.get_altitude(altitude_source="SIM_STATE.alt") - gnd_alt
est_climb = start_z - self.assert_receive_message('LOCAL_POSITION_NED', timeout=10).z
self.progress("climbed %.1fm, EKF estimated %.1fm" % (true_climb, est_climb))
# bound by the climb asked for, not the one achieved: a vehicle flying a
# lagging estimate over-throttles and would otherwise widen its own bound
if abs(true_climb - est_climb) > 0.25 * climb_m:
raise NotAchievedException(
"EKF height did not track the climb: true=%.1fm est=%.1fm" %
(true_climb, est_climb))

def assert_ekf_terrain_offset_above(self, want_m):
'''check the EKF terrain offset in the current log stayed above -want_m'''
dfreader = self.dfreader_for_current_onboard_log()
low = None
while True:
m = dfreader.recv_match(type='XKF5')
if m is None:
break
# XKF5 is logged for the primary core only, so read every record
# XKF5.TOfs is the terrain position relative to the EKF datum,
# logged as int16 centimetres and scaled by the reader. Feeding the
# estimator a range that never leaves the ground drags the terrain up
# under the vehicle, which puts the datum far below it, so the floor
# is what shows the damage - HAGL only dips once TOfs has walked.
if low is None or m.TOfs < low:
low = m.TOfs
if low is None:
raise NotAchievedException("No XKF5 messages in the log")
self.progress("lowest EKF terrain offset %.1fm" % low)
if low < -want_m:
raise NotAchievedException(
"Terrain datum walked away from the vehicle: TOfs=%.1fm want>-%.1fm" % (low, want_m))

def assert_ekf_moved_since_arming_was_set(self):
'''check the movement latch was published in the current log'''
# bit 10 of the filter status, which this PR redefines from optical flow
# takeoff detection to "has moved since arming"
moved_bit = 1 << 10
dfreader = self.dfreader_for_current_onboard_log()
seen = False
while True:
m = dfreader.recv_match(type='XKF4')
if m is None:
break
if m.C == 0 and (m.SS & moved_bit):
seen = True
break
if not seen:
raise NotAchievedException(
"Movement was never detected: filter status bit 10 stayed clear")
self.progress("movement latch was set during the flight")

def EKF3RangeFinderOnGround(self):
'''Test the EKF assumes the on-ground range until the vehicle moves'''
# A range finder whose minimum range is above its ground clearance
# reads out-of-range-low while the vehicle sits on the ground, so the
# EKF substitutes RNGFND1_GNDCLR for the missing measurement. That
# substitution is released by the movement detector rather than by the
# arming state, so it has to survive arming, and it has to stop once
# the vehicle is up - whether or not the sensor ever comes into range.
self.set_parameters({
"RNGFND1_TYPE": 100, # SITL
"RNGFND1_GNDCLR": 0.4,
"RNGFND1_MIN": 1.0, # on the ground the sensor is out of range low
"RNGFND1_MAX": 60.0,
"EK3_SRC1_POSZ": 2, # rangefinder, so losing it shows up in the height
"DISARM_DELAY": 0, # sit armed on the ground for as long as we like
})

def reboot_and_restream(**kwargs):
# every reboot puts the stream rate back to sitl_streamrate(), and
# the default 5Hz lags the truth sample by most of a metre in a climb
self.reboot_sitl(**kwargs)
self.set_message_rate_hz('LOCAL_POSITION_NED', 20)

reboot_and_restream()

self.start_subtest("On-ground range is still fused while armed and stationary")
self.change_mode('ALT_HOLD')
self.wait_ready_to_arm()
self.arm_vehicle()
# Walk the baro away only once we are armed, so all of the drift falls
# inside the window the substitution is meant to cover. Both instances,
# because the EKF is free to pick either.
self.set_parameters({
"SIM_BARO_DRIFT": 0.3,
"SIM_BAR2_DRIFT": 0.3,
})
# nothing observable marks the drift accumulating, so this one is a
# plain wait: 20s at 0.3m/s puts 6m between the baro and the truth
self.delay_sim_time(20, reason='let the baro drift while armed on the ground')
m = self.assert_receive_message('LOCAL_POSITION_NED')
self.progress("EKF height while armed on the ground: %.2fm" % -m.z)
# with the on-ground range still fused the estimate is pinned near
# RNGFND1_GNDCLR; on the baro it walks off with the drift
if abs(m.z) > 1:
raise NotAchievedException(
"EKF followed the drifting baro on the ground (z=%.2fm, want<1m)" % m.z)
self.set_parameters({
"SIM_BARO_DRIFT": 0,
"SIM_BAR2_DRIFT": 0,
})

self.start_subtest("Coming into range releases the on-ground assumption")
# the sensor crosses RNGFND1_MIN early in this climb, which is the
# release the range term of the detector exists for
self.set_rc(3, 1800)
self.assert_ekf_height_tracks_climb(6)
self.set_rc(3, 1500)
self.land_and_disarm()
# the next arm is refused while the throttle stick is still centred
self.zero_throttle()

self.start_subtest("Movement releases it even if the sensor never comes into range")
# Nothing can bring this sensor into range, so the range term can never
# fire and the estimate can only follow the climb if the detector
# releases on movement. If it never does, the EKF keeps reading GNDCLR
# as its height and the vehicle climbs away.
self.set_parameter("RNGFND1_MIN", 50.0)
# no horizontal position control in ALT_HOLD, so allow the drift Copter
# already records for a takeoff and land
reboot_and_restream(startup_location_dist_max=2)
self.change_mode('ALT_HOLD')
self.wait_ready_to_arm()
self.arm_vehicle()
self.set_rc(3, 1800)
# The estimate is pinned until the detector releases, and the baro
# offset filter tracks the pinned height, so the error accumulated
# before the release is carried forward rather than corrected. Climb
# clear of that first and measure a leg that starts after it.
gnd_alt = self.get_altitude(altitude_source="SIM_STATE.alt")
self.wait_altitude(gnd_alt + 8, gnd_alt + 40,
altitude_source="SIM_STATE.alt", timeout=90)
self.assert_ekf_height_tracks_climb(6)
self.set_rc(3, 1500)
self.land_and_disarm()
self.zero_throttle()

self.start_subtest("The terrain offset is not built from the on-ground assumption")
# With the baro as the height source the substituted range no longer
# drives the height estimate, it drives the terrain estimator. A sensor
# reading short for the whole flight must not be able to place the
# terrain right underneath a vehicle that is ten metres up.
self.set_parameters({
"EK3_SRC1_POSZ": 1, # baro, so the range only reaches terrain
"RNGFND1_MIN": 50.0, # reads short for the whole flight
})
# also starts the log this leg is read from
reboot_and_restream(startup_location_dist_max=2)
self.change_mode('ALT_HOLD')
self.wait_ready_to_arm()
self.arm_vehicle()
self.set_rc(3, 1800)
gnd_alt = self.get_altitude(altitude_source="SIM_STATE.alt")
self.wait_altitude(gnd_alt + 10, gnd_alt + 40,
altitude_source="SIM_STATE.alt", timeout=90)
self.set_rc(3, 1500)
self.delay_sim_time(5, reason='let the terrain estimator settle at altitude')
self.land_and_disarm()
self.zero_throttle()
self.assert_ekf_terrain_offset_above(2)
self.assert_ekf_moved_since_arming_was_set()

self.start_subtest("A second sensor out of range low does not hide the first")
# Both sensors read the same simulated range. The first is in range on
# the ground; the second is short until 5m, and an on-ground range
# substituted for it would be fused in place of the first one's reading.
self.set_parameters({
"EK3_SRC1_POSZ": 2,
"RNGFND1_MIN": 0,
"RNGFND2_TYPE": 100,
"RNGFND2_GNDCLR": 0.4,
"RNGFND2_MIN": 5.0,
"RNGFND2_MAX": 60.0,
})
reboot_and_restream(startup_location_dist_max=2)
self.change_mode('ALT_HOLD')
self.wait_ready_to_arm()
self.arm_vehicle()
self.set_rc(3, 1800)
# the latch's height term ends the substitution once 1.5m is flown and
# the estimate catches up soon after, so the damage shows in a short climb
self.assert_ekf_height_tracks_climb(3)
self.set_rc(3, 1500)
self.land_and_disarm()
self.zero_throttle()
# the next leg reboots, which removes the second sensor
self.set_parameter("RNGFND2_TYPE", 0)

self.start_subtest("Indoor config, no vertical velocity source")
# optical flow for XY, range finder for Z, no GPS. This is the config
# the EKF3 playbook recommends indoors, and the one where the range
# finder is the only vertical observation the filter has.
self.set_parameters({
"SIM_FLOW_ENABLE": 1,
"FLOW_TYPE": 10,
"SIM_GPS1_ENABLE": 0,
"SIM_TERRAIN": 0,
})
self.configure_EKFs_to_use_optical_flow_instead_of_GPS() # sets EK3_SRC1_VELZ=0
self.set_parameters({
"EK3_SRC1_POSZ": 2,
"RNGFND1_MIN": 50.0,
})
reboot_and_restream(startup_location_dist_max=2)
self.change_mode('ALT_HOLD')
self.wait_ready_to_arm(require_absolute=False)
self.arm_vehicle()
self.set_rc(3, 1800)
gnd_alt = self.get_altitude(altitude_source="SIM_STATE.alt")
self.wait_altitude(gnd_alt + 8, gnd_alt + 40,
altitude_source="SIM_STATE.alt", timeout=90)
# with no vertical velocity source the estimate lags harder, and that lag
# is roughly fixed in metres, so measure over a longer climb
self.assert_ekf_height_tracks_climb(10)
self.set_rc(3, 1500)
self.land_and_disarm()
self.zero_throttle()

# Copter only: this says nothing about the fly-forward branch of
# detectFlight() that a Plane takes. Nor does it cover a premature
# release from airframe vibration - SITL models no gyro noise, so the
# detector's 0.1 rad/s threshold is never approached on the ground.

def _MAV_CMD_CONDITION_YAW(self, command):
self.start_subtest("absolute")
self.takeoff(20, mode='GUIDED')
Expand Down Expand Up @@ -19123,6 +19355,7 @@ def tests1c(self):
self.HomeCircleInclusionFence_MultipleHomeCircle,
self.HomeCircleInclusionFence_Avoidance,
self.HomeAltResetTest,
self.EKF3RangeFinderOnGround,
])
return ret

Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_NavEKF/AP_Nav_Common.h
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ enum class NavFilterStatusBit {
CONST_POS_MODE = 128, // in constant position mode
PRED_HORIZ_POS_REL = 256, // expected good relative horizontal position estimate - used before takeoff
PRED_HORIZ_POS_ABS = 512, // expected good absolute horizontal position estimate - used before takeoff
TAKEOFF_DETECTED = 1024, // optical flow takeoff has been detected
TAKEOFF_DETECTED = 1024, // EKF2: optical flow takeoff detected. EKF3: vehicle has moved since arming
TAKEOFF_EXPECTED = 2048, // compensating for baro errors during takeoff
TOUCHDOWN_EXPECTED = 4096, // compensating for baro errors during touchdown
USING_GPS = 8192, // using GPS position
Expand All @@ -57,7 +57,7 @@ union nav_filter_status {
bool const_pos_mode : 1; // 7 - true if we are in const position mode
bool pred_horiz_pos_rel : 1; // 8 - true if filter expects it can produce a good relative horizontal position estimate - used before takeoff
bool pred_horiz_pos_abs : 1; // 9 - true if filter expects it can produce a good absolute horizontal position estimate - used before takeoff
bool takeoff_detected : 1; // 10 - true if optical flow takeoff has been detected
bool takeoff_detected : 1; // 10 - EKF2: true if optical flow takeoff detected; EKF3: true if the vehicle has moved since arming
bool takeoff : 1; // 11 - true if filter is compensating for baro errors during takeoff
bool touchdown : 1; // 12 - true if filter is compensating for baro errors during touchdown
bool using_gps : 1; // 13 - true if we are using GPS position
Expand Down
4 changes: 3 additions & 1 deletion libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,8 @@ void NavEKF3_core::controlFilterModes()
// Detect if we are in flight on or ground
detectFlight();

detectMovementSinceArming();

// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
// avoid unnecessary operations
setWindMagStateLearningMode();
Expand Down Expand Up @@ -817,7 +819,7 @@ void NavEKF3_core::updateFilterStatus(void)
status.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode
status.flags.pred_horiz_pos_rel = status.flags.horiz_pos_rel; // EKF3 enters the required mode before flight
status.flags.pred_horiz_pos_abs = status.flags.horiz_pos_abs; // EKF3 enters the required mode before flight
status.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected
status.flags.takeoff_detected = movedSinceArming; // the vehicle has moved since it was armed
status.flags.takeoff = dal.get_takeoff_expected(); // The EKF has been told to expect takeoff is in a ground effect mitigation mode and has started the EKF-GSF yaw estimator
status.flags.touchdown = dal.get_touchdown_expected(); // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
status.flags.using_gps = ((imuSampleTime_ms - lastGpsPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
Expand Down
57 changes: 33 additions & 24 deletions libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@ void NavEKF3_core::readRangeFinder(void)
if (_rng == nullptr) {
return;
}

// update global range-on-ground, used for all rangefinders
rngOnGnd = MAX(_rng->ground_clearance_orient(ROTATION_PITCH_270), 0.05f);

// limit update rate to maximum allowed by data buffers
Expand All @@ -36,25 +38,48 @@ void NavEKF3_core::readRangeFinder(void)
// reset the timer used to control the measurement rate
lastRngMeasTime_ms = imuSampleTime_ms;

// the sensors share one buffer and recall takes the newest sample due, so
// a range substituted for one sensor would replace a good one from the other
bool anyRngGood = false;
for (uint8_t sensorIndex = 0; sensorIndex < ARRAY_SIZE(rngMeasIndex); sensorIndex++) {
const auto *sensor = _rng->get_backend(sensorIndex);
if ((sensor != nullptr) && (sensor->orientation() == ROTATION_PITCH_270) &&
(sensor->status() == AP_DAL_RangeFinder::Status::Good)) {
anyRngGood = true;
}
}

// store samples and sample time into a ring buffer if valid
// use data from two range finders if available

for (uint8_t sensorIndex = 0; sensorIndex < ARRAY_SIZE(rngMeasIndex); sensorIndex++) {
const auto *sensor = _rng->get_backend(sensorIndex);
if (sensor == nullptr) {
continue;
}
if ((sensor->orientation() == ROTATION_PITCH_270) && (sensor->status() == AP_DAL_RangeFinder::Status::Good)) {
rngMeasIndex[sensorIndex] ++;
if (rngMeasIndex[sensorIndex] > 2) {
rngMeasIndex[sensorIndex] = 0;
}
storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25;
storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance();
if (sensor->orientation() != ROTATION_PITCH_270) {
// only consume downward facing rangefinder data
continue;
}
float range_distance = 0.0f;
if (sensor->status() == AP_DAL_RangeFinder::Status::Good) {
// get the current range measurement
range_distance = sensor->distance();
} else if (!movedSinceArming && !anyRngGood && sensor->status() == AP_DAL_RangeFinder::Status::OutOfRangeLow) {
// assume the on-ground range until the vehicle moves
range_distance = rngOnGnd;
} else {
// ignore out-of-range data
continue;
}

// place distance in the range data buffer
rngMeasIndex[sensorIndex]++;
if (rngMeasIndex[sensorIndex] > 2) {
rngMeasIndex[sensorIndex] = 0;
}
storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25;
storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = range_distance;

// check for three fresh samples
bool sampleFresh[DOWNWARD_RANGEFINDER_MAX_INSTANCES][3] = {};
for (uint8_t index = 0; index <= 2; index++) {
Expand Down Expand Up @@ -94,18 +119,6 @@ void NavEKF3_core::readRangeFinder(void)

// indicate we have updated the measurement
rngValidMeaTime_ms = imuSampleTime_ms;

} else if (onGround && ((imuSampleTime_ms - rngValidMeaTime_ms) > 200)) {
// before takeoff we assume on-ground range value if there is no data
rangeDataNew.time_ms = imuSampleTime_ms;
rangeDataNew.rng = rngOnGnd;

// write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
storedRange.push(rangeDataNew);

// indicate we have updated the measurement
rngValidMeaTime_ms = imuSampleTime_ms;

}
}
}
Expand Down Expand Up @@ -197,10 +210,6 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
delAngBodyOF.zero();
delTimeOF = 0.0f;
}
// by definition if this function is called, then flow measurements have been provided so we
// need to run the optical flow takeoff detection
detectOptFlowTakeoff();

// don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)
if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) {
// correct flow sensor body rates for bias and write
Expand Down
Loading
Loading