forked from Archive/PX4-Autopilot
ekf2: add kconfig option to enable/disable optical flow fusion
This commit is contained in:
Daniel Agar
parent
9f8fa99d70
commit
0784901a66
|
|
@ -86,8 +86,6 @@ list(APPEND EKF_SRCS
|
|||
EKF/imu_down_sampler.cpp
|
||||
EKF/mag_control.cpp
|
||||
EKF/mag_fusion.cpp
|
||||
EKF/optical_flow_control.cpp
|
||||
EKF/optflow_fusion.cpp
|
||||
EKF/output_predictor.cpp
|
||||
EKF/range_finder_consistency_check.cpp
|
||||
EKF/range_height_control.cpp
|
||||
|
|
@ -124,6 +122,13 @@ if(CONFIG_EKF2_GNSS_YAW)
|
|||
list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
list(APPEND EKF_SRCS
|
||||
EKF/optical_flow_control.cpp
|
||||
EKF/optflow_fusion.cpp
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_SIDESLIP)
|
||||
list(APPEND EKF_SRCS EKF/sideslip_fusion.cpp)
|
||||
endif()
|
||||
|
|
|
|||
|
|
@ -51,8 +51,6 @@ list(APPEND EKF_SRCS
|
|||
imu_down_sampler.cpp
|
||||
mag_control.cpp
|
||||
mag_fusion.cpp
|
||||
optical_flow_control.cpp
|
||||
optflow_fusion.cpp
|
||||
output_predictor.cpp
|
||||
range_finder_consistency_check.cpp
|
||||
range_height_control.cpp
|
||||
|
|
@ -85,11 +83,17 @@ if(CONFIG_EKF2_EXTERNAL_VISION)
|
|||
)
|
||||
endif()
|
||||
|
||||
|
||||
if(CONFIG_EKF2_GNSS_YAW)
|
||||
list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
list(APPEND EKF_SRCS
|
||||
optical_flow_control.cpp
|
||||
optflow_fusion.cpp
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_SIDESLIP)
|
||||
list(APPEND EKF_SRCS sideslip_fusion.cpp)
|
||||
endif()
|
||||
|
|
|
|||
|
|
@ -310,7 +310,6 @@ struct parameters {
|
|||
float mag_delay_ms{0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec)
|
||||
float baro_delay_ms{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec)
|
||||
float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec)
|
||||
float flow_delay_ms{5.0f}; ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
|
||||
float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec)
|
||||
|
||||
// input noise
|
||||
|
|
@ -412,11 +411,15 @@ struct parameters {
|
|||
// gravity fusion
|
||||
float gravity_noise{1.0f}; ///< accelerometer measurement gaussian noise (m/s**2)
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
float flow_delay_ms{5.0f}; ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
|
||||
|
||||
// optical flow fusion
|
||||
float flow_noise{0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec)
|
||||
float flow_noise_qual_min{0.5f}; ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
|
||||
int32_t flow_qual_min{1}; ///< minimum acceptable quality integer from the flow sensor
|
||||
float flow_innov_gate{3.0f}; ///< optical flow fusion innovation consistency gate size (STD)
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
// these parameters control the strictness of GPS quality checks used to determine if the GPS is
|
||||
// good enough to set a local origin and commence aiding
|
||||
|
|
|
|||
|
|
@ -104,7 +104,11 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
|
|||
|
||||
// control use of observations for aiding
|
||||
controlMagFusion();
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
controlOpticalFlowFusion(imu_delayed);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
controlGpsFusion(imu_delayed);
|
||||
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
|
|
|
|||
|
|
@ -148,8 +148,10 @@ void Ekf::reset()
|
|||
resetEstimatorAidStatus(_aid_src_aux_vel);
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
resetEstimatorAidStatus(_aid_src_optical_flow);
|
||||
resetEstimatorAidStatus(_aid_src_terrain_optical_flow);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
}
|
||||
|
||||
bool Ekf::update()
|
||||
|
|
|
|||
|
|
@ -105,6 +105,7 @@ public:
|
|||
void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = math::max(_aid_src_aux_vel.test_ratio[0], _aid_src_aux_vel.test_ratio[1]); }
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
void getFlowInnov(float flow_innov[2]) const;
|
||||
void getFlowInnovVar(float flow_innov_var[2]) const;
|
||||
void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_optical_flow.test_ratio[0], _aid_src_optical_flow.test_ratio[1]); }
|
||||
|
|
@ -122,6 +123,10 @@ public:
|
|||
void getTerrainFlowInnovVar(float flow_innov_var[2]) const;
|
||||
void getTerrainFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]); }
|
||||
|
||||
const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
|
||||
const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; }
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
void getHeadingInnov(float &heading_innov) const
|
||||
{
|
||||
if (_control_status.flags.mag_hdg) {
|
||||
|
|
@ -494,9 +499,6 @@ public:
|
|||
const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
|
||||
const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; }
|
||||
|
||||
private:
|
||||
|
||||
// set the internal states and status to their default value
|
||||
|
|
@ -645,8 +647,10 @@ private:
|
|||
estimator_aid_source2d_s _aid_src_aux_vel{};
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
estimator_aid_source2d_s _aid_src_optical_flow{};
|
||||
estimator_aid_source2d_s _aid_src_terrain_optical_flow{};
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
// variables used for the GPS quality checks
|
||||
Vector3f _gps_pos_deriv_filt{}; ///< GPS NED position derivative (m/sec)
|
||||
|
|
@ -809,12 +813,6 @@ private:
|
|||
|
||||
void resetVerticalVelocityToZero();
|
||||
|
||||
// fuse optical flow line of sight rate measurements
|
||||
void updateOptFlow(estimator_aid_source2d_s &aid_src);
|
||||
void fuseOptFlow();
|
||||
float predictFlowRange();
|
||||
Vector2f predictFlowVelBody();
|
||||
|
||||
// horizontal and vertical position aid source
|
||||
void updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const;
|
||||
void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source1d_s &aid_src) const;
|
||||
|
|
@ -850,6 +848,7 @@ private:
|
|||
void stopHaglRngFusion();
|
||||
float getRngVar();
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
// update the terrain vertical position estimate using an optical flow measurement
|
||||
void controlHaglFlowFusion();
|
||||
void startHaglFlowFusion();
|
||||
|
|
@ -857,6 +856,13 @@ private:
|
|||
void stopHaglFlowFusion();
|
||||
void fuseFlowForTerrain(estimator_aid_source2d_s &flow);
|
||||
|
||||
// fuse optical flow line of sight rate measurements
|
||||
void updateOptFlow(estimator_aid_source2d_s &aid_src);
|
||||
void fuseOptFlow();
|
||||
float predictFlowRange();
|
||||
Vector2f predictFlowVelBody();
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
void controlHaglFakeFusion();
|
||||
|
||||
// reset the heading and magnetic field states using the declination and magnetometer measurements
|
||||
|
|
|
|||
|
|
@ -355,6 +355,7 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
|
|||
}
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
void Ekf::getFlowInnov(float flow_innov[2]) const
|
||||
{
|
||||
flow_innov[0] = _aid_src_optical_flow.innovation[0];
|
||||
|
|
@ -378,6 +379,7 @@ void Ekf::getTerrainFlowInnovVar(float flow_innov_var[2]) const
|
|||
flow_innov_var[0] = _aid_src_terrain_optical_flow.innovation_variance[0];
|
||||
flow_innov_var[1] = _aid_src_terrain_optical_flow.innovation_variance[1];
|
||||
}
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
// get the state vector at the delayed time horizon
|
||||
matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
|
||||
|
|
@ -519,10 +521,12 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
|
|||
if (_horizontal_deadreckon_time_exceeded) {
|
||||
float vel_err_conservative = 0.0f;
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
if (_control_status.flags.opt_flow) {
|
||||
float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
|
||||
vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm();
|
||||
}
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
if (_control_status.flags.gps) {
|
||||
vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm());
|
||||
|
|
@ -568,7 +572,6 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
|
|||
|
||||
// TODO : calculate visual odometry limits
|
||||
const bool relying_on_rangefinder = isOnlyActiveSourceOfVerticalPositionAiding(_control_status.flags.rng_hgt);
|
||||
const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow);
|
||||
|
||||
// Keep within range sensor limit when using rangefinder as primary height source
|
||||
if (relying_on_rangefinder) {
|
||||
|
|
@ -576,7 +579,10 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
|
|||
*hagl_max = rangefinder_hagl_max;
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
// Keep within flow AND range sensor limits when exclusively using optical flow
|
||||
const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow);
|
||||
|
||||
if (relying_on_optical_flow) {
|
||||
// Calculate optical flow limits
|
||||
const float flow_hagl_min = fmaxf(rangefinder_hagl_min, _flow_min_distance);
|
||||
|
|
@ -591,6 +597,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
|
|||
*hagl_min = flow_hagl_min;
|
||||
*hagl_max = flow_hagl_max;
|
||||
}
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
}
|
||||
|
||||
void Ekf::resetImuBias()
|
||||
|
|
@ -680,10 +687,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
|
|||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) {
|
||||
float of_vel = sqrtf(Vector2f(_aid_src_optical_flow.test_ratio).max());
|
||||
vel = math::max(of_vel, FLT_MIN);
|
||||
}
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
// return the combined vertical position innovation test ratio
|
||||
float hgt_sum = 0.f;
|
||||
|
|
@ -802,7 +811,10 @@ void Ekf::updateHorizontalDeadReckoningstatus()
|
|||
&& (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
|
||||
|| isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max));
|
||||
|
||||
const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max);
|
||||
bool optFlowAiding = false;
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
optFlowAiding = _control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
bool airDataAiding = false;
|
||||
|
||||
|
|
|
|||
|
|
@ -53,7 +53,9 @@ EstimatorInterface::~EstimatorInterface()
|
|||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
delete _airspeed_buffer;
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
delete _flow_buffer;
|
||||
#endif // _flow_buffer
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
delete _ext_vision_buffer;
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
|
@ -322,6 +324,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample)
|
|||
}
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
|
||||
{
|
||||
if (!_initialised) {
|
||||
|
|
@ -356,6 +359,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
|
|||
ECL_WARN("optical flow data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _flow_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
|
||||
|
|
@ -560,9 +564,11 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
|||
max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms);
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
if (_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) {
|
||||
max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
|
||||
}
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_params.ev_ctrl > 0) {
|
||||
|
|
@ -704,9 +710,11 @@ void EstimatorInterface::print_status()
|
|||
}
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
if (_flow_buffer) {
|
||||
printf("flow buffer: %d/%d (%d Bytes)\n", _flow_buffer->entries(), _flow_buffer->get_length(), _flow_buffer->get_total_size());
|
||||
}
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_ext_vision_buffer) {
|
||||
|
|
|
|||
|
|
@ -96,9 +96,19 @@ public:
|
|||
|
||||
void setRangeData(const rangeSample &range_sample);
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
// if optical flow sensor gyro delta angles are not available, set gyro_xyz vector fields to NaN and the EKF will use its internal delta angle data instead
|
||||
void setOpticalFlowData(const flowSample &flow);
|
||||
|
||||
// set sensor limitations reported by the optical flow sensor
|
||||
void set_optical_flow_limits(float max_flow_rate, float min_distance, float max_distance)
|
||||
{
|
||||
_flow_max_rate = max_flow_rate;
|
||||
_flow_min_distance = min_distance;
|
||||
_flow_max_distance = max_distance;
|
||||
}
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
// set external vision position and attitude data
|
||||
void setExtVisionData(const extVisionSample &evdata);
|
||||
|
|
@ -159,14 +169,6 @@ public:
|
|||
_range_sensor.setLimits(min_distance, max_distance);
|
||||
}
|
||||
|
||||
// set sensor limitations reported by the optical flow sensor
|
||||
void set_optical_flow_limits(float max_flow_rate, float min_distance, float max_distance)
|
||||
{
|
||||
_flow_max_rate = max_flow_rate;
|
||||
_flow_min_distance = min_distance;
|
||||
_flow_max_distance = max_distance;
|
||||
}
|
||||
|
||||
// the flags considered are opt_flow, gps, ev_vel and ev_pos
|
||||
bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const;
|
||||
|
||||
|
|
@ -302,8 +304,6 @@ protected:
|
|||
airspeedSample _airspeed_sample_delayed{};
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
||||
flowSample _flow_sample_delayed{};
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
extVisionSample _ev_sample_prev{};
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
|
@ -312,10 +312,16 @@ protected:
|
|||
|
||||
float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3)
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
RingBuffer<flowSample> *_flow_buffer{nullptr};
|
||||
|
||||
flowSample _flow_sample_delayed{};
|
||||
|
||||
// Sensor limitations
|
||||
float _flow_max_rate{1.0f}; ///< maximum angular flow rate that the optical flow sensor can measure (rad/s)
|
||||
float _flow_min_distance{0.0f}; ///< minimum distance that the optical flow sensor can operate at (m)
|
||||
float _flow_max_distance{10.f}; ///< maximum distance that the optical flow sensor can operate at (m)
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
bool _imu_updated{false}; // true if the ekf should update (completed downsampling process)
|
||||
bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized
|
||||
|
|
@ -365,7 +371,6 @@ protected:
|
|||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
RingBuffer<airspeedSample> *_airspeed_buffer{nullptr};
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
RingBuffer<flowSample> *_flow_buffer{nullptr};
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
RingBuffer<extVisionSample> *_ext_vision_buffer{nullptr};
|
||||
|
|
|
|||
|
|
@ -274,9 +274,19 @@ bool Ekf::shouldResetGpsFusion() const
|
|||
/* We are relying on aiding to constrain drift so after a specified time
|
||||
* with no aiding we need to do something
|
||||
*/
|
||||
const bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
|
||||
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
|
||||
&& isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max);
|
||||
bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
|
||||
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max);
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
|
||||
if (has_horizontal_aiding_timed_out) {
|
||||
// horizontal aiding hasn't timed out if optical flow still active
|
||||
if (_control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max)) {
|
||||
has_horizontal_aiding_timed_out = false;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
const bool is_reset_required = has_horizontal_aiding_timed_out
|
||||
|| isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
|
||||
|
|
|
|||
|
|
@ -45,7 +45,10 @@
|
|||
|
||||
void Ekf::initHagl()
|
||||
{
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
stopHaglFlowFusion();
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
stopHaglRngFusion();
|
||||
|
||||
// assume a ground clearance
|
||||
|
|
@ -66,7 +69,9 @@ void Ekf::runTerrainEstimator(const imuSample &imu_delayed)
|
|||
predictHagl(imu_delayed);
|
||||
|
||||
controlHaglRngFusion();
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
controlHaglFlowFusion();
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
controlHaglFakeFusion();
|
||||
|
||||
// constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2)
|
||||
|
|
@ -246,6 +251,7 @@ void Ekf::fuseHaglRng()
|
|||
}
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
void Ekf::controlHaglFlowFusion()
|
||||
{
|
||||
if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow)) {
|
||||
|
|
@ -392,6 +398,7 @@ void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow)
|
|||
//_aid_src_optical_flow.time_last_fuse = _time_delayed_us; // TODO: separate aid source status for OF terrain?
|
||||
_aid_src_optical_flow.fused = true;
|
||||
}
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
void Ekf::controlHaglFakeFusion()
|
||||
{
|
||||
|
|
|
|||
|
|
@ -63,7 +63,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
|||
_param_ekf2_mag_delay(_params->mag_delay_ms),
|
||||
_param_ekf2_baro_delay(_params->baro_delay_ms),
|
||||
_param_ekf2_gps_delay(_params->gps_delay_ms),
|
||||
_param_ekf2_of_delay(_params->flow_delay_ms),
|
||||
_param_ekf2_rng_delay(_params->range_delay_ms),
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
_param_ekf2_avel_delay(_params->auxvel_delay_ms),
|
||||
|
|
@ -145,10 +144,16 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
|||
_param_ekf2_ev_pos_z(_params->ev_pos_body(2)),
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
_param_ekf2_grav_noise(_params->gravity_noise),
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_param_ekf2_of_delay(_params->flow_delay_ms),
|
||||
_param_ekf2_of_n_min(_params->flow_noise),
|
||||
_param_ekf2_of_n_max(_params->flow_noise_qual_min),
|
||||
_param_ekf2_of_qmin(_params->flow_qual_min),
|
||||
_param_ekf2_of_gate(_params->flow_innov_gate),
|
||||
_param_ekf2_of_pos_x(_params->flow_pos_body(0)),
|
||||
_param_ekf2_of_pos_y(_params->flow_pos_body(1)),
|
||||
_param_ekf2_of_pos_z(_params->flow_pos_body(2)),
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
_param_ekf2_imu_pos_x(_params->imu_pos_body(0)),
|
||||
_param_ekf2_imu_pos_y(_params->imu_pos_body(1)),
|
||||
_param_ekf2_imu_pos_z(_params->imu_pos_body(2)),
|
||||
|
|
@ -158,9 +163,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
|||
_param_ekf2_rng_pos_x(_params->rng_pos_body(0)),
|
||||
_param_ekf2_rng_pos_y(_params->rng_pos_body(1)),
|
||||
_param_ekf2_rng_pos_z(_params->rng_pos_body(2)),
|
||||
_param_ekf2_of_pos_x(_params->flow_pos_body(0)),
|
||||
_param_ekf2_of_pos_y(_params->flow_pos_body(1)),
|
||||
_param_ekf2_of_pos_z(_params->flow_pos_body(2)),
|
||||
_param_ekf2_gbias_init(_params->switch_on_gyro_bias),
|
||||
_param_ekf2_abias_init(_params->switch_on_accel_bias),
|
||||
_param_ekf2_angerr_init(_params->initial_tilt_err),
|
||||
|
|
@ -217,7 +219,9 @@ EKF2::~EKF2()
|
|||
#endif // CONFIG_EKF2_AUXVEL
|
||||
perf_free(_msg_missed_magnetometer_perf);
|
||||
perf_free(_msg_missed_odometry_perf);
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
perf_free(_msg_missed_optical_flow_perf);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_MULTI_INSTANCE)
|
||||
|
|
@ -229,7 +233,9 @@ bool EKF2::multi_init(int imu, int mag)
|
|||
_estimator_innovation_test_ratios_pub.advertise();
|
||||
_estimator_innovation_variances_pub.advertise();
|
||||
_estimator_innovations_pub.advertise();
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_estimator_optical_flow_vel_pub.advertise();
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
_estimator_sensor_bias_pub.advertise();
|
||||
_estimator_states_pub.advertise();
|
||||
_estimator_status_flags_pub.advertise();
|
||||
|
|
@ -342,7 +348,9 @@ int EKF2::print_status()
|
|||
#endif // CONFIG_EKF2_AUXVEL
|
||||
perf_print_counter(_msg_missed_magnetometer_perf);
|
||||
perf_print_counter(_msg_missed_odometry_perf);
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
perf_print_counter(_msg_missed_optical_flow_perf);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(DEBUG_BUILD)
|
||||
_ekf.print_status();
|
||||
|
|
@ -645,7 +653,9 @@ void EKF2::Run()
|
|||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
UpdateExtVisionSample(ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
UpdateFlowSample(ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
UpdateGpsSample(ekf2_timestamps);
|
||||
UpdateMagSample(ekf2_timestamps);
|
||||
UpdateRangeSample(ekf2_timestamps);
|
||||
|
|
@ -674,7 +684,9 @@ void EKF2::Run()
|
|||
PublishInnovations(now);
|
||||
PublishInnovationTestRatios(now);
|
||||
PublishInnovationVariances(now);
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
PublishOpticalFlowVel(now);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
PublishStates(now);
|
||||
PublishStatus(now);
|
||||
PublishStatusFlags(now);
|
||||
|
|
@ -883,10 +895,12 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp)
|
|||
PublishAidSourceStatus(_ekf.aid_src_aux_vel(), _status_aux_vel_pub_last, _estimator_aid_src_aux_vel_pub);
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
// optical flow
|
||||
PublishAidSourceStatus(_ekf.aid_src_optical_flow(), _status_optical_flow_pub_last, _estimator_aid_src_optical_flow_pub);
|
||||
PublishAidSourceStatus(_ekf.aid_src_terrain_optical_flow(), _status_terrain_optical_flow_pub_last,
|
||||
_estimator_aid_src_terrain_optical_flow_pub);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
}
|
||||
|
||||
void EKF2::PublishAttitude(const hrt_abstime ×tamp)
|
||||
|
|
@ -1171,7 +1185,10 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp)
|
|||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
_ekf.getAuxVelInnov(innovations.aux_hvel);
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_ekf.getFlowInnov(innovations.flow);
|
||||
_ekf.getTerrainFlowInnov(innovations.terr_flow);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
_ekf.getHeadingInnov(innovations.heading);
|
||||
_ekf.getMagInnov(innovations.mag_field);
|
||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||
|
|
@ -1185,7 +1202,6 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp)
|
|||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
_ekf.getHaglInnov(innovations.hagl);
|
||||
_ekf.getHaglRateInnov(innovations.hagl_rate);
|
||||
_ekf.getTerrainFlowInnov(innovations.terr_flow);
|
||||
_ekf.getGravityInnov(innovations.gravity);
|
||||
// Not yet supported
|
||||
innovations.aux_vvel = NAN;
|
||||
|
|
@ -1202,7 +1218,9 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp)
|
|||
if (!_ekf.control_status_flags().in_air) {
|
||||
// TODO: move to run before publications
|
||||
_preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps);
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos);
|
||||
|
|
@ -1235,7 +1253,10 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp)
|
|||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
_ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]);
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_ekf.getFlowInnovRatio(test_ratios.flow[0]);
|
||||
_ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
_ekf.getHeadingInnovRatio(test_ratios.heading);
|
||||
_ekf.getMagInnovRatio(test_ratios.mag_field[0]);
|
||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||
|
|
@ -1249,7 +1270,6 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp)
|
|||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
_ekf.getHaglInnovRatio(test_ratios.hagl);
|
||||
_ekf.getHaglRateInnovRatio(test_ratios.hagl_rate);
|
||||
_ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]);
|
||||
_ekf.getGravityInnovRatio(test_ratios.gravity[0]);
|
||||
// Not yet supported
|
||||
test_ratios.aux_vvel = NAN;
|
||||
|
|
@ -1272,7 +1292,10 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp)
|
|||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
_ekf.getAuxVelInnovVar(variances.aux_hvel);
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_ekf.getFlowInnovVar(variances.flow);
|
||||
_ekf.getTerrainFlowInnovVar(variances.terr_flow);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
_ekf.getHeadingInnovVar(variances.heading);
|
||||
_ekf.getMagInnovVar(variances.mag_field);
|
||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||
|
|
@ -1286,7 +1309,6 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp)
|
|||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
_ekf.getHaglInnovVar(variances.hagl);
|
||||
_ekf.getHaglRateInnovVar(variances.hagl_rate);
|
||||
_ekf.getTerrainFlowInnovVar(variances.terr_flow);
|
||||
_ekf.getGravityInnovVar(variances.gravity);
|
||||
// Not yet supported
|
||||
variances.aux_vvel = NAN;
|
||||
|
|
@ -1711,6 +1733,7 @@ void EKF2::PublishWindEstimate(const hrt_abstime ×tamp)
|
|||
}
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp)
|
||||
{
|
||||
const hrt_abstime timestamp_sample = _ekf.aid_src_optical_flow().timestamp_sample;
|
||||
|
|
@ -1734,6 +1757,7 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp)
|
|||
_estimator_optical_flow_vel_pub.publish(flow_vel);
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
|
||||
{
|
||||
|
|
@ -2055,6 +2079,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
|
||||
{
|
||||
// EKF flow sample
|
||||
|
|
@ -2113,6 +2138,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
|
||||
return new_optical_flow;
|
||||
}
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -88,8 +88,6 @@
|
|||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_magnetometer.h>
|
||||
#include <uORB/topics/vehicle_odometry.h>
|
||||
#include <uORB/topics/vehicle_optical_flow.h>
|
||||
#include <uORB/topics/vehicle_optical_flow_vel.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/wind.h>
|
||||
#include <uORB/topics/yaw_estimator_status.h>
|
||||
|
|
@ -103,6 +101,11 @@
|
|||
# include <uORB/topics/landing_target_pose.h>
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
# include <uORB/topics/vehicle_optical_flow.h>
|
||||
# include <uORB/topics/vehicle_optical_flow_vel.h>
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
extern pthread_mutex_t ekf2_module_mutex;
|
||||
|
||||
class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
|
||||
|
|
@ -166,7 +169,9 @@ private:
|
|||
void PublishLocalPosition(const hrt_abstime ×tamp);
|
||||
void PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu_sample);
|
||||
void PublishOdometryAligned(const hrt_abstime ×tamp, const vehicle_odometry_s &ev_odom);
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
void PublishOpticalFlowVel(const hrt_abstime ×tamp);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
void PublishSensorBias(const hrt_abstime ×tamp);
|
||||
void PublishStates(const hrt_abstime ×tamp);
|
||||
void PublishStatus(const hrt_abstime ×tamp);
|
||||
|
|
@ -181,8 +186,12 @@ private:
|
|||
void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps);
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
|
||||
void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
|
||||
void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
|
||||
|
|
@ -244,7 +253,6 @@ private:
|
|||
perf_counter_t _msg_missed_gps_perf{nullptr};
|
||||
perf_counter_t _msg_missed_magnetometer_perf{nullptr};
|
||||
perf_counter_t _msg_missed_odometry_perf{nullptr};
|
||||
perf_counter_t _msg_missed_optical_flow_perf{nullptr};
|
||||
|
||||
// Used to control saving of mag declination to be used on next startup
|
||||
bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved
|
||||
|
|
@ -319,8 +327,16 @@ private:
|
|||
perf_counter_t _msg_missed_landing_target_pose_perf{nullptr};
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
uORB::Subscription _vehicle_optical_flow_sub {ORB_ID(vehicle_optical_flow)};
|
||||
uORB::PublicationMulti<vehicle_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
|
||||
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)};
|
||||
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_terrain_optical_flow_pub{ORB_ID(estimator_aid_src_terrain_optical_flow)};
|
||||
|
||||
hrt_abstime _status_optical_flow_pub_last{0};
|
||||
hrt_abstime _status_terrain_optical_flow_pub_last{0};
|
||||
perf_counter_t _msg_missed_optical_flow_perf{nullptr};
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
float _last_baro_bias_published{};
|
||||
float _last_gnss_hgt_bias_published{};
|
||||
|
|
@ -356,7 +372,6 @@ private:
|
|||
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
|
||||
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
|
||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Subscription _vehicle_optical_flow_sub{ORB_ID(vehicle_optical_flow)};
|
||||
|
||||
uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
|
||||
|
|
@ -394,7 +409,6 @@ private:
|
|||
uORB::PublicationMulti<estimator_states_s> _estimator_states_pub{ORB_ID(estimator_states)};
|
||||
uORB::PublicationMulti<estimator_status_flags_s> _estimator_status_flags_pub{ORB_ID(estimator_status_flags)};
|
||||
uORB::PublicationMulti<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
|
||||
uORB::PublicationMulti<vehicle_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
|
||||
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_baro_hgt_pub {ORB_ID(estimator_aid_src_baro_hgt)};
|
||||
|
|
@ -415,9 +429,6 @@ private:
|
|||
|
||||
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)};
|
||||
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_terrain_optical_flow_pub{ORB_ID(estimator_aid_src_terrain_optical_flow)};
|
||||
|
||||
// publications with topic dependent on multi-mode
|
||||
uORB::PublicationMulti<vehicle_attitude_s> _attitude_pub;
|
||||
uORB::PublicationMulti<vehicle_local_position_s> _local_position_pub;
|
||||
|
|
@ -442,8 +453,6 @@ private:
|
|||
_param_ekf2_baro_delay, ///< barometer height measurement delay relative to the IMU (mSec)
|
||||
(ParamExtFloat<px4::params::EKF2_GPS_DELAY>)
|
||||
_param_ekf2_gps_delay, ///< GPS measurement delay relative to the IMU (mSec)
|
||||
(ParamExtFloat<px4::params::EKF2_OF_DELAY>)
|
||||
_param_ekf2_of_delay, ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_DELAY>)
|
||||
_param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec)
|
||||
|
||||
|
|
@ -606,15 +615,25 @@ private:
|
|||
(ParamExtFloat<px4::params::EKF2_GRAV_NOISE>)
|
||||
_param_ekf2_grav_noise, ///< default accelerometer noise for gravity fusion measurements (m/s**2)
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
// optical flow fusion
|
||||
(ParamExtFloat<px4::params::EKF2_OF_DELAY>)
|
||||
_param_ekf2_of_delay, ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
|
||||
(ParamExtFloat<px4::params::EKF2_OF_N_MIN>)
|
||||
_param_ekf2_of_n_min, ///< best quality observation noise for optical flow LOS rate measurements (rad/sec)
|
||||
_param_ekf2_of_n_min, ///< best quality observation noise for optical flow LOS rate measurements (rad/sec)
|
||||
(ParamExtFloat<px4::params::EKF2_OF_N_MAX>)
|
||||
_param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec)
|
||||
_param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec)
|
||||
(ParamExtInt<px4::params::EKF2_OF_QMIN>)
|
||||
_param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor
|
||||
_param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor
|
||||
(ParamExtFloat<px4::params::EKF2_OF_GATE>)
|
||||
_param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD)
|
||||
_param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD)
|
||||
(ParamExtFloat<px4::params::EKF2_OF_POS_X>)
|
||||
_param_ekf2_of_pos_x, ///< X position of optical flow sensor focal point in body frame (m)
|
||||
(ParamExtFloat<px4::params::EKF2_OF_POS_Y>)
|
||||
_param_ekf2_of_pos_y, ///< Y position of optical flow sensor focal point in body frame (m)
|
||||
(ParamExtFloat<px4::params::EKF2_OF_POS_Z>)
|
||||
_param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m)
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
// sensor positions in body frame
|
||||
(ParamExtFloat<px4::params::EKF2_IMU_POS_X>) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m)
|
||||
|
|
@ -626,12 +645,6 @@ private:
|
|||
(ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m)
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_POS_Y>) _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m)
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_POS_Z>) _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m)
|
||||
(ParamExtFloat<px4::params::EKF2_OF_POS_X>)
|
||||
_param_ekf2_of_pos_x, ///< X position of optical flow sensor focal point in body frame (m)
|
||||
(ParamExtFloat<px4::params::EKF2_OF_POS_Y>)
|
||||
_param_ekf2_of_pos_y, ///< Y position of optical flow sensor focal point in body frame (m)
|
||||
(ParamExtFloat<px4::params::EKF2_OF_POS_Z>)
|
||||
_param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m)
|
||||
|
||||
// output predictor filter time constants
|
||||
(ParamFloat<px4::params::EKF2_TAU_VEL>)
|
||||
|
|
|
|||
|
|
@ -56,6 +56,13 @@ depends on MODULES_EKF2
|
|||
---help---
|
||||
EKF2 GNSS yaw fusion support.
|
||||
|
||||
menuconfig EKF2_OPTICAL_FLOW
|
||||
depends on MODULES_EKF2
|
||||
bool "optical flow fusion support"
|
||||
default y
|
||||
---help---
|
||||
EKF2 optical flow fusion support.
|
||||
|
||||
menuconfig EKF2_SIDESLIP
|
||||
depends on MODULES_EKF2
|
||||
bool "sideslip fusion support"
|
||||
|
|
|
|||
|
|
@ -82,6 +82,8 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_
|
|||
has_failed |= checkInnov2DFailed(vel_ne_innov_lpf, vel_ne_innov, _vel_innov_test_lim, _vel_innov_spike_lim);
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
|
||||
if (_is_using_flow_aiding) {
|
||||
const Vector2f flow_innov = Vector2f(innov.flow);
|
||||
Vector2f flow_innov_lpf;
|
||||
|
|
@ -90,6 +92,7 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_
|
|||
has_failed |= checkInnov2DFailed(flow_innov_lpf, flow_innov, _flow_innov_test_lim, 5.f * _flow_innov_spike_lim);
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
return has_failed;
|
||||
}
|
||||
|
||||
|
|
@ -143,7 +146,6 @@ bool PreFlightChecker::checkInnov2DFailed(const Vector2f &innov_lpf, const Vecto
|
|||
void PreFlightChecker::reset()
|
||||
{
|
||||
_is_using_gps_aiding = false;
|
||||
_is_using_flow_aiding = false;
|
||||
_is_using_ev_pos_aiding = false;
|
||||
_is_using_ev_vel_aiding = false;
|
||||
_is_using_baro_hgt_aiding = false;
|
||||
|
|
@ -162,6 +164,11 @@ void PreFlightChecker::reset()
|
|||
_filter_rng_hgt_innov.reset();
|
||||
_filter_ev_hgt_innov.reset();
|
||||
_filter_heading_innov.reset();
|
||||
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_is_using_flow_aiding = false;
|
||||
_filter_flow_x_innov.reset();
|
||||
_filter_flow_y_innov.reset();
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
}
|
||||
|
|
|
|||
|
|
@ -75,7 +75,9 @@ public:
|
|||
void setVehicleCanObserveHeadingInFlight(bool val) { _can_observe_heading_in_flight = val; }
|
||||
|
||||
void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; }
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; }
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; }
|
||||
void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; }
|
||||
|
||||
|
|
@ -150,7 +152,6 @@ private:
|
|||
|
||||
bool _can_observe_heading_in_flight{};
|
||||
bool _is_using_gps_aiding{};
|
||||
bool _is_using_flow_aiding{};
|
||||
bool _is_using_ev_pos_aiding{};
|
||||
bool _is_using_ev_vel_aiding{};
|
||||
|
||||
|
|
@ -164,8 +165,6 @@ private:
|
|||
InnovationLpf _filter_vel_e_innov; ///< Preflight low pass filter E axis velocity innovations (m/sec)
|
||||
InnovationLpf _filter_vel_d_innov; ///< Preflight low pass filter D axis velocity innovations (m/sec)
|
||||
InnovationLpf _filter_heading_innov; ///< Preflight low pass filter heading innovation magntitude (rad)
|
||||
InnovationLpf _filter_flow_x_innov; ///< Preflight low pass filter optical flow innovation (rad)
|
||||
InnovationLpf _filter_flow_y_innov; ///< Preflight low pass filter optical flow innovation (rad)
|
||||
|
||||
// Preflight low pass filter height innovation (m)
|
||||
InnovationLpf _filter_baro_hgt_innov;
|
||||
|
|
@ -173,6 +172,18 @@ private:
|
|||
InnovationLpf _filter_rng_hgt_innov;
|
||||
InnovationLpf _filter_ev_hgt_innov;
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
bool _is_using_flow_aiding {};
|
||||
InnovationLpf _filter_flow_x_innov; ///< Preflight low pass filter optical flow innovation (rad)
|
||||
InnovationLpf _filter_flow_y_innov; ///< Preflight low pass filter optical flow innovation (rad)
|
||||
|
||||
// Maximum permissible flow innovation to pass pre-flight checks
|
||||
static constexpr float _flow_innov_test_lim = 0.5f;
|
||||
|
||||
// Preflight flow innovation spike limit (rad)
|
||||
static constexpr float _flow_innov_spike_lim = 2.0f * _flow_innov_test_lim;
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
// Preflight low pass filter time constant inverse (1/sec)
|
||||
static constexpr float _innov_lpf_tau_inv = 0.2f;
|
||||
// Maximum permissible velocity innovation to pass pre-flight checks (m/sec)
|
||||
|
|
@ -183,13 +194,11 @@ private:
|
|||
static constexpr float _nav_heading_innov_test_lim = 0.25f;
|
||||
// Maximum permissible yaw innovation to pass pre-flight checks when not aiding inertial nav using NE frame observations (rad)
|
||||
static constexpr float _heading_innov_test_lim = 0.52f;
|
||||
// Maximum permissible flow innovation to pass pre-flight checks
|
||||
static constexpr float _flow_innov_test_lim = 0.5f;
|
||||
|
||||
// Preflight velocity innovation spike limit (m/sec)
|
||||
static constexpr float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim;
|
||||
// Preflight position innovation spike limit (m)
|
||||
static constexpr float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim;
|
||||
// Preflight flow innovation spike limit (rad)
|
||||
static constexpr float _flow_innov_spike_lim = 2.0f * _flow_innov_test_lim;
|
||||
|
||||
};
|
||||
#endif // !EKF2_PREFLIGHTCHECKER_HPP
|
||||
|
|
|
|||
Loading…
Reference in New Issue