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
68 changes: 68 additions & 0 deletions Tools/autotest/arducopter.py
Original file line number Diff line number Diff line change
Expand Up @@ -4921,6 +4921,73 @@ def true_agl_m(ground_alt_m):
# just reboot.
self.reboot_sitl()

def OpticalFlowFocusAlt(self):
'''Below FLOW_ALT_MIN the EKF does not fuse optical flow so bad flow cannot drive a phantom velocity'''
# The check is driven by the altitude above terrain (HAGL) the EKF uses for flow fusion. FLOW_ALT_MIN
# is set far above any real sensor so the vehicle can hover below it long enough to measure.
self.set_parameters({
"SIM_FLOW_ENABLE": 1,
"FLOW_TYPE": 10,
"SIM_GPS1_ENABLE": 0,
})
self.configure_EKFs_to_use_optical_flow_instead_of_GPS()
self.set_analog_rangefinder_parameters()

hover_alt_m = 2.0

def fly_with_bad_flow(flow_alt_min):
self.set_parameters({"FLOW_ALT_MIN": flow_alt_min, "SIM_FLOW_OFS_X": 0})
self.reboot_sitl()
self.wait_ready_to_arm(require_absolute=False, timeout=120)
# flow is not healthy while stationary on the ground, so climb in ALT_HOLD
# before entering a mode that needs a position estimate
self.takeoff(
altitude_min=5,
mode='ALT_HOLD',
require_absolute=False,
takeoff_throttle=1700,
)
self.change_mode('GUIDED')
self.send_position_target_local_ned(0, 0, hover_alt_m)
self.wait_altitude(
hover_alt_m - 0.3,
hover_alt_m + 0.3,
relative=True,
minimum_duration=3,
timeout=90,
)
# measure in ALT_HOLD, which leaves a phantom velocity in the estimate. A
# position-controlled mode flies it away instead, hiding the effect.
self.hover()
self.change_mode('ALT_HOLD')
# only messages from the hover below FLOW_ALT_MIN are of interest
self.context_collect('STATUSTEXT')
self.context_clear_collection('STATUSTEXT')
# a flow rate offset reads as motion that is not happening, as an unfocused
# sensor does near the ground
self.set_parameter("SIM_FLOW_OFS_X", 1.0)

self.start_subtest("Below FLOW_ALT_MIN the flow is not fused")
fly_with_bad_flow(3.0)
self.wait_groundspeed(0, 0.5, minimum_duration=15, timeout=25)
# the EKF loses flow aiding after 5s without flow to fuse but must not start it again
if self.statustext_in_collections("started relative aiding"):
raise NotAchievedException("Relative aiding restarted below FLOW_ALT_MIN")
self.set_parameter("SIM_FLOW_OFS_X", 0)
self.disarm_vehicle(force=True)

self.start_subtest("FLOW_ALT_MIN disabled: bad flow drives a phantom velocity")
fly_with_bad_flow(0)
self.wait_groundspeed(0.8, 1000, timeout=30)
self.set_parameter("SIM_FLOW_OFS_X", 0)
self.disarm_vehicle(force=True)

self.start_subtest("Above FLOW_ALT_MIN the flow is still fused")
fly_with_bad_flow(1.0)
self.wait_groundspeed(0.8, 1000, timeout=30)
self.set_parameter("SIM_FLOW_OFS_X", 0)
self.disarm_vehicle(force=True)

def OpticalFlowCalibration(self):
'''test optical flow calibration'''
ex = None
Expand Down Expand Up @@ -20285,6 +20352,7 @@ def tests1e(self):
self.ModeAltHold,
self.OpticalFlow,
self.OpticalFlowLimits,
self.OpticalFlowFocusAlt,
self.BodyFrameOdom,
self.RTLSpeed,
self.MountYawVehicleForMountROI,
Expand Down
6 changes: 3 additions & 3 deletions libraries/AP_AHRS/AP_AHRS.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1428,13 +1428,13 @@ bool AP_AHRS::pre_arm_check(bool requires_position, char *failure_msg, uint8_t f
}

// write optical flow data to EKF
void AP_AHRS::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
void AP_AHRS::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride, float altMin)
{
#if HAL_NAVEKF2_AVAILABLE
ekf2.EKF2.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
ekf2.EKF2.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride, altMin);
#endif
#if EK3_FEATURE_OPTFLOW_FUSION
ekf3.EKF3.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
ekf3.EKF3.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride, altMin);
#endif
}

Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_AHRS/AP_AHRS.h
Original file line number Diff line number Diff line change
Expand Up @@ -300,7 +300,7 @@ class AP_AHRS {
bool get_vert_pos_rate_D(float &velocity) const;

// write optical flow measurements to EKF
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, const float heightOverride);
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, const float heightOverride, const float altMin);

// retrieve latest corrected optical flow samples (used for calibration)
bool getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const;
Expand Down
7 changes: 4 additions & 3 deletions libraries/AP_DAL/AP_DAL.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -345,7 +345,7 @@ bool AP_DAL::ekf_low_time_remaining(EKFType etype, uint8_t core)
}

