From f907c4a0a16b908ee530a485dcdca2a550bae30a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 5 Oct 2023 14:58:17 -0400 Subject: [PATCH] ekf: merge backend classes EstimatorInterface + Ekf --- src/modules/ekf2/EKF/airspeed_fusion.cpp | 35 ++ src/modules/ekf2/EKF/auxvel_fusion.cpp | 35 ++ src/modules/ekf2/EKF/baro_height_control.cpp | 36 ++ src/modules/ekf2/EKF/drag_fusion.cpp | 62 ++ src/modules/ekf2/EKF/ekf.h | 531 +++++++++++++++--- src/modules/ekf2/EKF/estimator_interface.cpp | 518 ++--------------- src/modules/ekf2/EKF/estimator_interface.h | 489 ---------------- src/modules/ekf2/EKF/ev_control.cpp | 37 ++ src/modules/ekf2/EKF/gps_control.cpp | 79 +++ src/modules/ekf2/EKF/mag_control.cpp | 36 ++ src/modules/ekf2/EKF/optical_flow_control.cpp | 35 ++ src/modules/ekf2/EKF/range_height_control.cpp | 36 ++ src/modules/ekf2/EKF2.cpp | 2 +- .../ekf2/test/sensor_simulator/ekf_logger.h | 1 - .../ekf2/test/sensor_simulator/ekf_wrapper.h | 1 - 15 files changed, 910 insertions(+), 1023 deletions(-) delete mode 100644 src/modules/ekf2/EKF/estimator_interface.h diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 5ad89e76cb..63f1ade137 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -49,6 +49,41 @@ #include +void Ekf::setAirspeedData(const airspeedSample &airspeed_sample) +{ + if (!_initialised) { + return; + } + + // Allocate the required buffer size if not previously done + if (_airspeed_buffer == nullptr) { + _airspeed_buffer = new RingBuffer(_obs_buffer_length); + + if (_airspeed_buffer == nullptr || !_airspeed_buffer->valid()) { + delete _airspeed_buffer; + _airspeed_buffer = nullptr; + printBufferAllocationFailed("airspeed"); + return; + } + } + + const int64_t time_us = airspeed_sample.time_us + - static_cast(_params.airspeed_delay_ms * 1000) + - 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(_airspeed_buffer->get_newest().time_us + _min_obs_interval_us)) { + + airspeedSample airspeed_sample_new{airspeed_sample}; + airspeed_sample_new.time_us = time_us; + + _airspeed_buffer->push(airspeed_sample_new); + + } else { + ECL_WARN("airspeed data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _airspeed_buffer->get_newest().time_us, _min_obs_interval_us); + } +} + void Ekf::controlAirDataFusion(const imuSample &imu_delayed) { // control activation and initialisation/reset of wind states required for airspeed fusion diff --git a/src/modules/ekf2/EKF/auxvel_fusion.cpp b/src/modules/ekf2/EKF/auxvel_fusion.cpp index dbe88ccb87..d78cad3e45 100644 --- a/src/modules/ekf2/EKF/auxvel_fusion.cpp +++ b/src/modules/ekf2/EKF/auxvel_fusion.cpp @@ -33,6 +33,41 @@ #include "ekf.h" +void Ekf::setAuxVelData(const auxVelSample &auxvel_sample) +{ + if (!_initialised) { + return; + } + + // Allocate the required buffer size if not previously done + if (_auxvel_buffer == nullptr) { + _auxvel_buffer = new RingBuffer(_obs_buffer_length); + + if (_auxvel_buffer == nullptr || !_auxvel_buffer->valid()) { + delete _auxvel_buffer; + _auxvel_buffer = nullptr; + printBufferAllocationFailed("aux vel"); + return; + } + } + + const int64_t time_us = auxvel_sample.time_us + - static_cast(_params.auxvel_delay_ms * 1000) + - 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(_auxvel_buffer->get_newest().time_us + _min_obs_interval_us)) { + + auxVelSample auxvel_sample_new{auxvel_sample}; + auxvel_sample_new.time_us = time_us; + + _auxvel_buffer->push(auxvel_sample_new); + + } else { + ECL_WARN("aux velocity data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _auxvel_buffer->get_newest().time_us, _min_obs_interval_us); + } +} + void Ekf::controlAuxVelFusion() { if (_auxvel_buffer) { diff --git a/src/modules/ekf2/EKF/baro_height_control.cpp b/src/modules/ekf2/EKF/baro_height_control.cpp index f3c5b0df64..70d71e3cbf 100644 --- a/src/modules/ekf2/EKF/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/baro_height_control.cpp @@ -38,6 +38,42 @@ #include "ekf.h" +void Ekf::setBaroData(const baroSample &baro_sample) +{ + if (!_initialised) { + return; + } + + // Allocate the required buffer size if not previously done + if (_baro_buffer == nullptr) { + _baro_buffer = new RingBuffer(_obs_buffer_length); + + if (_baro_buffer == nullptr || !_baro_buffer->valid()) { + delete _baro_buffer; + _baro_buffer = nullptr; + printBufferAllocationFailed("baro"); + return; + } + } + + const int64_t time_us = baro_sample.time_us + - static_cast(_params.baro_delay_ms * 1000) + - 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(_baro_buffer->get_newest().time_us + _min_obs_interval_us)) { + + baroSample baro_sample_new{baro_sample}; + baro_sample_new.time_us = time_us; + + _baro_buffer->push(baro_sample_new); + _time_last_baro_buffer_push = _time_latest_us; + + } else { + ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _baro_buffer->get_newest().time_us, _min_obs_interval_us); + } +} + void Ekf::controlBaroHeightFusion() { static constexpr const char *HGT_SRC_NAME = "baro"; diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index 7a715caeef..8d4b77a125 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -42,6 +42,68 @@ #include +void Ekf::setDragData(const imuSample &imu) +{ + // down-sample the drag specific force data by accumulating and calculating the mean when + // sufficient samples have been collected + if (_params.drag_ctrl > 0) { + + // Allocate the required buffer size if not previously done + if (_drag_buffer == nullptr) { + _drag_buffer = new RingBuffer(_obs_buffer_length); + + if (_drag_buffer == nullptr || !_drag_buffer->valid()) { + delete _drag_buffer; + _drag_buffer = nullptr; + printBufferAllocationFailed("drag"); + return; + } + } + + // don't use any accel samples that are clipping + if (imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]) { + // reset accumulators + _drag_sample_count = 0; + _drag_down_sampled.accelXY.zero(); + _drag_down_sampled.time_us = 0; + _drag_sample_time_dt = 0.0f; + + return; + } + + _drag_sample_count++; + // note acceleration is accumulated as a delta velocity + _drag_down_sampled.accelXY(0) += imu.delta_vel(0); + _drag_down_sampled.accelXY(1) += imu.delta_vel(1); + _drag_down_sampled.time_us += imu.time_us; + _drag_sample_time_dt += imu.delta_vel_dt; + + // calculate the downsample ratio for drag specific force data + uint8_t min_sample_ratio = (uint8_t) ceilf((float)_imu_buffer_length / _obs_buffer_length); + + if (min_sample_ratio < 5) { + min_sample_ratio = 5; + } + + // calculate and store means from accumulated values + if (_drag_sample_count >= min_sample_ratio) { + // note conversion from accumulated delta velocity to acceleration + _drag_down_sampled.accelXY(0) /= _drag_sample_time_dt; + _drag_down_sampled.accelXY(1) /= _drag_sample_time_dt; + _drag_down_sampled.time_us /= _drag_sample_count; + + // write to buffer + _drag_buffer->push(_drag_down_sampled); + + // reset accumulators + _drag_sample_count = 0; + _drag_down_sampled.accelXY.zero(); + _drag_down_sampled.time_us = 0; + _drag_sample_time_dt = 0.0f; + } + } +} + void Ekf::controlDragFusion(const imuSample &imu_delayed) { if ((_params.drag_ctrl > 0) && _drag_buffer) { diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index e40cc52c8e..59588bf612 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -43,7 +43,39 @@ #ifndef EKF_EKF_H #define EKF_EKF_H -#include "estimator_interface.h" +#if defined(MODULE_NAME) +#include +# define ECL_INFO PX4_DEBUG +# define ECL_WARN PX4_DEBUG +# define ECL_ERR PX4_DEBUG +# define ECL_DEBUG PX4_DEBUG +#else +# define ECL_INFO(X, ...) printf(X "\n", ##__VA_ARGS__) +# define ECL_WARN(X, ...) fprintf(stderr, X "\n", ##__VA_ARGS__) +# define ECL_ERR(X, ...) fprintf(stderr, X "\n", ##__VA_ARGS__) + +# if defined(DEBUG_BUILD) +# define ECL_DEBUG(X, ...) fprintf(stderr, X "\n", ##__VA_ARGS__) +# else +# define ECL_DEBUG(X, ...) +#endif + +#endif + +#include "common.h" +#include "RingBuffer.h" +#include "imu_down_sampler.hpp" +#include "output_predictor.h" + +#if defined(CONFIG_EKF2_RANGE_FINDER) +# include "range_finder_consistency_check.hpp" +# include "sensor_range_finder.hpp" +#endif // CONFIG_EKF2_RANGE_FINDER + +#include +#include +#include +#include #include "EKFGSF_yaw.h" #include "bias_estimator.hpp" @@ -55,9 +87,11 @@ #include #include +using namespace estimator; + enum class Likelihood { LOW, MEDIUM, HIGH }; -class Ekf final : public EstimatorInterface +class Ekf { public: typedef matrix::Vector VectorState; @@ -69,17 +103,84 @@ public: reset(); }; - virtual ~Ekf() = default; + ~Ekf(); + + // return a address to the parameters struct + // in order to give access to the application + parameters *getParamHandle() { return &_params; } // initialise variables to sane values (also interface class) - bool init(uint64_t timestamp) override; + bool init(uint64_t timestamp); + + void print_status(); // should be called every time new data is pushed into the filter bool update(); static uint8_t getNumberOfStates() { return State::size; } + // Getter for the average EKF update period in s + float get_dt_ekf_avg() const { return _dt_ekf_avg; } + + // Getters for samples on the delayed time horizon + const imuSample &get_imu_sample_delayed() const { return _imu_buffer.get_oldest(); } + const uint64_t &time_delayed_us() const { return _time_delayed_us; } + + // set vehicle landed status data + void set_in_air_status(bool in_air) + { + if (!in_air) { + if (_control_status.flags.in_air) { + ECL_DEBUG("no longer in air"); + } + + _time_last_on_ground_us = _time_delayed_us; + + } else { + if (!_control_status.flags.in_air) { + ECL_DEBUG("in air"); + } + + _time_last_in_air = _time_delayed_us; + } + + _control_status.flags.in_air = in_air; + } + + void set_vehicle_at_rest(bool at_rest) + { + if (!_control_status.flags.vehicle_at_rest && at_rest) { + ECL_DEBUG("at rest"); + + } else if (_control_status.flags.vehicle_at_rest && !at_rest) { + ECL_DEBUG("no longer at rest"); + } + + _control_status.flags.vehicle_at_rest = at_rest; + } + + // set air density used by the multi-rotor specific drag force fusion + void set_air_density(float air_density) { _air_density = air_density; } + + // set vehicle is fixed wing status + void set_is_fixed_wing(bool is_fixed_wing) { _control_status.flags.fixed_wing = is_fixed_wing; } + + // set flag if static pressure rise due to ground effect is expected + // use _params.gnd_effect_deadzone to adjust for expected rise in static pressure + // flag will clear after GNDEFFECT_TIMEOUT uSec + void set_gnd_effect() + { + _control_status.flags.gnd_effect = true; + _time_last_gnd_effect_on = _time_delayed_us; + } + + void setIMUData(const imuSample &imu_sample); + + void setSystemFlagData(const systemFlagUpdate &system_flags); + #if defined(CONFIG_EKF2_BAROMETER) + void setBaroData(const baroSample &baro_sample); + const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; } const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); } #endif // CONFIG_EKF2_BAROMETER @@ -115,6 +216,13 @@ public: #if defined(CONFIG_EKF2_RANGE_FINDER) // range height + void setRangeData(const rangeSample &range_sample); + + // set sensor limitations reported by the rangefinder + void set_rangefinder_limits(float min_distance, float max_distance) { _range_sensor.setLimits(min_distance, max_distance); } + + const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); } + const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); } const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; } @@ -124,6 +232,17 @@ public: #endif // CONFIG_EKF2_RANGE_FINDER #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; + } + const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; } const Vector2f &getFlowVelBody() const { return _flow_vel_body; } @@ -142,6 +261,7 @@ public: float getHeadingInnov() const { #if defined(CONFIG_EKF2_MAGNETOMETER) + if (_control_status.flags.mag_hdg) { return _aid_src_mag_heading.innovation; } @@ -149,18 +269,23 @@ public: if (_control_status.flags.mag_3D) { return Vector3f(_aid_src_mag.innovation).max(); } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) + if (_control_status.flags.gps_yaw) { return _aid_src_gnss_yaw.innovation; } + #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_yaw) { return _aid_src_ev_yaw.innovation; } + #endif // CONFIG_EKF2_EXTERNAL_VISION return 0.f; @@ -169,6 +294,7 @@ public: float getHeadingInnovVar() const { #if defined(CONFIG_EKF2_MAGNETOMETER) + if (_control_status.flags.mag_hdg) { return _aid_src_mag_heading.innovation_variance; } @@ -176,18 +302,23 @@ public: if (_control_status.flags.mag_3D) { return Vector3f(_aid_src_mag.innovation_variance).max(); } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) + if (_control_status.flags.gps_yaw) { return _aid_src_gnss_yaw.innovation_variance; } + #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_yaw) { return _aid_src_ev_yaw.innovation_variance; } + #endif // CONFIG_EKF2_EXTERNAL_VISION return 0.f; @@ -196,6 +327,7 @@ public: float getHeadingInnovRatio() const { #if defined(CONFIG_EKF2_MAGNETOMETER) + if (_control_status.flags.mag_hdg) { return _aid_src_mag_heading.test_ratio; } @@ -203,34 +335,37 @@ public: if (_control_status.flags.mag_3D) { return Vector3f(_aid_src_mag.test_ratio).max(); } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) + if (_control_status.flags.gps_yaw) { return _aid_src_gnss_yaw.test_ratio; } + #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_yaw) { return _aid_src_ev_yaw.test_ratio; } + #endif // CONFIG_EKF2_EXTERNAL_VISION return 0.f; } #if defined(CONFIG_EKF2_DRAG_FUSION) + void setDragData(const imuSample &imu); + const auto &aid_src_drag() const { return _aid_src_drag; } #endif // CONFIG_EKF2_DRAG_FUSION // get the state vector at the delayed time horizon matrix::Vector getStateAtFusionHorizonAsVector() const; - // get the wind velocity in m/s - const Vector2f &getWindVelocity() const { return _state.wind_vel; }; - Vector2f getWindVelocityVariance() const { return getStateVariance(); } - template matrix::VectorgetStateVariance() const { return P.slice(S.idx, S.idx).diag(); } // calling getStateCovariance().diag() uses more flash space @@ -243,13 +378,84 @@ public: // get the diagonal elements of the covariance matrix matrix::Vector covariances_diagonal() const { return P.diag(); } + // orientation state + const matrix::Quatf &getQuaternion() const { return _output_predictor.getQuaternion(); } matrix::Vector getQuaternionVariance() const { return getStateVariance(); } + bool attitude_valid() const { return _control_status.flags.tilt_align; } // return true if the attitude is usable + float getUnaidedYaw() const { return _output_predictor.getUnaidedYaw(); } + + // velocity state + Vector3f getVelocity() const { return _output_predictor.getVelocity(); } Vector3f getVelocityVariance() const { return getStateVariance(); }; + + // position state + Vector3f getPosition() const { return _output_predictor.getPosition(); } Vector3f getPositionVariance() const { return getStateVariance(); } + // gyro bias state + const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s + Vector3f getGyroBiasVariance() const { return getStateVariance(); } // get the gyroscope bias variance in rad/s + float getGyroBiasLimit() const { return _params.gyro_bias_lim; } + bool gyro_bias_inhibited() const { return _gyro_bias_inhibit[0] || _gyro_bias_inhibit[1] || _gyro_bias_inhibit[2]; } + + // accel bias state + const Vector3f &getAccelBias() const { return _state.accel_bias; } // get the accelerometer bias in m/s**2 + Vector3f getAccelBiasVariance() const { return getStateVariance(); } // get the accelerometer bias variance in m/s**2 + float getAccelBiasLimit() const { return _params.acc_bias_lim; } + bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; } + +#if defined(CONFIG_EKF2_MAGNETOMETER) + // mag I state + const Vector3f &getMagEarthField() const { return _state.mag_I; } + + // mag B state + const Vector3f &getMagBias() const { return _state.mag_B; } + Vector3f getMagBiasVariance() const + { + if (_control_status.flags.mag) { + return getStateVariance(); + } + + return _saved_mag_bf_covmat.diag(); + } + float getMagBiasLimit() const { return 0.5f; } // 0.5 Gauss +#endif // CONFIG_EKF2_MAGNETOMETER + + // wind state + const Vector2f &getWindVelocity() const { return _state.wind_vel; }; + Vector2f getWindVelocityVariance() const { return getStateVariance(); } + + // get EKF mode status + const filter_control_status_u &control_status() const { return _control_status; } + const decltype(filter_control_status_u::flags) &control_status_flags() const { return _control_status.flags; } + + const filter_control_status_u &control_status_prev() const { return _control_status_prev; } + const decltype(filter_control_status_u::flags) &control_status_prev_flags() const { return _control_status_prev.flags; } + + // get EKF internal fault status + const fault_status_u &fault_status() const { return _fault_status; } + const decltype(fault_status_u::flags) &fault_status_flags() const { return _fault_status.flags; } + + const innovation_fault_status_u &innov_check_fail_status() const { return _innov_check_fail_status; } + const decltype(innovation_fault_status_u::flags) &innov_check_fail_status_flags() const { return _innov_check_fail_status.flags; } + + const warning_event_status_u &warning_event_status() const { return _warning_events; } + const decltype(warning_event_status_u::flags) &warning_event_flags() const { return _warning_events.flags; } + void clear_warning_events() { _warning_events.value = 0; } + + const information_event_status_u &information_event_status() const { return _information_events; } + const decltype(information_event_status_u::flags) &information_event_flags() const { return _information_events.flags; } + void clear_information_events() { _information_events.value = 0; } + + + // output predictor + OutputPredictor &output_predictor() { return _output_predictor; }; + const Vector3f &getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); } + float getVerticalPositionDerivative() const { return _output_predictor.getVerticalPositionDerivative(); } + + const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); } + - // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined - bool collect_gps(const gpsMessage &gps) override; // get the ekf WGS-84 origin position and height and the system time it was last set // return true if the origin is valid @@ -259,6 +465,8 @@ public: float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; } bool setEkfGlobalOriginAltitude(const float altitude); + const bool &global_origin_valid() const { return _NED_origin_initialised; } + const MapProjection &global_origin() const { return _pos_ref; } // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const; @@ -277,13 +485,9 @@ public: void resetGyroBias(); void resetAccelBias(); - // First argument returns GPS drift metrics in the following array locations - // 0 : Horizontal position drift rate (m/s) - // 1 : Vertical position drift rate (m/s) - // 2 : Filtered horizontal velocity (m/s) - // Second argument returns true when IMU movement is blocking the drift calculation - // Function returns true if the metrics have been updated and not returned previously by this function - bool get_gps_drift_metrics(float drift[3], bool *blocked); + float gps_horizontal_position_drift_rate_m_s() const { return _gps_horizontal_position_drift_rate_m_s; } + float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; } + float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; } // return true if the global position estimate is valid // return true if the origin is set we are not doing unconstrained free inertial navigation @@ -323,34 +527,6 @@ public: #endif } - // gyro bias - const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s - Vector3f getGyroBiasVariance() const { return getStateVariance(); } // get the gyroscope bias variance in rad/s - float getGyroBiasLimit() const { return _params.gyro_bias_lim; } - - // accel bias - const Vector3f &getAccelBias() const { return _state.accel_bias; } // get the accelerometer bias in m/s**2 - Vector3f getAccelBiasVariance() const { return getStateVariance(); } // get the accelerometer bias variance in m/s**2 - float getAccelBiasLimit() const { return _params.acc_bias_lim; } - -#if defined(CONFIG_EKF2_MAGNETOMETER) - const Vector3f &getMagEarthField() const { return _state.mag_I; } - - const Vector3f &getMagBias() const { return _state.mag_B; } - Vector3f getMagBiasVariance() const - { - if (_control_status.flags.mag) { - return getStateVariance(); - } - - return _saved_mag_bf_covmat.diag(); - } - float getMagBiasLimit() const { return 0.5f; } // 0.5 Gauss -#endif // CONFIG_EKF2_MAGNETOMETER - - bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; } - bool gyro_bias_inhibited() const { return _gyro_bias_inhibit[0] || _gyro_bias_inhibit[1] || _gyro_bias_inhibit[2]; } - const auto &state_reset_status() const { return _state_reset_status; } // return the amount the local vertical position changed in the last reset and the number of reset events @@ -427,6 +603,8 @@ public: uint8_t getHeightSensorRef() const { return _height_sensor_ref; } #if defined(CONFIG_EKF2_AIRSPEED) + void setAirspeedData(const airspeedSample &airspeed_sample); + const auto &aid_src_airspeed() const { return _aid_src_airspeed; } #endif // CONFIG_EKF2_AIRSPEED @@ -438,6 +616,8 @@ public: const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; } #if defined(CONFIG_EKF2_EXTERNAL_VISION) + void setExtVisionData(const extVisionSample &evdata); + const auto &aid_src_ev_hgt() const { return _aid_src_ev_hgt; } const auto &aid_src_ev_pos() const { return _aid_src_ev_pos; } const auto &aid_src_ev_vel() const { return _aid_src_ev_vel; } @@ -447,6 +627,13 @@ public: const BiasEstimator::status &getEvPosBiasEstimatorStatus(int i) const { return _ev_pos_b_est.getStatus(i); } #endif // CONFIG_EKF2_EXTERNAL_VISION + void setGpsData(const gpsMessage &gps); + + const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; } + + // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined + bool collect_gps(const gpsMessage &gps); + const auto &aid_src_gnss_hgt() const { return _aid_src_gnss_hgt; } const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; } const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; } @@ -458,28 +645,141 @@ public: #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_MAGNETOMETER) + void setMagData(const magSample &mag_sample); + const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; } const auto &aid_src_mag() const { return _aid_src_mag; } + + // Get the value of magnetic declination in degrees to be saved for use at the next startup + // Returns true when the declination can be saved + // At the next startup, set param.mag_declination_deg to the value saved + bool get_mag_decl_deg(float &val) const + { + if (_NED_origin_initialised && (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)) { + val = math::degrees(_mag_declination_gps); + return true; + + } else { + return false; + } + } + + bool get_mag_inc_deg(float &val) const + { + if (_NED_origin_initialised) { + val = math::degrees(_mag_inclination_gps); + return true; + + } else { + return false; + } + } + + void get_mag_checks(float &inc_deg, float &inc_ref_deg, float &strength_gs, float &strength_ref_gs) const + { + inc_deg = math::degrees(_mag_inclination); + inc_ref_deg = math::degrees(_mag_inclination_gps); + strength_gs = _mag_strength; + strength_ref_gs = _mag_strength_gps; + } #endif // CONFIG_EKF2_MAGNETOMETER const auto &aid_src_gravity() const { return _aid_src_gravity; } #if defined(CONFIG_EKF2_AUXVEL) + void setAuxVelData(const auxVelSample &auxvel_sample); + const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; } #endif // CONFIG_EKF2_AUXVEL -private: + // the flags considered are opt_flow, gps, ev_vel and ev_pos + bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const { return aiding_flag && !isOtherSourceOfHorizontalAidingThan(aiding_flag); } + + /* + * Check if there are any other active source of horizontal aiding + * Warning: does not tell if the selected source is + * active, use isOnlyActiveSourceOfHorizontalAiding() for this + * + * The flags considered are opt_flow, gps, ev_vel and ev_pos + * + * @param aiding_flag a flag in _control_status.flags + * @return true if an other source than aiding_flag is active + */ + bool isOtherSourceOfHorizontalAidingThan(bool aiding_flag) const; + + // Return true if at least one source of horizontal aiding is active + // the flags considered are opt_flow, gps, ev_vel and ev_pos + bool isHorizontalAidingActive() const { return getNumberOfActiveHorizontalAidingSources() > 0; } + bool isVerticalAidingActive() const { return isVerticalPositionAidingActive() || isVerticalVelocityAidingActive(); } + + int getNumberOfActiveHorizontalAidingSources() const; + + bool isOtherSourceOfVerticalPositionAidingThan(bool aiding_flag) const; + bool isVerticalPositionAidingActive() const { return getNumberOfActiveVerticalPositionAidingSources() > 0; } + bool isOnlyActiveSourceOfVerticalPositionAiding(bool aiding_flag) const { return aiding_flag && !isOtherSourceOfVerticalPositionAidingThan(aiding_flag); } + int getNumberOfActiveVerticalPositionAidingSources() const; + + bool isVerticalVelocityAidingActive() const { return getNumberOfActiveVerticalVelocityAidingSources() > 0; } + int getNumberOfActiveVerticalVelocityAidingSources() const { return int(_control_status.flags.gps) + int(_control_status.flags.ev_vel);} + +private: // set the internal states and status to their default value void reset(); + // allocate data buffers and initialize interface variables + bool initialise_interface(uint64_t timestamp); + bool initialiseTilt(); + void printBufferAllocationFailed(const char *buffer_name); + // check if the EKF is dead reckoning horizontal velocity using inertial data only void updateDeadReckoningStatus(); void updateHorizontalDeadReckoningstatus(); void updateVerticalDeadReckoningStatus(); + SquareMatrixState P{}; ///< state covariance matrix + stateSample _state{}; ///< state struct of the ekf running at the delayed time horizon + + bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized + bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised + bool _imu_updated{false}; // true if the ekf should update (completed downsampling process) + + static constexpr uint8_t kBufferLengthDefault = 12; + RingBuffer _imu_buffer{kBufferLengthDefault}; + RingBuffer *_system_flag_buffer{nullptr}; + + parameters _params{}; // filter parameters + + /* + OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer + which sets the maximum frequency at which we can process non-IMU measurements. Measurements that + arrive too soon after the previous measurement will not be processed. + max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_S) + This can be adjusted to match the max sensor data rate plus some margin for jitter. + */ + uint8_t _obs_buffer_length{0}; + + /* + IMU_BUFFER_LENGTH defines how many IMU samples we buffer which sets the time delay from current time to the + EKF fusion time horizon and therefore the maximum sensor time offset relative to the IMU that we can compensate for. + max sensor time offet (msec) = IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS + This can be adjusted to a value that is FILTER_UPDATE_PERIOD_MS longer than the maximum observation time delay. + */ + uint8_t _imu_buffer_length{0}; + + unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec) + + float _dt_ekf_avg{0.010f}; ///< average update rate of the ekf in s + + uint64_t _time_delayed_us{0}; // captures the imu sample on the delayed time horizon + uint64_t _time_latest_us{0}; // imu sample capturing the newest imu data + + ImuDownSampler _imu_down_sampler{_params.filter_update_interval_us}; + OutputPredictor _output_predictor{}; + + struct StateResetCounts { uint8_t velNE{0}; ///< number of horizontal position reset events (allow to wrap if count exceeds 255) uint8_t velD{0}; ///< number of vertical velocity reset events (allow to wrap if count exceeds 255) @@ -503,13 +803,28 @@ private: Vector3f _ang_rate_delayed_raw{}; ///< uncorrected angular rate vector at fusion time horizon (rad/sec) - stateSample _state{}; ///< state struct of the ekf running at the delayed time horizon + float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3) - bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised + RingBuffer *_gps_buffer{nullptr}; + uint64_t _time_last_gps_buffer_push{0}; - // booleans true when fresh sensor data is available at the fusion time horizon bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused + bool _NED_origin_initialised{false}; + float _gpos_origin_eph{0.0f}; // horizontal position uncertainty of the global origin + float _gpos_origin_epv{0.0f}; // vertical position uncertainty of the global origin + MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin + MapProjection _gps_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message + float _gps_alt_prev{0.0f}; // height from the previous GPS message (m) + + float _gps_horizontal_position_drift_rate_m_s{NAN}; // Horizontal position drift rate (m/s) + float _gps_vertical_position_drift_rate_m_s{NAN}; // Vertical position drift rate (m/s) + float _gps_filtered_horizontal_velocity_m_s{NAN}; // Filtered horizontal velocity (m/s) + + uint64_t _time_last_on_ground_us{0}; ///< last time we were on the ground (uSec) + uint64_t _time_last_in_air{0}; ///< last time we were in air (uSec) + uint64_t _time_last_gnd_effect_on{0}; + uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec) uint64_t _time_last_v_pos_aiding{0}; uint64_t _time_last_v_vel_aiding{0}; @@ -521,6 +836,10 @@ private: uint64_t _time_last_heading_fuse{0}; uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec) + bool _horizontal_deadreckon_time_exceeded{true}; + bool _vertical_position_deadreckon_time_exceeded{true}; + bool _vertical_velocity_deadreckon_time_exceeded{true}; + Vector3f _last_known_pos{}; ///< last known local position vector (m) uint64_t _time_acc_bias_check{0}; ///< last time the accel bias check passed (uSec) @@ -537,9 +856,14 @@ private: float _yaw_delta_ef{0.0f}; ///< Recent change in yaw angle measured about the earth frame D axis (rad) float _yaw_rate_lpf_ef{0.0f}; ///< Filtered angular rate about earth frame D axis (rad/sec) - SquareMatrixState P{}; ///< state covariance matrix - #if defined(CONFIG_EKF2_DRAG_FUSION) + RingBuffer *_drag_buffer {nullptr}; + dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate) + + // Used by the multi-rotor specific drag force fusion + uint8_t _drag_sample_count{0}; // number of drag specific force samples assumulated at the filter prediction rate + float _drag_sample_time_dt{0.0f}; // time integral across all samples used to form _drag_down_sampled (sec) + estimator_aid_source2d_s _aid_src_drag{}; #endif // CONFIG_EKF2_DRAG_FUSION @@ -553,23 +877,38 @@ private: float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m) # if defined(CONFIG_EKF2_RANGE_FINDER) + RingBuffer *_range_buffer {nullptr}; + uint64_t _time_last_range_buffer_push{0}; + + sensor::SensorRangeFinder _range_sensor{}; + RangeFinderConsistencyCheck _rng_consistency_check; + estimator_aid_source1d_s _aid_src_terrain_range_finder{}; uint64_t _time_last_healthy_rng_data{0}; # endif // CONFIG_EKF2_RANGE_FINDER # if defined(CONFIG_EKF2_OPTICAL_FLOW) + RingBuffer *_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) + estimator_aid_source2d_s _aid_src_terrain_optical_flow{}; # endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_RANGE_FINDER) - estimator_aid_source1d_s _aid_src_rng_hgt{}; + estimator_aid_source1d_s _aid_src_rng_hgt {}; HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref}; #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) - estimator_aid_source2d_s _aid_src_optical_flow{}; + estimator_aid_source2d_s _aid_src_optical_flow {}; // optical flow processing Vector3f _flow_gyro_bias{}; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec) @@ -588,16 +927,24 @@ private: #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_AIRSPEED) + RingBuffer *_airspeed_buffer {nullptr}; + airspeedSample _airspeed_sample_delayed{}; + estimator_aid_source1d_s _aid_src_airspeed{}; #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_SIDESLIP) - estimator_aid_source1d_s _aid_src_sideslip{}; + estimator_aid_source1d_s _aid_src_sideslip {}; #endif // CONFIG_EKF2_SIDESLIP estimator_aid_source2d_s _aid_src_fake_pos{}; estimator_aid_source1d_s _aid_src_fake_hgt{}; #if defined(CONFIG_EKF2_EXTERNAL_VISION) + RingBuffer *_ext_vision_buffer {nullptr}; + uint64_t _time_last_ext_vision_buffer_push{0}; + + extVisionSample _ev_sample_prev{}; + estimator_aid_source1d_s _aid_src_ev_hgt{}; estimator_aid_source2d_s _aid_src_ev_pos{}; estimator_aid_source3d_s _aid_src_ev_vel{}; @@ -615,6 +962,11 @@ private: estimator_aid_source3d_s _aid_src_gnss_vel{}; #if defined(CONFIG_EKF2_GNSS_YAW) + float _gps_yaw_offset {0.0f}; // Yaw offset angle for dual GPS antennas used for yaw estimation (radians). + // innovation consistency check monitoring ratios + AlphaFilter _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state + uint64_t _time_last_gps_yaw_buffer_push{0}; + estimator_aid_source1d_s _aid_src_gnss_yaw{}; uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source #endif // CONFIG_EKF2_GNSS_YAW @@ -622,6 +974,8 @@ private: estimator_aid_source3d_s _aid_src_gravity{}; #if defined(CONFIG_EKF2_AUXVEL) + RingBuffer *_auxvel_buffer {nullptr}; + estimator_aid_source2d_s _aid_src_aux_vel{}; #endif // CONFIG_EKF2_AUXVEL @@ -639,12 +993,24 @@ private: // Variables used to publish the WGS-84 location of the EKF local NED origin float _gps_alt_ref{NAN}; ///< WGS-84 height (m) + uint64_t _wmm_gps_time_last_checked{0}; // time WMM last checked + uint64_t _wmm_gps_time_last_set{0}; // time WMM last set + float _mag_declination_gps{NAN}; // magnetic declination returned by the geo library using the last valid GPS position (rad) + float _mag_inclination_gps{NAN}; // magnetic inclination returned by the geo library using the last valid GPS position (rad) + float _mag_strength_gps{NAN}; // magnetic strength returned by the geo library using the last valid GPS position (T) + + float _mag_inclination{NAN}; + float _mag_strength{NAN}; + // Variables used by the initial filter alignment bool _is_first_imu_sample{true}; AlphaFilter _accel_lpf{0.1f}; ///< filtered accelerometer measurement used to align tilt (m/s/s) AlphaFilter _gyro_lpf{0.1f}; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec) #if defined(CONFIG_EKF2_BAROMETER) + RingBuffer *_baro_buffer {nullptr}; + uint64_t _time_last_baro_buffer_push{0}; + estimator_aid_source1d_s _aid_src_baro_hgt{}; // Variables used to perform in flight resets and switch between height sources @@ -657,6 +1023,9 @@ private: #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_MAGNETOMETER) + RingBuffer *_mag_buffer {nullptr}; + uint64_t _time_last_mag_buffer_push{0}; + float _mag_heading_prev{}; ///< previous value of mag heading (rad) float _mag_heading_pred_prev{}; ///< previous value of yaw state used by mag heading fusion (rad) @@ -685,6 +1054,20 @@ private: Matrix3f _saved_mag_bf_covmat{}; ///< magnetic field state covariance sub-matrix that has been saved for use at the next initialisation (Gauss**2) #endif // CONFIG_EKF2_MAGNETOMETER + // this is the current status of the filter control modes + filter_control_status_u _control_status{}; + + // this is the previous status of the filter control modes - used to detect mode transitions + filter_control_status_u _control_status_prev{}; + + fault_status_u _fault_status{}; + innovation_fault_status_u _innov_check_fail_status{}; + + // these are used to record single frame events for external monitoring and should NOT be used for + // state logic becasue they will be cleared externally after being read. + warning_event_status_u _warning_events{}; + information_event_status_u _information_events{}; + gps_check_fail_status_u _gps_check_fail_status{}; // variables used to inhibit accel bias learning @@ -720,7 +1103,8 @@ private: void predictCovariance(const imuSample &imu_delayed); template - void resetStateCovariance(const matrix::SquareMatrix &cov) { + void resetStateCovariance(const matrix::SquareMatrix &cov) + { P.uncorrelateCovarianceSetVariance(S.idx, 0.0f); P.slice(S.idx, S.idx) = cov; } @@ -813,12 +1197,16 @@ private: void resetVerticalVelocityToZero(); // 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; + 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; // 2d & 3d velocity aid source - void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const; - void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, const float innov_gate, estimator_aid_source3d_s &aid_src) const; + void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, + const float innov_gate, estimator_aid_source2d_s &aid_src) const; + void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, + const float innov_gate, estimator_aid_source3d_s &aid_src) const; // horizontal and vertical position fusion void fuseHorizontalPosition(estimator_aid_source2d_s &pos_aid_src); @@ -990,13 +1378,18 @@ private: // control fusion of external vision observations void controlExternalVisionFusion(); void updateEvAttitudeErrorFilter(extVisionSample &ev_sample, bool ev_reset); - void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src); - void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src); - void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src); - void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src); + void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, + const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src); + void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, + const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src); + void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, + const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src); + void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, + const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src); void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src); - void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src); + void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, + bool reset, estimator_aid_source2d_s &aid_src); void stopEvPosFusion(); void stopEvHgtFusion(); void stopEvVelFusion(); @@ -1011,8 +1404,10 @@ private: #if defined(CONFIG_EKF2_MAGNETOMETER) // control fusion of magnetometer observations void controlMagFusion(); - void controlMagHeadingFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, estimator_aid_source1d_s &aid_src); - void controlMag3DFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, estimator_aid_source3d_s &aid_src); + void controlMagHeadingFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, + estimator_aid_source1d_s &aid_src); + void controlMag3DFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, + estimator_aid_source3d_s &aid_src); bool checkHaglYawResetReq() const; @@ -1129,10 +1524,12 @@ private: uint8_t _height_sensor_ref{HeightSensor::UNKNOWN}; uint8_t _position_sensor_ref{static_cast(PositionSensor::GNSS)}; + // measurement samples capturing measurements on the delayed time horizon + gpsSample _gps_sample_delayed{}; HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref}; #if defined(CONFIG_EKF2_EXTERNAL_VISION) - HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref}; + HeightBiasEstimator _ev_hgt_b_est {HeightSensor::EV, _height_sensor_ref}; PositionBiasEstimator _ev_pos_b_est{static_cast(PositionSensor::EV), _position_sensor_ref}; AlphaFilter _ev_q_error_filt{0.001f}; bool _ev_q_error_initialized{false}; diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 04879dfd92..38e401e6d7 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -40,11 +40,11 @@ * @author Siddharth B Purohit */ -#include "estimator_interface.h" +#include "ekf.h" #include -EstimatorInterface::~EstimatorInterface() +Ekf::~Ekf() { delete _gps_buffer; #if defined(CONFIG_EKF2_MAGNETOMETER) @@ -74,7 +74,7 @@ EstimatorInterface::~EstimatorInterface() } // Accumulate imu data and store to buffer at desired rate -void EstimatorInterface::setIMUData(const imuSample &imu_sample) +void Ekf::setIMUData(const imuSample &imu_sample) { // TODO: resolve misplaced responsibility if (!_initialised) { @@ -84,7 +84,9 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample) _time_latest_us = imu_sample.time_us; // the output observer always runs - _output_predictor.calculateOutputStates(imu_sample.time_us, imu_sample.delta_ang, imu_sample.delta_ang_dt, imu_sample.delta_vel, imu_sample.delta_vel_dt); + _output_predictor.calculateOutputStates(imu_sample.time_us, + imu_sample.delta_ang, imu_sample.delta_ang_dt, + imu_sample.delta_vel, imu_sample.delta_vel_dt); // accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available if (_imu_down_sampler.update(imu_sample)) { @@ -106,350 +108,7 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample) #endif // CONFIG_EKF2_DRAG_FUSION } -#if defined(CONFIG_EKF2_MAGNETOMETER) -void EstimatorInterface::setMagData(const magSample &mag_sample) -{ - if (!_initialised) { - return; - } - - // Allocate the required buffer size if not previously done - if (_mag_buffer == nullptr) { - _mag_buffer = new RingBuffer(_obs_buffer_length); - - if (_mag_buffer == nullptr || !_mag_buffer->valid()) { - delete _mag_buffer; - _mag_buffer = nullptr; - printBufferAllocationFailed("mag"); - return; - } - } - - const int64_t time_us = mag_sample.time_us - - static_cast(_params.mag_delay_ms * 1000) - - 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(_mag_buffer->get_newest().time_us + _min_obs_interval_us)) { - - magSample mag_sample_new{mag_sample}; - mag_sample_new.time_us = time_us; - - _mag_buffer->push(mag_sample_new); - _time_last_mag_buffer_push = _time_latest_us; - - } else { - ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, _min_obs_interval_us); - } -} -#endif // CONFIG_EKF2_MAGNETOMETER - -void EstimatorInterface::setGpsData(const gpsMessage &gps) -{ - if (!_initialised) { - return; - } - - // Allocate the required buffer size if not previously done - if (_gps_buffer == nullptr) { - _gps_buffer = new RingBuffer(_obs_buffer_length); - - if (_gps_buffer == nullptr || !_gps_buffer->valid()) { - delete _gps_buffer; - _gps_buffer = nullptr; - printBufferAllocationFailed("GPS"); - return; - } - } - - const int64_t time_us = gps.time_usec - - static_cast(_params.gps_delay_ms * 1000) - - static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 - - if (time_us >= static_cast(_gps_buffer->get_newest().time_us + _min_obs_interval_us)) { - - if (!gps.vel_ned_valid || (gps.fix_type == 0)) { - return; - } - - gpsSample gps_sample_new; - - gps_sample_new.time_us = time_us; - - gps_sample_new.vel = gps.vel_ned; - - gps_sample_new.sacc = gps.sacc; - gps_sample_new.hacc = gps.eph; - gps_sample_new.vacc = gps.epv; - - gps_sample_new.hgt = (float)gps.alt * 1e-3f; - -#if defined(CONFIG_EKF2_GNSS_YAW) - - if (PX4_ISFINITE(gps.yaw)) { - _time_last_gps_yaw_buffer_push = _time_latest_us; - gps_sample_new.yaw = gps.yaw; - gps_sample_new.yaw_acc = PX4_ISFINITE(gps.yaw_accuracy) ? gps.yaw_accuracy : 0.f; - - } else { - gps_sample_new.yaw = NAN; - gps_sample_new.yaw_acc = 0.f; - } - - if (PX4_ISFINITE(gps.yaw_offset)) { - _gps_yaw_offset = gps.yaw_offset; - - } else { - _gps_yaw_offset = 0.0f; - } - -#endif // CONFIG_EKF2_GNSS_YAW - - // Only calculate the relative position if the WGS-84 location of the origin is set - if (collect_gps(gps)) { - gps_sample_new.pos = _pos_ref.project((gps.lat / 1.0e7), (gps.lon / 1.0e7)); - - } else { - gps_sample_new.pos(0) = 0.0f; - gps_sample_new.pos(1) = 0.0f; - } - - _gps_buffer->push(gps_sample_new); - _time_last_gps_buffer_push = _time_latest_us; - - - } else { - ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us); - } -} - -#if defined(CONFIG_EKF2_BAROMETER) -void EstimatorInterface::setBaroData(const baroSample &baro_sample) -{ - if (!_initialised) { - return; - } - - // Allocate the required buffer size if not previously done - if (_baro_buffer == nullptr) { - _baro_buffer = new RingBuffer(_obs_buffer_length); - - if (_baro_buffer == nullptr || !_baro_buffer->valid()) { - delete _baro_buffer; - _baro_buffer = nullptr; - printBufferAllocationFailed("baro"); - return; - } - } - - const int64_t time_us = baro_sample.time_us - - static_cast(_params.baro_delay_ms * 1000) - - 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(_baro_buffer->get_newest().time_us + _min_obs_interval_us)) { - - baroSample baro_sample_new{baro_sample}; - baro_sample_new.time_us = time_us; - - _baro_buffer->push(baro_sample_new); - _time_last_baro_buffer_push = _time_latest_us; - - } else { - ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _baro_buffer->get_newest().time_us, _min_obs_interval_us); - } -} -#endif // CONFIG_EKF2_BAROMETER - -#if defined(CONFIG_EKF2_AIRSPEED) -void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample) -{ - if (!_initialised) { - return; - } - - // Allocate the required buffer size if not previously done - if (_airspeed_buffer == nullptr) { - _airspeed_buffer = new RingBuffer(_obs_buffer_length); - - if (_airspeed_buffer == nullptr || !_airspeed_buffer->valid()) { - delete _airspeed_buffer; - _airspeed_buffer = nullptr; - printBufferAllocationFailed("airspeed"); - return; - } - } - - const int64_t time_us = airspeed_sample.time_us - - static_cast(_params.airspeed_delay_ms * 1000) - - 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(_airspeed_buffer->get_newest().time_us + _min_obs_interval_us)) { - - airspeedSample airspeed_sample_new{airspeed_sample}; - airspeed_sample_new.time_us = time_us; - - _airspeed_buffer->push(airspeed_sample_new); - - } else { - ECL_WARN("airspeed data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _airspeed_buffer->get_newest().time_us, _min_obs_interval_us); - } -} -#endif // CONFIG_EKF2_AIRSPEED - -#if defined(CONFIG_EKF2_RANGE_FINDER) -void EstimatorInterface::setRangeData(const rangeSample &range_sample) -{ - if (!_initialised) { - return; - } - - // Allocate the required buffer size if not previously done - if (_range_buffer == nullptr) { - _range_buffer = new RingBuffer(_obs_buffer_length); - - if (_range_buffer == nullptr || !_range_buffer->valid()) { - delete _range_buffer; - _range_buffer = nullptr; - printBufferAllocationFailed("range"); - return; - } - } - - const int64_t time_us = range_sample.time_us - - static_cast(_params.range_delay_ms * 1000) - - 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(_range_buffer->get_newest().time_us + _min_obs_interval_us)) { - - rangeSample range_sample_new{range_sample}; - range_sample_new.time_us = time_us; - - _range_buffer->push(range_sample_new); - _time_last_range_buffer_push = _time_latest_us; - - } else { - ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us, _min_obs_interval_us); - } -} -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) -void EstimatorInterface::setOpticalFlowData(const flowSample &flow) -{ - if (!_initialised) { - return; - } - - // Allocate the required buffer size if not previously done - if (_flow_buffer == nullptr) { - _flow_buffer = new RingBuffer(_imu_buffer_length); - - if (_flow_buffer == nullptr || !_flow_buffer->valid()) { - delete _flow_buffer; - _flow_buffer = nullptr; - printBufferAllocationFailed("flow"); - return; - } - } - - const int64_t time_us = flow.time_us - - static_cast(_params.flow_delay_ms * 1000) - - 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(_flow_buffer->get_newest().time_us + _min_obs_interval_us)) { - - flowSample optflow_sample_new{flow}; - optflow_sample_new.time_us = time_us; - - _flow_buffer->push(optflow_sample_new); - - } else { - 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) -{ - if (!_initialised) { - return; - } - - // Allocate the required buffer size if not previously done - if (_ext_vision_buffer == nullptr) { - _ext_vision_buffer = new RingBuffer(_obs_buffer_length); - - if (_ext_vision_buffer == nullptr || !_ext_vision_buffer->valid()) { - delete _ext_vision_buffer; - _ext_vision_buffer = nullptr; - printBufferAllocationFailed("vision"); - return; - } - } - - // calculate the system time-stamp for the mid point of the integration period - const int64_t time_us = evdata.time_us - - static_cast(_params.ev_delay_ms * 1000) - - 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(_ext_vision_buffer->get_newest().time_us + _min_obs_interval_us)) { - - extVisionSample ev_sample_new{evdata}; - ev_sample_new.time_us = time_us; - - _ext_vision_buffer->push(ev_sample_new); - _time_last_ext_vision_buffer_push = _time_latest_us; - - } else { - ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us, _min_obs_interval_us); - } -} -#endif // CONFIG_EKF2_EXTERNAL_VISION - -#if defined(CONFIG_EKF2_AUXVEL) -void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample) -{ - if (!_initialised) { - return; - } - - // Allocate the required buffer size if not previously done - if (_auxvel_buffer == nullptr) { - _auxvel_buffer = new RingBuffer(_obs_buffer_length); - - if (_auxvel_buffer == nullptr || !_auxvel_buffer->valid()) { - delete _auxvel_buffer; - _auxvel_buffer = nullptr; - printBufferAllocationFailed("aux vel"); - return; - } - } - - const int64_t time_us = auxvel_sample.time_us - - static_cast(_params.auxvel_delay_ms * 1000) - - 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(_auxvel_buffer->get_newest().time_us + _min_obs_interval_us)) { - - auxVelSample auxvel_sample_new{auxvel_sample}; - auxvel_sample_new.time_us = time_us; - - _auxvel_buffer->push(auxvel_sample_new); - - } else { - ECL_WARN("aux velocity data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _auxvel_buffer->get_newest().time_us, _min_obs_interval_us); - } -} -#endif // CONFIG_EKF2_AUXVEL - -void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags) +void Ekf::setSystemFlagData(const systemFlagUpdate &system_flags) { if (!_initialised) { return; @@ -479,75 +138,12 @@ void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags) _system_flag_buffer->push(system_flags_new); } else { - ECL_DEBUG("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_DEBUG("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", + time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us); } } -#if defined(CONFIG_EKF2_DRAG_FUSION) -void EstimatorInterface::setDragData(const imuSample &imu) -{ - // down-sample the drag specific force data by accumulating and calculating the mean when - // sufficient samples have been collected - if (_params.drag_ctrl > 0) { - - // Allocate the required buffer size if not previously done - if (_drag_buffer == nullptr) { - _drag_buffer = new RingBuffer(_obs_buffer_length); - - if (_drag_buffer == nullptr || !_drag_buffer->valid()) { - delete _drag_buffer; - _drag_buffer = nullptr; - printBufferAllocationFailed("drag"); - return; - } - } - - // don't use any accel samples that are clipping - if (imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]) { - // reset accumulators - _drag_sample_count = 0; - _drag_down_sampled.accelXY.zero(); - _drag_down_sampled.time_us = 0; - _drag_sample_time_dt = 0.0f; - - return; - } - - _drag_sample_count++; - // note acceleration is accumulated as a delta velocity - _drag_down_sampled.accelXY(0) += imu.delta_vel(0); - _drag_down_sampled.accelXY(1) += imu.delta_vel(1); - _drag_down_sampled.time_us += imu.time_us; - _drag_sample_time_dt += imu.delta_vel_dt; - - // calculate the downsample ratio for drag specific force data - uint8_t min_sample_ratio = (uint8_t) ceilf((float)_imu_buffer_length / _obs_buffer_length); - - if (min_sample_ratio < 5) { - min_sample_ratio = 5; - } - - // calculate and store means from accumulated values - if (_drag_sample_count >= min_sample_ratio) { - // note conversion from accumulated delta velocity to acceleration - _drag_down_sampled.accelXY(0) /= _drag_sample_time_dt; - _drag_down_sampled.accelXY(1) /= _drag_sample_time_dt; - _drag_down_sampled.time_us /= _drag_sample_count; - - // write to buffer - _drag_buffer->push(_drag_down_sampled); - - // reset accumulators - _drag_sample_count = 0; - _drag_down_sampled.accelXY.zero(); - _drag_down_sampled.time_us = 0; - _drag_sample_time_dt = 0.0f; - } - } -} -#endif // CONFIG_EKF2_DRAG_FUSION - -bool EstimatorInterface::initialise_interface(uint64_t timestamp) +bool Ekf::initialise_interface(uint64_t timestamp) { // find the maximum time delay the buffers are required to handle @@ -565,10 +161,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) } #if defined(CONFIG_EKF2_AIRSPEED) + // using airspeed if (_params.arsp_thr > FLT_EPSILON) { max_time_delay_ms = math::max(_params.airspeed_delay_ms, max_time_delay_ms); } + #endif // CONFIG_EKF2_AIRSPEED // mag mode @@ -577,10 +175,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) } #if defined(CONFIG_EKF2_RANGE_FINDER) + // using range finder if ((_params.rng_ctrl != RngCtrl::DISABLED)) { max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms); } + #endif // CONFIG_EKF2_RANGE_FINDER if (_params.gnss_ctrl > 0) { @@ -588,15 +188,19 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) } #if defined(CONFIG_EKF2_OPTICAL_FLOW) + if (_params.flow_ctrl > 0) { 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) { max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms); } + #endif // CONFIG_EKF2_EXTERNAL_VISION const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f; @@ -629,18 +233,13 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) return true; } -bool EstimatorInterface::isOnlyActiveSourceOfHorizontalAiding(const bool aiding_flag) const -{ - return aiding_flag && !isOtherSourceOfHorizontalAidingThan(aiding_flag); -} - -bool EstimatorInterface::isOtherSourceOfHorizontalAidingThan(const bool aiding_flag) const +bool Ekf::isOtherSourceOfHorizontalAidingThan(const bool aiding_flag) const { const int nb_sources = getNumberOfActiveHorizontalAidingSources(); return aiding_flag ? nb_sources > 1 : nb_sources > 0; } -int EstimatorInterface::getNumberOfActiveHorizontalAidingSources() const +int Ekf::getNumberOfActiveHorizontalAidingSources() const { return int(_control_status.flags.gps) + int(_control_status.flags.opt_flow) @@ -651,28 +250,13 @@ int EstimatorInterface::getNumberOfActiveHorizontalAidingSources() const + int(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta); } -bool EstimatorInterface::isHorizontalAidingActive() const -{ - return getNumberOfActiveHorizontalAidingSources() > 0; -} - -bool EstimatorInterface::isOtherSourceOfVerticalPositionAidingThan(const bool aiding_flag) const +bool Ekf::isOtherSourceOfVerticalPositionAidingThan(const bool aiding_flag) const { const int nb_sources = getNumberOfActiveVerticalPositionAidingSources(); return aiding_flag ? nb_sources > 1 : nb_sources > 0; } -bool EstimatorInterface::isVerticalPositionAidingActive() const -{ - return getNumberOfActiveVerticalPositionAidingSources() > 0; -} - -bool EstimatorInterface::isOnlyActiveSourceOfVerticalPositionAiding(const bool aiding_flag) const -{ - return aiding_flag && !isOtherSourceOfVerticalPositionAidingThan(aiding_flag); -} - -int EstimatorInterface::getNumberOfActiveVerticalPositionAidingSources() const +int Ekf::getNumberOfActiveVerticalPositionAidingSources() const { return int(_control_status.flags.gps_hgt) + int(_control_status.flags.baro_hgt) @@ -680,30 +264,14 @@ int EstimatorInterface::getNumberOfActiveVerticalPositionAidingSources() const + int(_control_status.flags.ev_hgt); } -bool EstimatorInterface::isVerticalAidingActive() const -{ - return isVerticalPositionAidingActive() || isVerticalVelocityAidingActive(); -} - -bool EstimatorInterface::isVerticalVelocityAidingActive() const -{ - return getNumberOfActiveVerticalVelocityAidingSources() > 0; -} - -int EstimatorInterface::getNumberOfActiveVerticalVelocityAidingSources() const -{ - return int(_control_status.flags.gps) - + int(_control_status.flags.ev_vel); -} - -void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name) +void Ekf::printBufferAllocationFailed(const char *buffer_name) { if (buffer_name) { ECL_ERR("%s buffer allocation failed", buffer_name); } } -void EstimatorInterface::print_status() +void Ekf::print_status() { printf("EKF average dt: %.6f seconds\n", (double)_dt_ekf_avg); @@ -712,49 +280,71 @@ void EstimatorInterface::print_status() printf("minimum observation interval %d us\n", _min_obs_interval_us); if (_gps_buffer) { - printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size()); + printf("gps buffer: %d/%d (%d Bytes)\n", + _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size()); } #if defined(CONFIG_EKF2_MAGNETOMETER) + if (_mag_buffer) { - printf("mag buffer: %d/%d (%d Bytes)\n", _mag_buffer->entries(), _mag_buffer->get_length(), _mag_buffer->get_total_size()); + printf("mag buffer: %d/%d (%d Bytes)\n", + _mag_buffer->entries(), _mag_buffer->get_length(), _mag_buffer->get_total_size()); } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_BAROMETER) + if (_baro_buffer) { - printf("baro buffer: %d/%d (%d Bytes)\n", _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size()); + printf("baro buffer: %d/%d (%d Bytes)\n", + _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size()); } + #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_RANGE_FINDER) + if (_range_buffer) { - printf("range buffer: %d/%d (%d Bytes)\n", _range_buffer->entries(), _range_buffer->get_length(), _range_buffer->get_total_size()); + printf("range buffer: %d/%d (%d Bytes)\n", _range_buffer->entries(), _range_buffer->get_length(), + _range_buffer->get_total_size()); } + #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_AIRSPEED) + if (_airspeed_buffer) { - printf("airspeed buffer: %d/%d (%d Bytes)\n", _airspeed_buffer->entries(), _airspeed_buffer->get_length(), _airspeed_buffer->get_total_size()); + printf("airspeed buffer: %d/%d (%d Bytes)\n", + _airspeed_buffer->entries(), _airspeed_buffer->get_length(), _airspeed_buffer->get_total_size()); } + #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()); + 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) { - printf("vision buffer: %d/%d (%d Bytes)\n", _ext_vision_buffer->entries(), _ext_vision_buffer->get_length(), _ext_vision_buffer->get_total_size()); + printf("vision buffer: %d/%d (%d Bytes)\n", + _ext_vision_buffer->entries(), _ext_vision_buffer->get_length(), _ext_vision_buffer->get_total_size()); } + #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_DRAG_FUSION) + if (_drag_buffer) { - printf("drag buffer: %d/%d (%d Bytes)\n", _drag_buffer->entries(), _drag_buffer->get_length(), _drag_buffer->get_total_size()); + printf("drag buffer: %d/%d (%d Bytes)\n", + _drag_buffer->entries(), _drag_buffer->get_length(), _drag_buffer->get_total_size()); } + #endif // CONFIG_EKF2_DRAG_FUSION _output_predictor.print_status(); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h deleted file mode 100644 index f09745ec98..0000000000 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ /dev/null @@ -1,489 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015-2023 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. - * - ****************************************************************************/ - -/** - * @file estimator_interface.h - * Definition of base class for attitude estimators - * - * @author Roman Bast - * - */ - -#ifndef EKF_ESTIMATOR_INTERFACE_H -#define EKF_ESTIMATOR_INTERFACE_H - -#if defined(MODULE_NAME) -#include -# define ECL_INFO PX4_DEBUG -# define ECL_WARN PX4_DEBUG -# define ECL_ERR PX4_DEBUG -# define ECL_DEBUG PX4_DEBUG -#else -# define ECL_INFO(X, ...) printf(X "\n", ##__VA_ARGS__) -# define ECL_WARN(X, ...) fprintf(stderr, X "\n", ##__VA_ARGS__) -# define ECL_ERR(X, ...) fprintf(stderr, X "\n", ##__VA_ARGS__) - -# if defined(DEBUG_BUILD) -# define ECL_DEBUG(X, ...) fprintf(stderr, X "\n", ##__VA_ARGS__) -# else -# define ECL_DEBUG(X, ...) -#endif - -#endif - -#include "common.h" -#include "RingBuffer.h" -#include "imu_down_sampler.hpp" -#include "output_predictor.h" - -#if defined(CONFIG_EKF2_RANGE_FINDER) -# include "range_finder_consistency_check.hpp" -# include "sensor_range_finder.hpp" -#endif // CONFIG_EKF2_RANGE_FINDER - -#include -#include -#include -#include - -using namespace estimator; - -class EstimatorInterface -{ -public: - // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined - virtual bool collect_gps(const gpsMessage &gps) = 0; - - void setIMUData(const imuSample &imu_sample); - -#if defined(CONFIG_EKF2_MAGNETOMETER) - void setMagData(const magSample &mag_sample); -#endif // CONFIG_EKF2_MAGNETOMETER - - void setGpsData(const gpsMessage &gps); - -#if defined(CONFIG_EKF2_BAROMETER) - void setBaroData(const baroSample &baro_sample); -#endif // CONFIG_EKF2_BAROMETER - -#if defined(CONFIG_EKF2_AIRSPEED) - void setAirspeedData(const airspeedSample &airspeed_sample); -#endif // CONFIG_EKF2_AIRSPEED - -#if defined(CONFIG_EKF2_RANGE_FINDER) - void setRangeData(const rangeSample &range_sample); - - // set sensor limitations reported by the rangefinder - void set_rangefinder_limits(float min_distance, float max_distance) - { - _range_sensor.setLimits(min_distance, max_distance); - } - - const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); } -#endif // CONFIG_EKF2_RANGE_FINDER - -#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); -#endif // CONFIG_EKF2_EXTERNAL_VISION - -#if defined(CONFIG_EKF2_AUXVEL) - void setAuxVelData(const auxVelSample &auxvel_sample); -#endif // CONFIG_EKF2_AUXVEL - - 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; } - - // set vehicle landed status data - void set_in_air_status(bool in_air) - { - if (!in_air) { - if (_control_status.flags.in_air) { - ECL_DEBUG("no longer in air"); - } - - _time_last_on_ground_us = _time_delayed_us; - - } else { - if (!_control_status.flags.in_air) { - ECL_DEBUG("in air"); - } - - _time_last_in_air = _time_delayed_us; - } - - _control_status.flags.in_air = in_air; - } - - void set_vehicle_at_rest(bool at_rest) - { - if (!_control_status.flags.vehicle_at_rest && at_rest) { - ECL_DEBUG("at rest"); - - } else if (_control_status.flags.vehicle_at_rest && !at_rest) { - ECL_DEBUG("no longer at rest"); - } - - _control_status.flags.vehicle_at_rest = at_rest; - } - - // return true if the attitude is usable - bool attitude_valid() const { return _control_status.flags.tilt_align; } - - // get vehicle landed status data - bool get_in_air_status() const { return _control_status.flags.in_air; } - - // get wind estimation status - bool get_wind_status() const { return _control_status.flags.wind; } - - // set vehicle is fixed wing status - void set_is_fixed_wing(bool is_fixed_wing) { _control_status.flags.fixed_wing = is_fixed_wing; } - - // set flag if static pressure rise due to ground effect is expected - // use _params.gnd_effect_deadzone to adjust for expected rise in static pressure - // flag will clear after GNDEFFECT_TIMEOUT uSec - void set_gnd_effect() - { - _control_status.flags.gnd_effect = true; - _time_last_gnd_effect_on = _time_delayed_us; - } - - // set air density used by the multi-rotor specific drag force fusion - void set_air_density(float air_density) { _air_density = air_density; } - - // the flags considered are opt_flow, gps, ev_vel and ev_pos - bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const; - - /* - * Check if there are any other active source of horizontal aiding - * Warning: does not tell if the selected source is - * active, use isOnlyActiveSourceOfHorizontalAiding() for this - * - * The flags considered are opt_flow, gps, ev_vel and ev_pos - * - * @param aiding_flag a flag in _control_status.flags - * @return true if an other source than aiding_flag is active - */ - bool isOtherSourceOfHorizontalAidingThan(bool aiding_flag) const; - - // Return true if at least one source of horizontal aiding is active - // the flags considered are opt_flow, gps, ev_vel and ev_pos - bool isHorizontalAidingActive() const; - bool isVerticalAidingActive() const; - - int getNumberOfActiveHorizontalAidingSources() const; - - bool isOtherSourceOfVerticalPositionAidingThan(bool aiding_flag) const; - bool isVerticalPositionAidingActive() const; - bool isOnlyActiveSourceOfVerticalPositionAiding(const bool aiding_flag) const; - int getNumberOfActiveVerticalPositionAidingSources() const; - - bool isVerticalVelocityAidingActive() const; - int getNumberOfActiveVerticalVelocityAidingSources() const; - - const matrix::Quatf &getQuaternion() const { return _output_predictor.getQuaternion(); } - float getUnaidedYaw() const { return _output_predictor.getUnaidedYaw(); } - Vector3f getVelocity() const { return _output_predictor.getVelocity(); } - const Vector3f &getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); } - float getVerticalPositionDerivative() const { return _output_predictor.getVerticalPositionDerivative(); } - Vector3f getPosition() const { return _output_predictor.getPosition(); } - const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); } - -#if defined(CONFIG_EKF2_MAGNETOMETER) - // Get the value of magnetic declination in degrees to be saved for use at the next startup - // Returns true when the declination can be saved - // At the next startup, set param.mag_declination_deg to the value saved - bool get_mag_decl_deg(float &val) const - { - if (_NED_origin_initialised && (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)) { - val = math::degrees(_mag_declination_gps); - return true; - - } else { - return false; - } - } - - bool get_mag_inc_deg(float &val) const - { - if (_NED_origin_initialised) { - val = math::degrees(_mag_inclination_gps); - return true; - - } else { - return false; - } - } - - void get_mag_checks(float &inc_deg, float &inc_ref_deg, float &strength_gs, float &strength_ref_gs) const - { - inc_deg = math::degrees(_mag_inclination); - inc_ref_deg = math::degrees(_mag_inclination_gps); - strength_gs = _mag_strength; - strength_ref_gs = _mag_strength_gps; - } -#endif // CONFIG_EKF2_MAGNETOMETER - - // get EKF mode status - const filter_control_status_u &control_status() const { return _control_status; } - const decltype(filter_control_status_u::flags) &control_status_flags() const { return _control_status.flags; } - - const filter_control_status_u &control_status_prev() const { return _control_status_prev; } - const decltype(filter_control_status_u::flags) &control_status_prev_flags() const { return _control_status_prev.flags; } - - // get EKF internal fault status - const fault_status_u &fault_status() const { return _fault_status; } - const decltype(fault_status_u::flags) &fault_status_flags() const { return _fault_status.flags; } - - const innovation_fault_status_u &innov_check_fail_status() const { return _innov_check_fail_status; } - const decltype(innovation_fault_status_u::flags) &innov_check_fail_status_flags() const { return _innov_check_fail_status.flags; } - - const warning_event_status_u &warning_event_status() const { return _warning_events; } - const decltype(warning_event_status_u::flags) &warning_event_flags() const { return _warning_events.flags; } - void clear_warning_events() { _warning_events.value = 0; } - - const information_event_status_u &information_event_status() const { return _information_events; } - const decltype(information_event_status_u::flags) &information_event_flags() const { return _information_events.flags; } - void clear_information_events() { _information_events.value = 0; } - - // Getter for the average EKF update period in s - float get_dt_ekf_avg() const { return _dt_ekf_avg; } - - // Getters for samples on the delayed time horizon - const imuSample &get_imu_sample_delayed() const { return _imu_buffer.get_oldest(); } - const uint64_t &time_delayed_us() const { return _time_delayed_us; } - - const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; } - - const bool &global_origin_valid() const { return _NED_origin_initialised; } - const MapProjection &global_origin() const { return _pos_ref; } - - void print_status(); - - float gps_horizontal_position_drift_rate_m_s() const { return _gps_horizontal_position_drift_rate_m_s; } - float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; } - float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; } - - OutputPredictor &output_predictor() { return _output_predictor; }; - -protected: - - EstimatorInterface() = default; - virtual ~EstimatorInterface(); - - virtual bool init(uint64_t timestamp) = 0; - - parameters _params{}; // filter parameters - - /* - OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer - which sets the maximum frequency at which we can process non-IMU measurements. Measurements that - arrive too soon after the previous measurement will not be processed. - max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_S) - This can be adjusted to match the max sensor data rate plus some margin for jitter. - */ - uint8_t _obs_buffer_length{0}; - - /* - IMU_BUFFER_LENGTH defines how many IMU samples we buffer which sets the time delay from current time to the - EKF fusion time horizon and therefore the maximum sensor time offset relative to the IMU that we can compensate for. - max sensor time offet (msec) = IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS - This can be adjusted to a value that is FILTER_UPDATE_PERIOD_MS longer than the maximum observation time delay. - */ - uint8_t _imu_buffer_length{0}; - - float _dt_ekf_avg{0.010f}; ///< average update rate of the ekf in s - - uint64_t _time_delayed_us{0}; // captures the imu sample on the delayed time horizon - uint64_t _time_latest_us{0}; // imu sample capturing the newest imu data - - OutputPredictor _output_predictor{}; - - // measurement samples capturing measurements on the delayed time horizon - gpsSample _gps_sample_delayed{}; - - -#if defined(CONFIG_EKF2_AIRSPEED) - airspeedSample _airspeed_sample_delayed{}; -#endif // CONFIG_EKF2_AIRSPEED - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - extVisionSample _ev_sample_prev{}; -#endif // CONFIG_EKF2_EXTERNAL_VISION - -#if defined(CONFIG_EKF2_RANGE_FINDER) - RingBuffer *_range_buffer{nullptr}; - uint64_t _time_last_range_buffer_push{0}; - - sensor::SensorRangeFinder _range_sensor{}; - RangeFinderConsistencyCheck _rng_consistency_check; -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - RingBuffer *_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 - - float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3) - - 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 - - bool _NED_origin_initialised{false}; - float _gpos_origin_eph{0.0f}; // horizontal position uncertainty of the global origin - float _gpos_origin_epv{0.0f}; // vertical position uncertainty of the global origin - MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin - MapProjection _gps_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message - float _gps_alt_prev{0.0f}; // height from the previous GPS message (m) -#if defined(CONFIG_EKF2_GNSS_YAW) - float _gps_yaw_offset{0.0f}; // Yaw offset angle for dual GPS antennas used for yaw estimation (radians). - // innovation consistency check monitoring ratios - AlphaFilter _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state - uint64_t _time_last_gps_yaw_buffer_push{0}; -#endif // CONFIG_EKF2_GNSS_YAW - -#if defined(CONFIG_EKF2_DRAG_FUSION) - RingBuffer *_drag_buffer{nullptr}; - dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate) -#endif // CONFIG_EKF2_DRAG_FUSION - - innovation_fault_status_u _innov_check_fail_status{}; - - bool _horizontal_deadreckon_time_exceeded{true}; - bool _vertical_position_deadreckon_time_exceeded{true}; - bool _vertical_velocity_deadreckon_time_exceeded{true}; - - float _gps_horizontal_position_drift_rate_m_s{NAN}; // Horizontal position drift rate (m/s) - float _gps_vertical_position_drift_rate_m_s{NAN}; // Vertical position drift rate (m/s) - float _gps_filtered_horizontal_velocity_m_s{NAN}; // Filtered horizontal velocity (m/s) - - uint64_t _time_last_on_ground_us{0}; ///< last time we were on the ground (uSec) - uint64_t _time_last_in_air{0}; ///< last time we were in air (uSec) - - // data buffer instances - static constexpr uint8_t kBufferLengthDefault = 12; - RingBuffer _imu_buffer{kBufferLengthDefault}; - - RingBuffer *_gps_buffer{nullptr}; - -#if defined(CONFIG_EKF2_MAGNETOMETER) - RingBuffer *_mag_buffer{nullptr}; - uint64_t _time_last_mag_buffer_push{0}; -#endif // CONFIG_EKF2_MAGNETOMETER - -#if defined(CONFIG_EKF2_AIRSPEED) - RingBuffer *_airspeed_buffer{nullptr}; -#endif // CONFIG_EKF2_AIRSPEED - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - RingBuffer *_ext_vision_buffer{nullptr}; - uint64_t _time_last_ext_vision_buffer_push{0}; -#endif // CONFIG_EKF2_EXTERNAL_VISION -#if defined(CONFIG_EKF2_AUXVEL) - RingBuffer *_auxvel_buffer{nullptr}; -#endif // CONFIG_EKF2_AUXVEL - RingBuffer *_system_flag_buffer{nullptr}; - - uint64_t _time_last_gps_buffer_push{0}; - -#if defined(CONFIG_EKF2_BAROMETER) - RingBuffer *_baro_buffer{nullptr}; - uint64_t _time_last_baro_buffer_push{0}; -#endif // CONFIG_EKF2_BAROMETER - - uint64_t _time_last_gnd_effect_on{0}; - - fault_status_u _fault_status{}; - - // allocate data buffers and initialize interface variables - bool initialise_interface(uint64_t timestamp); - - uint64_t _wmm_gps_time_last_checked{0}; // time WMM last checked - uint64_t _wmm_gps_time_last_set{0}; // time WMM last set - float _mag_declination_gps{NAN}; // magnetic declination returned by the geo library using the last valid GPS position (rad) - float _mag_inclination_gps{NAN}; // magnetic inclination returned by the geo library using the last valid GPS position (rad) - float _mag_strength_gps{NAN}; // magnetic strength returned by the geo library using the last valid GPS position (T) - - float _mag_inclination{NAN}; - float _mag_strength{NAN}; - - // this is the current status of the filter control modes - filter_control_status_u _control_status{}; - - // this is the previous status of the filter control modes - used to detect mode transitions - filter_control_status_u _control_status_prev{}; - - // these are used to record single frame events for external monitoring and should NOT be used for - // state logic becasue they will be cleared externally after being read. - warning_event_status_u _warning_events{}; - information_event_status_u _information_events{}; - -private: - -#if defined(CONFIG_EKF2_DRAG_FUSION) - void setDragData(const imuSample &imu); - - // Used by the multi-rotor specific drag force fusion - uint8_t _drag_sample_count{0}; // number of drag specific force samples assumulated at the filter prediction rate - float _drag_sample_time_dt{0.0f}; // time integral across all samples used to form _drag_down_sampled (sec) -#endif // CONFIG_EKF2_DRAG_FUSION - - void printBufferAllocationFailed(const char *buffer_name); - - ImuDownSampler _imu_down_sampler{_params.filter_update_interval_us}; - - unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec) -}; -#endif // !EKF_ESTIMATOR_INTERFACE_H diff --git a/src/modules/ekf2/EKF/ev_control.cpp b/src/modules/ekf2/EKF/ev_control.cpp index 22ac4ec764..fe4c84300e 100644 --- a/src/modules/ekf2/EKF/ev_control.cpp +++ b/src/modules/ekf2/EKF/ev_control.cpp @@ -38,6 +38,43 @@ #include "ekf.h" +void Ekf::setExtVisionData(const extVisionSample &evdata) +{ + if (!_initialised) { + return; + } + + // Allocate the required buffer size if not previously done + if (_ext_vision_buffer == nullptr) { + _ext_vision_buffer = new RingBuffer(_obs_buffer_length); + + if (_ext_vision_buffer == nullptr || !_ext_vision_buffer->valid()) { + delete _ext_vision_buffer; + _ext_vision_buffer = nullptr; + printBufferAllocationFailed("vision"); + return; + } + } + + // calculate the system time-stamp for the mid point of the integration period + const int64_t time_us = evdata.time_us + - static_cast(_params.ev_delay_ms * 1000) + - 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(_ext_vision_buffer->get_newest().time_us + _min_obs_interval_us)) { + + extVisionSample ev_sample_new{evdata}; + ev_sample_new.time_us = time_us; + + _ext_vision_buffer->push(ev_sample_new); + _time_last_ext_vision_buffer_push = _time_latest_us; + + } else { + ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us, _min_obs_interval_us); + } +} + void Ekf::controlExternalVisionFusion() { _ev_pos_b_est.predict(_dt_ekf_avg); diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index 4c620de17f..17f52f3f3a 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -39,6 +39,85 @@ #include "ekf.h" #include +void Ekf::setGpsData(const gpsMessage &gps) +{ + if (!_initialised) { + return; + } + + // Allocate the required buffer size if not previously done + if (_gps_buffer == nullptr) { + _gps_buffer = new RingBuffer(_obs_buffer_length); + + if (_gps_buffer == nullptr || !_gps_buffer->valid()) { + delete _gps_buffer; + _gps_buffer = nullptr; + printBufferAllocationFailed("GPS"); + return; + } + } + + const int64_t time_us = gps.time_usec + - static_cast(_params.gps_delay_ms * 1000) + - static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 + + if (time_us >= static_cast(_gps_buffer->get_newest().time_us + _min_obs_interval_us)) { + + if (!gps.vel_ned_valid || (gps.fix_type == 0)) { + return; + } + + gpsSample gps_sample_new; + + gps_sample_new.time_us = time_us; + + gps_sample_new.vel = gps.vel_ned; + + gps_sample_new.sacc = gps.sacc; + gps_sample_new.hacc = gps.eph; + gps_sample_new.vacc = gps.epv; + + gps_sample_new.hgt = (float)gps.alt * 1e-3f; + +#if defined(CONFIG_EKF2_GNSS_YAW) + + if (PX4_ISFINITE(gps.yaw)) { + _time_last_gps_yaw_buffer_push = _time_latest_us; + gps_sample_new.yaw = gps.yaw; + gps_sample_new.yaw_acc = PX4_ISFINITE(gps.yaw_accuracy) ? gps.yaw_accuracy : 0.f; + + } else { + gps_sample_new.yaw = NAN; + gps_sample_new.yaw_acc = 0.f; + } + + if (PX4_ISFINITE(gps.yaw_offset)) { + _gps_yaw_offset = gps.yaw_offset; + + } else { + _gps_yaw_offset = 0.0f; + } + +#endif // CONFIG_EKF2_GNSS_YAW + + // Only calculate the relative position if the WGS-84 location of the origin is set + if (collect_gps(gps)) { + gps_sample_new.pos = _pos_ref.project((gps.lat / 1.0e7), (gps.lon / 1.0e7)); + + } else { + gps_sample_new.pos(0) = 0.0f; + gps_sample_new.pos(1) = 0.0f; + } + + _gps_buffer->push(gps_sample_new); + _time_last_gps_buffer_push = _time_latest_us; + + + } else { + ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us); + } +} + void Ekf::controlGpsFusion(const imuSample &imu_delayed) { if (!_gps_buffer || (_params.gnss_ctrl == 0)) { diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 0dacfad3e1..fec21b15af 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -39,6 +39,42 @@ #include "ekf.h" #include +void Ekf::setMagData(const magSample &mag_sample) +{ + if (!_initialised) { + return; + } + + // Allocate the required buffer size if not previously done + if (_mag_buffer == nullptr) { + _mag_buffer = new RingBuffer(_obs_buffer_length); + + if (_mag_buffer == nullptr || !_mag_buffer->valid()) { + delete _mag_buffer; + _mag_buffer = nullptr; + printBufferAllocationFailed("mag"); + return; + } + } + + const int64_t time_us = mag_sample.time_us + - static_cast(_params.mag_delay_ms * 1000) + - 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(_mag_buffer->get_newest().time_us + _min_obs_interval_us)) { + + magSample mag_sample_new{mag_sample}; + mag_sample_new.time_us = time_us; + + _mag_buffer->push(mag_sample_new); + _time_last_mag_buffer_push = _time_latest_us; + + } else { + ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, _min_obs_interval_us); + } +} + void Ekf::controlMagFusion() { // reset the flight alignment flag so that the mag fields will be diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp index a8207db178..c020b6675a 100644 --- a/src/modules/ekf2/EKF/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/optical_flow_control.cpp @@ -38,6 +38,41 @@ #include "ekf.h" +void Ekf::setOpticalFlowData(const flowSample &flow) +{ + if (!_initialised) { + return; + } + + // Allocate the required buffer size if not previously done + if (_flow_buffer == nullptr) { + _flow_buffer = new RingBuffer(_imu_buffer_length); + + if (_flow_buffer == nullptr || !_flow_buffer->valid()) { + delete _flow_buffer; + _flow_buffer = nullptr; + printBufferAllocationFailed("flow"); + return; + } + } + + const int64_t time_us = flow.time_us + - static_cast(_params.flow_delay_ms * 1000) + - 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(_flow_buffer->get_newest().time_us + _min_obs_interval_us)) { + + flowSample optflow_sample_new{flow}; + optflow_sample_new.time_us = time_us; + + _flow_buffer->push(optflow_sample_new); + + } else { + ECL_WARN("optical flow data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _flow_buffer->get_newest().time_us, _min_obs_interval_us); + } +} + void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) { if (_flow_buffer) { diff --git a/src/modules/ekf2/EKF/range_height_control.cpp b/src/modules/ekf2/EKF/range_height_control.cpp index 177b841393..200a3efe24 100644 --- a/src/modules/ekf2/EKF/range_height_control.cpp +++ b/src/modules/ekf2/EKF/range_height_control.cpp @@ -38,6 +38,42 @@ #include "ekf.h" +void Ekf::setRangeData(const rangeSample &range_sample) +{ + if (!_initialised) { + return; + } + + // Allocate the required buffer size if not previously done + if (_range_buffer == nullptr) { + _range_buffer = new RingBuffer(_obs_buffer_length); + + if (_range_buffer == nullptr || !_range_buffer->valid()) { + delete _range_buffer; + _range_buffer = nullptr; + printBufferAllocationFailed("range"); + return; + } + } + + const int64_t time_us = range_sample.time_us + - static_cast(_params.range_delay_ms * 1000) + - 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(_range_buffer->get_newest().time_us + _min_obs_interval_us)) { + + rangeSample range_sample_new{range_sample}; + range_sample_new.time_us = time_us; + + _range_buffer->push(range_sample_new); + _time_last_range_buffer_push = _time_latest_us; + + } else { + ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us, _min_obs_interval_us); + } +} + void Ekf::controlRangeHeightFusion() { static constexpr const char *HGT_SRC_NAME = "RNG"; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 6c8a4fe33e..5b1d7ee3d1 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2077,7 +2077,7 @@ void EKF2::PublishYawEstimatorStatus(const hrt_abstime ×tamp) void EKF2::PublishWindEstimate(const hrt_abstime ×tamp) { - if (_ekf.get_wind_status()) { + if (_ekf.control_status_flags().wind) { // Publish wind estimate only if ekf declares them valid wind_s wind{}; wind.timestamp_sample = _ekf.time_delayed_us(); diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_logger.h b/src/modules/ekf2/test/sensor_simulator/ekf_logger.h index 748adcc758..f9da9826ce 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_logger.h +++ b/src/modules/ekf2/test/sensor_simulator/ekf_logger.h @@ -39,7 +39,6 @@ #define EKF_EKF_LOGGER_H #include "EKF/ekf.h" -#include "EKF/estimator_interface.h" #include "ekf_wrapper.h" #include #include diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h index beb43e64e3..511fc0422c 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h @@ -40,7 +40,6 @@ #include #include "EKF/ekf.h" -#include "EKF/estimator_interface.h" class EkfWrapper {