Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
27 commits
Select commit Hold shift + click to select a range
367ac1b
SITL: add SIM_FLOW_OFS optical flow rate offset for fault injection
andyp1per Jun 18, 2026
6292450
AP_OpticalFlow: apply SIM_FLOW_OFS offset to the SITL flow rate
andyp1per Jun 18, 2026
fa8dccc
AP_RangeFinder: add on_ground() for a reading taken on the ground
andyp1per Oct 3, 2026
fd54714
AP_DAL: force out an IFCHANGED block dropped before logging starts
andyp1per Sep 17, 2026
121a9b6
AP_DAL: carry the flow minimum focus height in a new ROFM message
andyp1per Sep 17, 2026
bbbdf32
AP_NavEKF2: pass the optical flow minimum focus height to the DAL
andyp1per Sep 17, 2026
c390522
AP_NavEKF3: discard optical flow below the sensor's focus height
andyp1per Sep 17, 2026
6892934
AP_DAL: pass the logged flow focus height to the estimators in Replay
andyp1per Oct 11, 2026
4237d0b
AP_AHRS: forward the optical flow minimum focus height
andyp1per Sep 17, 2026
358e199
AP_OpticalFlow: add FLOW_HGT_MIN, the sensor's minimum focus height
andyp1per Sep 17, 2026
5d370ee
AP_DAL: drop the default for the flow minimum focus height
andyp1per Oct 3, 2026
81284f0
AP_NavEKF2: drop the default for the flow minimum focus height
andyp1per Oct 3, 2026
e3bc739
AP_NavEKF3: drop the default for the flow minimum focus height
andyp1per Oct 3, 2026
52d3a4c
AP_AHRS: drop the default for the flow minimum focus height
andyp1per Oct 3, 2026
271f288
AP_DAL: give the range finder backend on_ground()
andyp1per Oct 3, 2026
b5cd1c9
AP_NavEKF3: fuse zero flow on the ground below the focus floor
andyp1per Oct 3, 2026
749bcd0
Replay: handle the ROFM optical flow metadata message
andyp1per Sep 17, 2026
63ff8f4
autotest: cover the optical flow focus height floor
andyp1per Sep 17, 2026
19b9e2b
AP_NavEKF3: clear the flow rest reading once no sample is stored
andyp1per Oct 12, 2026
8cd6410
autotest: cover a rest reading learned before the first arming
andyp1per Oct 12, 2026
d5db25e
AP_NavEKF3: leave the flow focus floor out of Rover builds
andyp1per Oct 15, 2026
215f298
AP_OpticalFlow: SITL flow from a sensor mounted above the ground
andyp1per Oct 13, 2026
88341d1
autotest: check a flow-only Rover keeps its speed over a range finder
andyp1per Oct 13, 2026
663224b
AP_OpticalFlow: say Rover does not use FLOW_HGT_MIN
andyp1per Oct 15, 2026
3dcdfb4
AP_NavEKF3: replay a Rover log without the flow focus floor
andyp1per Oct 19, 2026
a11e396
AP_NavEKF3: fuse the resting zero only while the vehicle is not flying
andyp1per Nov 15, 2026
51a7621
autotest: check the floor read from a hover is not a landing
andyp1per Nov 15, 2026
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
6 changes: 6 additions & 0 deletions Tools/Replay/LR_MsgHandler.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -309,6 +309,12 @@ void LR_MsgHandler_ROFH::process_message(uint8_t *msgbytes)
AP::dal().handle_message(msg, ekf2, ekf3);
}

void LR_MsgHandler_ROFM::process_message(uint8_t *msgbytes)
{
MSG_CREATE(ROFM, msgbytes);
AP::dal().handle_message(msg);
}

void LR_MsgHandler_RWOH::process_message(uint8_t *msgbytes)
{
MSG_CREATE(RWOH, msgbytes);
Expand Down
7 changes: 7 additions & 0 deletions Tools/Replay/LR_MsgHandler.h
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,13 @@ class LR_MsgHandler_ROFH : public LR_MsgHandler_EKF
void process_message(uint8_t *msg) override;
};

class LR_MsgHandler_ROFM : public LR_MsgHandler
{
public:
using LR_MsgHandler::LR_MsgHandler;
void process_message(uint8_t *msg) override;
};

