Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-10-06 09:02:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactor(ekf2): let optical flow sources read their own data and parameters

Browse Source
This commit is contained in:
Marco Hauswirth
2026-09-26 00:50:51 +02:00
parent f781fea29c
commit e15cc4e593
9 changed files with 243 additions and 299 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow.cpp
+107 -178
View File
File diff suppressed because it is too large Load Diff
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow.hpp
-103
View File
@@ -1,103 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#ifndef EKF_OPTICAL_FLOW_HPP
#define EKF_OPTICAL_FLOW_HPP
#include "../../common.h"
#if defined(CONFIG_EKF2_OPTICAL_FLOW) && defined(MODULE_NAME)
#include <drivers/drv_hrt.h>
#include <lib/parameters/param.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/topics/estimator_aid_source2d.h>
#include <uORB/topics/vehicle_optical_flow.h>
#include <uORB/topics/vehicle_optical_flow_vel.h>
class Ekf;
class OpticalFlow
{
public:
OpticalFlow()
{
for (uint8_t i = 0; i < MAX_OF_INSTANCES; i++) {
_slots[i].sub = uORB::Subscription(ORB_ID(vehicle_optical_flow), i);
}
}
void initParameters(Ekf &ekf);
void updateParameters(Ekf &ekf);
float maxEnabledDelayMs(Ekf &ekf) const;
void advertiseEnabledPublications(const Ekf &ekf);
void updateSamples(Ekf &ekf, ekf2_timestamps_s &ekf2_timestamps, const hrt_abstime &last_range_sensor_update);
void publishAidSourceStatus(const Ekf &ekf, const hrt_abstime &timestamp, uint8_t estimator_instance,
bool replay_mode);
void publishFlowVel(const Ekf &ekf, const hrt_abstime &timestamp, bool replay_mode);
private:
void resolveTuningHandles(uint8_t i);
struct ParamHandles {
param_t ctrl{PARAM_INVALID};
param_t delay{PARAM_INVALID};
param_t gyr_src{PARAM_INVALID};
param_t n_min{PARAM_INVALID};
param_t n_max{PARAM_INVALID};
param_t qmin{PARAM_INVALID};
param_t qmin_gnd{PARAM_INVALID};
param_t gate{PARAM_INVALID};
};
struct Slot {
uORB::Subscription sub{ORB_ID(vehicle_optical_flow)};
uORB::PublicationMulti<vehicle_optical_flow_vel_s> flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
uORB::PublicationMulti<estimator_aid_source2d_s> aid_src_pub{ORB_ID(estimator_aid_src_optical_flow)};
ParamHandles param_handles{};
hrt_abstime status_pub_last{};
hrt_abstime flow_vel_pub_last{};
};
Slot _slots[MAX_OF_INSTANCES] {};
#if defined(CONFIG_EKF2_RANGE_FINDER)
int8_t _range_instance {-1}; ///< first instance providing a distance, used as range finder fallback
#endif // CONFIG_EKF2_RANGE_FINDER
};
#endif // CONFIG_EKF2_OPTICAL_FLOW && MODULE_NAME
#endif // !EKF_OPTICAL_FLOW_HPP
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
+4
View File
@@ -42,6 +42,10 @@
void OpticalFlowAiding::update(Ekf &ekf, const imuSample &imu_delayed)
{
#if defined(MODULE_NAME)
updateSamples(ekf);
#endif // MODULE_NAME
bool any_ctrl_enabled = false;
for (uint8_t slot = 0; slot < MAX_OF_INSTANCES; slot++) {
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_source.hpp
+52 -1
View File
@@ -43,6 +43,12 @@
#include <mathlib/math/filter/AlphaFilter.hpp>
#include <uORB/topics/estimator_aid_source2d.h>
#if defined(MODULE_NAME)
# include <lib/parameters/param.h>
# include <uORB/Subscription.hpp>
# include <uORB/topics/vehicle_optical_flow.h>
#endif // MODULE_NAME
class Ekf;
class OpticalFlowSource
@@ -62,7 +68,14 @@ public:
~OpticalFlowSource() { delete _buffer; }
void setSlot(uint8_t slot) { _slot = slot; }
void setSlot(uint8_t slot)
{
_slot = slot;
#if defined(MODULE_NAME)
_sub = uORB::Subscription(ORB_ID(vehicle_optical_flow), slot);
initParams();
#endif // MODULE_NAME
}
void setData(const estimator::flowSample &sample, uint8_t buffer_length, uint64_t min_obs_interval_us, float dt_ekf_avg);
@@ -82,10 +95,36 @@ public:
void stop();
#if defined(MODULE_NAME)
// configured SENS_FLOW<slot>_DELAY, 0 while the slot is disabled
float delayMs() const;
#endif // MODULE_NAME
private:
friend class Ekf;
friend class OpticalFlowAiding;
#if defined(MODULE_NAME)
void initParams();
void updateParams();
// buffers a new vehicle_optical_flow sample of this slot
bool updateSample(Ekf &ekf, vehicle_optical_flow_s &optical_flow);
struct ParamHandles {
param_t ctrl{PARAM_INVALID};
param_t delay{PARAM_INVALID};
param_t gyr_src{PARAM_INVALID};
param_t n_min{PARAM_INVALID};
param_t n_max{PARAM_INVALID};
param_t qmin{PARAM_INVALID};
param_t qmin_gnd{PARAM_INVALID};
param_t gate{PARAM_INVALID};
} _param_handles{};
uORB::Subscription _sub{ORB_ID(vehicle_optical_flow)};
#endif // MODULE_NAME
bool fuse(Ekf &ekf, matrix::Vector<float, estimator::State::size> &H, bool update_terrain);
void reset(Ekf &ekf);
@@ -158,8 +197,20 @@ public:
// combined limits of the slots currently delivering data, falling back to the primary slot
void getLimits(const Ekf &ekf, float &hagl_min, float &hagl_max, float &max_rate) const;
#if defined(MODULE_NAME)
void updateParams();
#endif // MODULE_NAME
private:
#if defined(MODULE_NAME)
void updateSamples(Ekf &ekf);
#endif // MODULE_NAME
OpticalFlowSource _sources[estimator::MAX_OF_INSTANCES] {};
#if defined(MODULE_NAME) && defined(CONFIG_EKF2_RANGE_FINDER)
int8_t _range_instance {-1}; ///< first instance providing a distance, used as range finder fallback
#endif // MODULE_NAME && CONFIG_EKF2_RANGE_FINDER
};
#endif // CONFIG_EKF2_OPTICAL_FLOW
src/modules/ekf2/EKF/ekf.cpp
+4
View File
@@ -452,6 +452,10 @@ void Ekf::updateParameters()
_aux_global_position.paramsUpdated();
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
#if defined(CONFIG_EKF2_OPTICAL_FLOW) && defined(MODULE_NAME)
_flow_aiding.updateParams();
#endif // CONFIG_EKF2_OPTICAL_FLOW && MODULE_NAME
}
template<typename T>
src/modules/ekf2/EKF/estimator_interface.cpp
+6
View File
@@ -288,6 +288,12 @@ void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)
return;
}
_time_last_range_sensor_data = _time_latest_us;
pushRangeData(range_sample);
}
void EstimatorInterface::pushRangeData(const sensor::rangeSample &range_sample)
{
// Allocate the required buffer size if not previously done
if (_range_buffer == nullptr) {
_range_buffer = new TimestampedRingBuffer<sensor::rangeSample>(_obs_buffer_length);
src/modules/ekf2/EKF/estimator_interface.h
+3
View File
@@ -374,8 +374,11 @@ protected:
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_RANGE_FINDER)
void pushRangeData(const sensor::rangeSample &range_sample);
TimestampedRingBuffer<sensor::rangeSample> *_range_buffer {nullptr};
uint64_t _time_last_range_buffer_push{0};
uint64_t _time_last_range_sensor_data{0}; ///< last sample from a range finder, the optical flow fallback excluded
sensor::SensorRangeFinder _range_sensor{};
RangeFinderConsistencyCheck _rng_consistency_check;
src/modules/ekf2/EKF2.cpp
+55 -15
View File
@@ -207,10 +207,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_abl_tau(_params->ekf2_abl_tau),
_param_ekf2_gyr_b_lim(_params->ekf2_gyr_b_lim)
{
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
_optical_flow.initParameters(_ekf);
#endif // CONFIG_EKF2_OPTICAL_FLOW
initFusionControl();
AdvertiseTopics();
}
@@ -352,7 +348,14 @@ void EKF2::AdvertiseTopics()
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
_optical_flow.advertiseEnabledPublications(_ekf);
for (uint8_t i = 0; i < MAX_OF_INSTANCES; i++) {
if (_ekf.flowSource(i).params.ctrl) {
_optical_flow_pubs[i].aid_src.advertise();
_optical_flow_pubs[i].vel.advertise();
}
}
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_RANGE_FINDER)
@@ -452,10 +455,6 @@ void EKF2::Run()
// update parameters from storage
updateParams();
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
_optical_flow.updateParameters(_ekf);
#endif // CONFIG_EKF2_OPTICAL_FLOW
initFusionControl();
VerifyParams();
@@ -798,9 +797,6 @@ void EKF2::Run()
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
UpdateExtVisionSample(ekf2_timestamps);
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
_optical_flow.updateSamples(_ekf, ekf2_timestamps, _last_range_sensor_update);
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_GNSS)
UpdateGpsSample(ekf2_timestamps);
#endif // CONFIG_EKF2_GNSS
@@ -864,7 +860,7 @@ void EKF2::Run()
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
_optical_flow.publishFlowVel(_ekf, now, _replay_mode);
PublishOpticalFlowVel(now);
#endif // CONFIG_EKF2_OPTICAL_FLOW
UpdateAccelCalibration(now);
@@ -966,7 +962,11 @@ void EKF2::VerifyParams()
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
delay_max = math::max(delay_max, _optical_flow.maxEnabledDelayMs(_ekf));
for (uint8_t i = 0; i < MAX_OF_INSTANCES; i++) {
delay_max = math::max(delay_max, _ekf.flowSource(i).delayMs());
}
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
@@ -1159,8 +1159,13 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
#endif // CONFIG_EKF2_AUXVEL
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
// optical flow
_optical_flow.publishAidSourceStatus(_ekf, timestamp, _instance, _replay_mode);
for (uint8_t i = 0; i < MAX_OF_INSTANCES; i++) {
PublishAidSourceStatus(timestamp, _ekf.aid_src_optical_flow(i), _optical_flow_pubs[i].aid_src_last,
_optical_flow_pubs[i].aid_src);
}
#endif // CONFIG_EKF2_OPTICAL_FLOW
}
@@ -2194,6 +2199,41 @@ void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
}
#endif // CONFIG_EKF2_WIND
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
{
for (uint8_t i = 0; i < MAX_OF_INSTANCES; i++) {
const hrt_abstime timestamp_sample = _ekf.aid_src_optical_flow(i).timestamp_sample;
if ((timestamp_sample != 0) && (timestamp_sample > _optical_flow_pubs[i].vel_last)) {
vehicle_optical_flow_vel_s flow_vel{};
flow_vel.timestamp_sample = timestamp_sample;
_ekf.getFlowVelBody(i).copyTo(flow_vel.vel_body);
_ekf.getFlowVelNE(i).copyTo(flow_vel.vel_ne);
_ekf.getFilteredFlowVelBody(i).copyTo(flow_vel.vel_body_filtered);
_ekf.getFilteredFlowVelNE(i).copyTo(flow_vel.vel_ne_filtered);
_ekf.getFlowUncompensated(i).copyTo(flow_vel.flow_rate_uncompensated);
_ekf.getFlowCompensated(i).copyTo(flow_vel.flow_rate_compensated);
_ekf.getFlowGyro(i).copyTo(flow_vel.gyro_rate);
_ekf.getFlowGyroBias(i).copyTo(flow_vel.gyro_bias);
_ekf.getFlowRefBodyRate(i).copyTo(flow_vel.ref_gyro);
flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_optical_flow_pubs[i].vel.publish(flow_vel);
_optical_flow_pubs[i].vel_last = timestamp_sample;
}
}
}
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_AIRSPEED)
void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
{
src/modules/ekf2/EKF2.hpp
+12 -2
View File
@@ -112,7 +112,7 @@
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
# include "EKF/aid_sources/optical_flow/optical_flow.hpp"
# include <uORB/topics/vehicle_optical_flow_vel.h>
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_RANGE_FINDER)
@@ -226,6 +226,9 @@ private:
void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
void PublishOpticalFlowVel(const hrt_abstime &timestamp);
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_MAGNETOMETER)
void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
#endif // CONFIG_EKF2_MAGNETOMETER
@@ -351,7 +354,14 @@ private:
#endif // CONFIG_EKF2_RANGING_BEACON
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
OpticalFlow _optical_flow {};
struct OpticalFlowPublications {
uORB::PublicationMulti<estimator_aid_source2d_s> aid_src{ORB_ID(estimator_aid_src_optical_flow)};
uORB::PublicationMulti<vehicle_optical_flow_vel_s> vel{ORB_ID(estimator_optical_flow_vel)};
hrt_abstime aid_src_last{0};
hrt_abstime vel_last{0};
};
OpticalFlowPublications _optical_flow_pubs[MAX_OF_INSTANCES] {};
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_BAROMETER)