// log optical flow data
void AP_DAL::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
void AP_DAL::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride, float altMin)
{
end_frame();

Expand All @@ -356,6 +356,7 @@ void AP_DAL::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawF
_ROFH.msecFlowMeas = msecFlowMeas;
_ROFH.posOffset = posOffset;
_ROFH.heightOverride = heightOverride;
_ROFH.altMin = altMin;
WRITE_REPLAY_BLOCK_IFCHANGED(ROFH, _ROFH, old);
}

Expand Down Expand Up @@ -490,9 +491,9 @@ void AP_DAL::handle_message(const log_RFRF &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
void AP_DAL::handle_message(const log_ROFH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
{
_ROFH = msg;
ekf2.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset, msg.heightOverride);
ekf2.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset, msg.heightOverride, msg.altMin);
#if EK3_FEATURE_OPTFLOW_FUSION
ekf3.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset, msg.heightOverride);
ekf3.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset, msg.heightOverride, msg.altMin);
#endif
}

Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_DAL/AP_DAL.h
Original file line number Diff line number Diff line change
Expand Up @@ -229,7 +229,7 @@ class AP_DAL {
}

// log optical flow data
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride, float altMin);

// log external nav data
void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
Expand Down
10 changes: 9 additions & 1 deletion libraries/AP_DAL/LogStructure.h
Original file line number Diff line number Diff line change
Expand Up @@ -513,13 +513,21 @@ struct log_RVOH {
// @Field: PZ: body-frame offset, Z-axis
// @Field: HgtOvr: sensor height override
// @Field: Qual: flow quality measurement
// @Field: U1: unused1
// @Field: U2: unused2
// @Field: U3: unused3
// @Field: AltMin: altitude above terrain below which the sensor cannot focus
struct log_ROFH {
Vector2f rawFlowRates;
Vector2f rawGyroRates;
uint32_t msecFlowMeas;
Vector3f posOffset;
float heightOverride;
uint8_t rawFlowQuality;
uint8_t unused1; // 3 bytes of padding so altMin is aligned
uint8_t unused2;
uint8_t unused3;
float altMin;
uint8_t _end;
};

Expand Down Expand Up @@ -697,7 +705,7 @@ struct log_RTER {
{ LOG_RVOH_MSG, RLOG_SIZE(RVOH), \
"RVOH", "fffIBB", "OX,OY,OZ,Del,H,Ena", "------", "------" }, \
{ LOG_ROFH_MSG, RLOG_SIZE(ROFH), \
"ROFH", "ffffIffffB", "FX,FY,GX,GY,Tms,PX,PY,PZ,HgtOvr,Qual", "----------", "----------" }, \
"ROFH", "ffffIffffBBBBf", "FX,FY,GX,GY,Tms,PX,PY,PZ,HgtOvr,Qual,U1,U2,U3,AltMin", "--------------", "--------------" }, \
{ LOG_REPH_MSG, RLOG_SIZE(REPH), \
"REPH", "fffffffffIIH", "PX,PY,PZ,Q1,Q2,Q3,Q4,PEr,AEr,TS,RT,D", "------------", "------------" }, \
{ LOG_RSLL_MSG, RLOG_SIZE(RSLL), \
Expand Down
5 changes: 3 additions & 2 deletions libraries/AP_NavEKF2/AP_NavEKF2.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1220,9 +1220,10 @@ bool NavEKF2::using_gps(void) const
// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
// posOffset is the XYZ flow sensor position in the body frame in m
// heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
void NavEKF2::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
// altMin is the altitude above terrain in m below which the sensor cannot focus. It is not used by EKF2, but is passed to the DAL so that both EKFs write the same record
void NavEKF2::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride, float altMin)
{
AP::dal().writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
AP::dal().writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride, altMin);

if (core) {
for (uint8_t i=0; i<num_cores; i++) {
Expand Down
3 changes: 2 additions & 1 deletion libraries/AP_NavEKF2/AP_NavEKF2.h
Original file line number Diff line number Diff line change
Expand Up @@ -183,7 +183,8 @@ class NavEKF2 {
// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
// posOffset is the XYZ flow sensor position in the body frame in m
// heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
// altMin is the altitude above terrain in m below which the sensor cannot focus. It is not used by EKF2, but is passed to the DAL so that both EKFs write the same record
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride, float altMin);

// Set to true if the terrain underneath is stable enough to be used as a height reference
// in combination with a range finder. Set to false if the terrain underneath the vehicle
Expand Down
7 changes: 4 additions & 3 deletions libraries/AP_NavEKF3/AP_NavEKF3.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1650,14 +1650,15 @@ bool NavEKF3::configuredToUseGPSForPos(void) const
// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
// posOffset is the XYZ flow sensor position in the body frame in m
// heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
// altMin is the altitude above terrain in m below which the sensor cannot focus. 0 if not used
#if EK3_FEATURE_OPTFLOW_FUSION
void NavEKF3::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
void NavEKF3::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride, float altMin)
{
dal.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
dal.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride, altMin);

if (core) {
for (uint8_t i=0; i<num_cores; i++) {
core[i].writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
core[i].writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride, altMin);
}
}
}
Expand Down
3 changes: 2 additions & 1 deletion libraries/AP_NavEKF3/AP_NavEKF3.h
Original file line number Diff line number Diff line change
Expand Up @@ -198,7 +198,8 @@ class NavEKF3 {
// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
// posOffset is the XYZ flow sensor position in the body frame in m
// heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
// altMin is the altitude above terrain in m below which the sensor cannot focus. 0 if not used
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride, float altMin);