class LR_MsgHandler_REPH : public LR_MsgHandler_EKF
{
using LR_MsgHandler_EKF::LR_MsgHandler_EKF;
Expand Down
2 changes: 2 additions & 0 deletions Tools/Replay/LogReader.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -122,6 +122,8 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RVOH(formats[f.type]);
} else if (streq(name, "ROFH")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_ROFH(formats[f.type], ekf2, ekf3);
} else if (streq(name, "ROFM")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_ROFM(formats[f.type]);
} else if (streq(name, "REPH")) {
msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REPH(formats[f.type], ekf2, ekf3);
} else if (streq(name, "RSLL")) {
Expand Down
694 changes: 694 additions & 0 deletions Tools/autotest/arducopter.py

Large diffs are not rendered by default.

59 changes: 59 additions & 0 deletions Tools/autotest/rover.py
Original file line number Diff line number Diff line change
Expand Up @@ -7391,6 +7391,64 @@ def OpticalFlow(self):
self.context_pop()
self.reboot_sitl()

def OpticalFlowRangefinderOnGround(self):
'''flow-only Rover keeps its speed over a range finder at its ground clearance'''
# the range finder reads its ground clearance throughout, which a flying vehicle's EKF
# takes as resting on the ground; a Rover must still use the flow, declared height or not
legs = (
("sensors 1.5 m up, height declared", 1.5, 1.5, 1.5),
("sensors 1.5 m up, no declared height", 0, 1.5, 1.5),
("sensors 0.14 m up, default ground clearance", 0, 0.14, 0.10),
)
for (name, hgt_ovr, sensor_height, gndclr) in legs:
self.start_subtest(name)
self.set_parameters({
"SIM_FLOW_ENABLE": 1,
"FLOW_TYPE": 10,
"FLOW_HGT_OVR": hgt_ovr,
"SIM_FLOW_POS_Z": -sensor_height,
"RNGFND1_TYPE": 100,
"RNGFND1_ORIENT": 25,
"RNGFND1_MIN": 0.1,
"RNGFND1_MAX": 10,
"RNGFND1_GNDCLR": gndclr,
"SIM_SONAR_POS_Z": -sensor_height,
"EK3_SRC1_POSXY": 0,
"EK3_SRC1_VELXY": 5,
"EK3_SRC1_VELZ": 0,
})
self.reboot_sitl()
self.change_mode('MANUAL')
self.wait_ready_to_arm(require_absolute=False)
self.arm_vehicle()
# drive a circle, as the EKF counts the first turn as a takeoff
self.set_rc_from_map({1: 1700, 3: 1540})
self.delay_sim_time(20, "into the circle")
ekf_speed = []
true_speed = []
ranges = []
tstart = self.get_sim_time()
while self.get_sim_time_cached() - tstart < 20:
ekf = self.assert_receive_message('LOCAL_POSITION_NED', timeout=5)
# the simulated GPS is still running, though the EKF does not use it
gps = self.assert_receive_message('GPS_RAW_INT', timeout=5)
ekf_speed.append(math.hypot(ekf.vx, ekf.vy))
true_speed.append(gps.vel * 0.01)
ranges.append(self.assert_receive_message('DISTANCE_SENSOR', timeout=5).current_distance * 0.01)
self.set_rc_from_map({1: 1500, 3: 1500})
self.disarm_vehicle()
# the case under test: the range within 5 cm of the ground clearance, where a vehicle that flies is on the ground
worst = max(abs(r - gndclr) for r in ranges)
if worst > 0.05:
raise NotAchievedException("%s: range %.2f m off the ground clearance of %.2f m" % (name, worst, gndclr))
ekf_mean = sum(ekf_speed) / len(ekf_speed)
true_mean = sum(true_speed) / len(true_speed)
self.progress("%s: EKF speed %.2f m/s, true speed %.2f m/s" % (name, ekf_mean, true_mean))
if true_mean < 0.2:
raise NotAchievedException("%s: drove at only %.2f m/s, not a test of the flow" % (name, true_mean))
if ekf_mean < 0.7 * true_mean:
raise NotAchievedException("%s: EKF speed %.2f m/s against %.2f m/s" % (name, ekf_mean, true_mean))

def RCDuplicateOptionsExist(self):
'''ensure duplicate RC option detection works'''
self.wait_ready_to_arm()
Expand Down Expand Up @@ -8210,6 +8268,7 @@ def tests(self):
self.FenceFullAndPartialTransfer,
self.MissionPolyEnabledPreArm,
self.OpticalFlow,
self.OpticalFlowRangefinderOnGround,
self.RCDuplicateOptionsExist,
self.ClearMission,
self.JammingSimulation,
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 @@ -1549,13 +1549,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 minHeight)
{
#if HAL_NAVEKF2_AVAILABLE
ekf2.EKF2.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
ekf2.EKF2.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride, minHeight);
#endif
#if EK3_FEATURE_OPTFLOW_FUSION
ekf3.EKF3.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
ekf3.EKF3.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride, minHeight);
#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 @@ -302,7 +302,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 minHeight);

