From 639d1ddca2c29ca25e2a5d039079d3190189ebf0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 14 Nov 2022 11:35:40 -0500 Subject: [PATCH] ekf2: update flags (in_air, at_rest, etc) on delayed time horizon (#20576) - in a lot of cases this won't really matter, although you can see it in logs (especially with things like vehicle_at_rest), but it's something we might as well do properly Co-authored-by: Mathieu Bresciani --- src/modules/ekf2/EKF/common.h | 8 ++ src/modules/ekf2/EKF/control.cpp | 16 ++++ src/modules/ekf2/EKF/estimator_interface.cpp | 36 ++++++++ src/modules/ekf2/EKF/estimator_interface.h | 10 +- src/modules/ekf2/EKF2.cpp | 96 ++++++++++---------- src/modules/ekf2/EKF2.hpp | 1 + 6 files changed, 117 insertions(+), 50 deletions(-) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 7598af6182..c4964787be 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -254,6 +254,14 @@ struct auxVelSample { Vector2f velVar{}; ///< estimated error variance of the NE velocity (m/sec)**2 }; +struct systemFlagUpdate { + uint64_t time_us{}; + bool at_rest{false}; + bool in_air{true}; + bool is_fixed_wing{false}; + bool gnd_effect{false}; +}; + struct stateSample { Quatf quat_nominal{}; ///< quaternion defining the rotation from body to earth frame Vector3f vel{}; ///< NED velocity in earth frame in m/s diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 3ae57ef44b..e994c09c4e 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -48,6 +48,22 @@ void Ekf::controlFusionModes() // Store the status to enable change detection _control_status_prev.value = _control_status.value; + if (_system_flag_buffer) { + systemFlagUpdate system_flags_delayed; + + if (_system_flag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &system_flags_delayed)) { + + set_vehicle_at_rest(system_flags_delayed.at_rest); + set_in_air_status(system_flags_delayed.in_air); + + set_is_fixed_wing(system_flags_delayed.is_fixed_wing); + + if (system_flags_delayed.gnd_effect) { + set_gnd_effect(); + } + } + } + // monitor the tilt alignment if (!_control_status.flags.tilt_align) { // whilst we are aligning the tilt, monitor the variances diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index bdecf7d1c2..1dc892a2fd 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -410,6 +410,42 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample) } } +void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags) +{ + if (!_initialised) { + return; + } + + // Allocate the required buffer size if not previously done + if (_system_flag_buffer == nullptr) { + _system_flag_buffer = new RingBuffer(_obs_buffer_length); + + if (_system_flag_buffer == nullptr || !_system_flag_buffer->valid()) { + delete _system_flag_buffer; + _system_flag_buffer = nullptr; + printBufferAllocationFailed("system flag"); + return; + } + } + + _system_flag_buffer->push(system_flags); + + const int64_t time_us = system_flags.time_us + - static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 + + // limit data rate to prevent data being lost + if (time_us >= static_cast(_system_flag_buffer->get_newest().time_us + _min_obs_interval_us)) { + + systemFlagUpdate system_flags_new{system_flags}; + system_flags_new.time_us = time_us; + + _system_flag_buffer->push(system_flags_new); + + } else { + ECL_WARN("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us); + } +} + void EstimatorInterface::setDragData(const imuSample &imu) { // down-sample the drag specific force data by accumulating and calculating the mean when diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 23bee11ca0..edbd29776c 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -101,6 +101,8 @@ public: void setAuxVelData(const auxVelSample &auxvel_sample); + void setSystemFlagData(const systemFlagUpdate &system_flags); + // return a address to the parameters struct // in order to give access to the application parameters *getParamHandle() { return &_params; } @@ -355,9 +357,10 @@ protected: uint64_t _time_last_in_air{0}; ///< last time we were in air (uSec) // data buffer instances - RingBuffer _imu_buffer{12}; // buffer length 12 with default parameters - RingBuffer _output_buffer{12}; - RingBuffer _output_vert_buffer{12}; + static constexpr uint8_t kBufferLengthDefault = 12; + RingBuffer _imu_buffer{kBufferLengthDefault}; + RingBuffer _output_buffer{kBufferLengthDefault}; + RingBuffer _output_vert_buffer{kBufferLengthDefault}; RingBuffer *_gps_buffer{nullptr}; RingBuffer *_mag_buffer{nullptr}; @@ -368,6 +371,7 @@ protected: RingBuffer *_ext_vision_buffer{nullptr}; RingBuffer *_drag_buffer{nullptr}; RingBuffer *_auxvel_buffer{nullptr}; + RingBuffer *_system_flag_buffer{nullptr}; uint64_t _time_last_gps_buffer_push{0}; uint64_t _time_last_gps_yaw_buffer_push{0}; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index cf1aacd056..8bac7f06c1 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -530,53 +530,6 @@ void EKF2::Run() _last_time_slip_us = 0; } - // update all other topics if they have new data - if (_status_sub.updated()) { - vehicle_status_s vehicle_status; - - if (_status_sub.copy(&vehicle_status)) { - const bool is_fixed_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING); - - // let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind) - _ekf.set_is_fixed_wing(is_fixed_wing); - - _preflt_checker.setVehicleCanObserveHeadingInFlight(vehicle_status.vehicle_type != - vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); - } - } - - if (_vehicle_land_detected_sub.updated()) { - vehicle_land_detected_s vehicle_land_detected; - - if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { - - _ekf.set_vehicle_at_rest(vehicle_land_detected.at_rest); - - if (vehicle_land_detected.in_ground_effect) { - _ekf.set_gnd_effect(); - } - - const bool was_in_air = _ekf.control_status_flags().in_air; - const bool in_air = !vehicle_land_detected.landed; - - if (!was_in_air && in_air) { - // takeoff - _ekf.set_in_air_status(true); - - // reset learned sensor calibrations on takeoff - _accel_cal = {}; - _gyro_cal = {}; - _mag_cal = {}; - - _preflt_checker.reset(); - - } else if (was_in_air && !in_air) { - // landed - _ekf.set_in_air_status(false); - } - } - } - // ekf2_timestamps (using 0.1 ms relative timestamps) ekf2_timestamps_s ekf2_timestamps { .timestamp = now, @@ -595,6 +548,7 @@ void EKF2::Run() UpdateGpsSample(ekf2_timestamps); UpdateMagSample(ekf2_timestamps); UpdateRangeSample(ekf2_timestamps); + UpdateSystemFlagsSample(ekf2_timestamps); vehicle_odometry_s ev_odom; const bool new_ev_odom = UpdateExtVisionSample(ekf2_timestamps, ev_odom); @@ -1037,6 +991,11 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _estimator_innovations_pub.publish(innovations); // calculate noise filtered velocity innovations which are used for pre-flight checking + if (_ekf.control_status_prev_flags().in_air != _ekf.control_status_flags().in_air) { + // fully reset on takeoff or landing + _preflt_checker.reset(); + } + if (!_ekf.control_status_flags().in_air) { // TODO: move to run before publications _preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps); @@ -1049,6 +1008,8 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _preflt_checker.setUsingRngHgtAiding(_ekf.control_status_flags().rng_hgt); _preflt_checker.setUsingEvHgtAiding(_ekf.control_status_flags().ev_hgt); + _preflt_checker.setVehicleCanObserveHeadingInFlight(_ekf.control_status_flags().fixed_wing); + _preflt_checker.update(_ekf.get_imu_sample_delayed().delta_ang_dt, innovations); } } @@ -2107,9 +2068,50 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) } } +void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps) +{ + // EKF system flags + if (_status_sub.updated() || _vehicle_land_detected_sub.updated()) { + + systemFlagUpdate flags{}; + flags.time_us = ekf2_timestamps.timestamp; + + // vehicle_status + vehicle_status_s vehicle_status; + + if (_status_sub.copy(&vehicle_status) + && (ekf2_timestamps.timestamp < vehicle_status.timestamp + 3_s)) { + + // initially set in_air from arming_state (will be overridden if land detector is available) + flags.in_air = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + + // let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind) + flags.is_fixed_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING); + } + + // vehicle_land_detected + vehicle_land_detected_s vehicle_land_detected; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected) + && (ekf2_timestamps.timestamp < vehicle_land_detected.timestamp + 3_s)) { + + flags.at_rest = vehicle_land_detected.at_rest; + flags.in_air = !vehicle_land_detected.landed; + flags.gnd_effect = vehicle_land_detected.in_ground_effect; + } + + _ekf.setSystemFlagData(flags); + } +} + void EKF2::UpdateCalibration(const hrt_abstime ×tamp, InFlightCalibration &cal, const matrix::Vector3f &bias, const matrix::Vector3f &bias_variance, float bias_limit, bool bias_valid, bool learning_valid) { + // reset existing cal on takeoff + if (!_ekf.control_status_prev_flags().in_air && _ekf.control_status_flags().in_air) { + cal = {}; + } + // Check if conditions are OK for learning of accelerometer bias values // the EKF is operating in the correct mode and there are no filter faults static constexpr float max_var_allowed = 1e-3f; diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 48627e79a0..227fa9ebe4 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -170,6 +170,7 @@ private: void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps); + void UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps); // Used to check, save and use learned accel/gyro/mag biases struct InFlightCalibration {