// retrieve latest corrected optical flow samples (used for calibration)
bool getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const;
Expand Down
6 changes: 6 additions & 0 deletions libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -560,6 +560,12 @@ bool NavEKF3_core::readyToUseOptFlow(void) const
return false;
}

// flow that is not fused because the sensor is below its focus altitude cannot aid. Without this a vehicle
// held below that altitude leaves relative aiding on the flow timeout and re-enters it on the next sample
if (flowRngBelowAltMin) {
return false;
}

// We need stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use optical flow
return (imuSampleTime_ms - flowMeaTime_ms < 200) && tiltAlignComplete && delAngBiasLearned;
}
Expand Down
4 changes: 3 additions & 1 deletion libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -174,7 +174,7 @@ void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStam
#if EK3_FEATURE_OPTFLOW_FUSION
// write the raw optical flow measurements
// this needs to be called externally.
void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride, float altMin)
{
// limit update rate to maximum allowed by sensor buffers
if ((imuSampleTime_ms - flowMeaTime_ms) < frontend->sensorIntervalMin_ms) {
Expand Down Expand Up @@ -225,6 +225,8 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
ofDataNew.body_offset = posOffset.toftype();
// write the flow sensor height override
ofDataNew.heightOverride = heightOverride;
// write the altitude above terrain below which the flow sensor cannot focus
ofDataNew.altMin = altMin;
// write flow rate measurements corrected for body rates
ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x;
ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y;
Expand Down
11 changes: 11 additions & 0 deletions libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -338,13 +338,24 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus
range -= posOffsetEarth.z / prevTnb.c.z;
}

bool rangeIsEstimated = true;
#if APM_BUILD_TYPE(APM_BUILD_Rover)
// override with user specified height (if given, for rover)
if (ofDataDelayed.heightOverride > 0) {
range = ofDataDelayed.heightOverride;
rangeIsEstimated = false;
}
#endif

// a flow sensor cannot focus on the ground below its minimum altitude and returns values that are
// not motion. Don't fuse them but still calculate the innovations so they can be reported.
// On the ground before takeoff the flow is fused as zero instead, see SelectFlowFusion(),
// so that relative aiding is not lost. Skipped if the altitude is a user specified override
flowRngBelowAltMin = takeOffDetected && rangeIsEstimated && (range < ofDataDelayed.altMin);
if (flowRngBelowAltMin) {
really_fuse = false;
}

// Fuse X and Y axis measurements sequentially assuming observation errors are uncorrelated
for (uint8_t obsIndex=0; obsIndex<=1; obsIndex++) { // fuse X axis data first

Expand Down
1 change: 1 addition & 0 deletions libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -248,6 +248,7 @@ void NavEKF3_core::InitialiseVariables()
memset(&Pmut[0][0], 0, sizeof(Pmut));
memset(&KHP[0][0], 0, sizeof(KHP));
flowDataValid = false;
flowRngBelowAltMin = false;
rangeDataToFuse = false;
#if EK3_FEATURE_OPTFLOW_FUSION
Popt = 0.0f;
Expand Down
5 changes: 4 additions & 1 deletion libraries/AP_NavEKF3/AP_NavEKF3_core.h
Original file line number Diff line number Diff line change
Expand Up @@ -283,7 +283,8 @@ class NavEKF3_core : public NavEKF_core_common
// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
// posOffset is the XYZ flow sensor position in the body frame in m
// heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
// altMin is the altitude above terrain in m below which the sensor cannot focus. 0 if not used
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride, float altMin);

// retrieve latest corrected optical flow samples (used for calibration)
bool getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const;
Expand Down Expand Up @@ -646,6 +647,7 @@ class NavEKF3_core : public NavEKF_core_common
Vector3F bodyRadXYZ; // body frame XYZ axis angular rates averaged across the optical flow measurement interval (rad/sec)
Vector3F body_offset; // XYZ position of the optical flow sensor in body frame (m)
float heightOverride; // The fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
float altMin; // The altitude above terrain in m below which the sensor cannot focus. 0 if not used
};

struct vel_odm_elements : EKF_obs_element_t {
Expand Down Expand Up @@ -1321,6 +1323,7 @@ class NavEKF3_core : public NavEKF_core_common
// variables added for optical flow fusion
EKF_obs_buffer_t<of_elements> storedOF; // OF data buffer
bool flowDataValid; // true while optical flow data is still fresh
bool flowRngBelowAltMin; // true if the range used for fusing optical flow is below the altitude minimum
Vector2F auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator
uint32_t flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec)
uint32_t rngValidMeaTime_ms; // time stamp from latest valid range measurement (msec)
Expand Down
Loading
Loading