// retrieve latest corrected optical flow samples (used for calibration)
bool getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const;
Expand Down
31 changes: 25 additions & 6 deletions libraries/AP_DAL/AP_DAL.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@ void AP_DAL::start_frame(AP_DAL::FrameType frametype)
bool logging = AP::logger().logging_started() && AP::logger().allow_start_ekf();
if (logging && !logging_started) {
force_write = true;
rofm_resend = true;
}
logging_started = logging;
#endif
Expand Down Expand Up @@ -301,12 +302,14 @@ uint8_t AP_DAL::logging_core(uint8_t c) const
// only write if the content has changed
void AP_DAL::WriteLogMessage(enum LogMessages msg_type, void *msg, const void *old_msg, uint8_t msg_size)
{
// we use the _end byte to hold a flag for forcing output
uint8_t &_end = ((uint8_t *)msg)[msg_size];
if (!logging_started) {
// we're not logging
// we're not logging. Mark for forced output so that a block which is
// only written when it changes is not lost for the whole of the next log
_end = 1;
return;
}
// we use the _end byte to hold a flag for forcing output
uint8_t &_end = ((uint8_t *)msg)[msg_size];
if (old_msg && !force_write && _end == 0 && memcmp(msg, old_msg, msg_size) == 0) {
// no change, skip this block write
return;
Expand Down Expand Up @@ -345,10 +348,23 @@ 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 minHeight)
{
end_frame();

// the focus height is a sensor property, so it goes in its own record ahead of
Comment thread
andyp1per marked this conversation as resolved.
Resolved
// the sample it applies to, and older logs without one replay with it disabled
const log_ROFM old_ROFM = _ROFM;
_ROFM.minHeight = minHeight;
// written from the flow callback, after force_write has been cleared, so a new log
// would never see an unchanged value
if (rofm_resend) {
WRITE_REPLAY_BLOCK(ROFM, _ROFM);
rofm_resend = false;
} else {
WRITE_REPLAY_BLOCK_IFCHANGED(ROFM, _ROFM, old_ROFM);
}

const log_ROFH old = _ROFH;
_ROFH.rawFlowQuality = rawFlowQuality;
_ROFH.rawFlowRates = rawFlowRates;
Expand Down Expand Up @@ -490,9 +506,12 @@ 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);
// both estimators are given the focus height from the last ROFM so that their
// writes back through this call match and do not re-log the record
const float minHeight = _ROFM.minHeight;
ekf2.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset, msg.heightOverride, minHeight);
#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, minHeight);
#endif
}

Expand Down
7 changes: 6 additions & 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 minHeight);

// 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 Expand Up @@ -353,6 +353,9 @@ class AP_DAL {
#endif
}
void handle_message(const log_ROFH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
void handle_message(const log_ROFM &msg) {
_ROFM = msg;
}
void handle_message(const log_REPH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
void handle_message(const log_REVH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
void handle_message(const log_RWOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
Expand Down Expand Up @@ -380,6 +383,7 @@ class AP_DAL {

// push-based sensor structures
struct log_ROFH _ROFH;
struct log_ROFM _ROFM;
struct log_REPH _REPH;
struct log_REVH _REVH;
struct log_RWOH _RWOH;
Expand Down Expand Up @@ -412,6 +416,7 @@ class AP_DAL {

static bool logging_started;
static bool force_write;
bool rofm_resend; // ROFM is due out in the log that has just started

bool ekf2_init_done;
bool ekf3_init_done;
Expand Down
10 changes: 10 additions & 0 deletions libraries/AP_DAL/AP_DAL_RangeFinder.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -106,6 +106,16 @@ void AP_DAL_RangeFinder_Backend::start_frame(AP_RangeFinder_Backend *backend) {
WRITE_REPLAY_BLOCK_IFCHANGED(RRNI, _RRNI, old);
}

bool AP_DAL_RangeFinder_Backend::on_ground() const
{
const auto *rangefinder = AP::dal().rangefinder();
if (rangefinder == nullptr) {
return false;
}
return RangeFinder::reading_on_ground((RangeFinder::Status)_RRNI.status, _RRNI.distance,
rangefinder->ground_clearance_orient(ROTATION_PITCH_270));
}

// return true if we have a range finder with the specified orientation
bool AP_DAL_RangeFinder::has_orientation(enum Rotation orientation) const
{
Expand Down
3 changes: 3 additions & 0 deletions libraries/AP_DAL/AP_DAL_RangeFinder.h
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,9 @@ class AP_DAL_RangeFinder_Backend {

const Vector3f &get_pos_offset() const { return _RRNI.pos_offset; }

// as AP_RangeFinder_Backend::on_ground(), from the logged reading and the downward ground clearance
bool on_ground() const;

// DAL methods:
void start_frame(AP_RangeFinder_Backend *backend);

Expand Down
15 changes: 13 additions & 2 deletions libraries/AP_DAL/LogStructure.h
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,8 @@
LOG_REVH_MSG, \
LOG_RWOH_MSG, \
LOG_RBOH_MSG, \
LOG_RTER_MSG
LOG_RTER_MSG, \
LOG_ROFM_MSG

// @LoggerMessage: RFRH
// @Description: Replay FRame Header
Expand Down Expand Up @@ -523,6 +524,14 @@ struct log_ROFH {
uint8_t _end;
};

// @LoggerMessage: ROFM
// @Description: Replay optical flow sensor metadata (low-rate, only logged when changed)
// @Field: HgtMin: height below which the sensor cannot focus, in meters
struct log_ROFM {
float minHeight;
uint8_t _end;
};

// @LoggerMessage: REPH
// @Description: Replay external position data
// @Field: PX: external position estimate, X-axis
Expand Down Expand Up @@ -709,4 +718,6 @@ struct log_RTER {
{ LOG_RBOH_MSG, RLOG_SIZE(RBOH), \
"RBOH", "ffffffffIfffH", "Q,DPX,DPY,DPZ,DAX,DAY,DAZ,DT,TS,OX,OY,OZ,D", "-------------", "-------------" }, \
{ LOG_RTER_MSG, RLOG_SIZE(RTER), \
"RTER", "f", "Alt", "m", "0" },
"RTER", "f", "Alt", "m", "0" }, \
{ LOG_ROFM_MSG, RLOG_SIZE(ROFM), \
"ROFM", "f", "HgtMin", "m", "0" },
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)
void NavEKF2::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride, float minHeight)
{
AP::dal().writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
// EKF2 does not use minHeight, but shares the replay record with EKF3 so must round-trip it
AP::dal().writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride, minHeight);

if (core) {
for (uint8_t i=0; i<num_cores; i++) {
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_NavEKF2/AP_NavEKF2.h
Original file line number Diff line number Diff line change
Expand Up @@ -183,7 +183,7 @@ 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);
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride, float minHeight);

// 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
6 changes: 3 additions & 3 deletions libraries/AP_NavEKF3/AP_NavEKF3.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1651,13 +1651,13 @@ bool NavEKF3::configuredToUseGPSForPos(void) const
// 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
#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 minHeight)
{
dal.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
dal.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride, minHeight);

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, minHeight);
}
}
}
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_NavEKF3/AP_NavEKF3.h
Original file line number Diff line number Diff line change
Expand Up @@ -198,7 +198,7 @@ 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);
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride, float minHeight);

// retrieve latest corrected optical flow samples (used for calibration)
bool getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const;
Expand Down
11 changes: 11 additions & 0 deletions libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,9 @@ void NavEKF3_core::controlFilterModes()
if (motorsArmed && !prevMotorsArmed) {
// set the time at which we arm to assist with checks
timeAtArming_ms = imuSampleTime_ms;
} else if (!motorsArmed && prevMotorsArmed) {
// a rest reading only holds for the flight it was taken before
flowFocusRestRng = 0;
}

// Detect if we are in flight on or ground
Expand Down Expand Up @@ -431,6 +434,7 @@ void NavEKF3_core::setAidingMode()
// this reduces the time required for the baro noise filter to settle before the filtered baro data can be used
meaHgtAtTakeOff = baroDataDelayed.hgt;
// reset the vertical position state to faster recover from baro errors experienced during touchdown
flowFocusRngPosD += -meaHgtAtTakeOff - stateStruct.position.z;
stateStruct.position.z = -meaHgtAtTakeOff;
// store the current height to be used to keep reporting
// the last known position
Expand Down Expand Up @@ -560,6 +564,13 @@ bool NavEKF3_core::readyToUseOptFlow(void) const
return false;
}

// flow the focus height check is discarding cannot aid, and without this a vehicle held below
// its floor leaves AID_RELATIVE on the timeout and re-enters on the next sample, every 5 s.
// At rest on the ground it fuses zero flow instead, which can aid
if (flowFocusBelow && !flowFocusResting) {
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
Loading