diff --git a/msg/estimator_aid_source_1d.msg b/msg/estimator_aid_source_1d.msg index b1c67c30f5..e2a964ea08 100644 --- a/msg/estimator_aid_source_1d.msg +++ b/msg/estimator_aid_source_1d.msg @@ -20,5 +20,5 @@ bool fused # true if the sample was successfully fused # TOPICS estimator_aid_source_1d # TOPICS estimator_aid_src_baro_hgt estimator_aid_src_rng_hgt estimator_aid_src_airspeed -# TOPICS estimator_aid_src_fake_hgt +# TOPICS estimator_aid_src_fake_hgt estimator_aid_src_sideslip # TOPICS estimator_aid_src_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 356c63ecb1..7d818b3bdb 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -46,6 +46,8 @@ if(${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0) set(EKF_GENERATED_SRC_FILES ${EKF_DERIVATION_DIR}/generated/compute_airspeed_h_and_k.h ${EKF_DERIVATION_DIR}/generated/compute_airspeed_innov_and_innov_var.h + ${EKF_DERIVATION_DIR}/generated/compute_sideslip_h_and_k.h + ${EKF_DERIVATION_DIR}/generated/compute_sideslip_innov_and_innov_var.h ) add_custom_command( diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index b5b2afcacf..e9911dbc5b 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -646,7 +646,7 @@ void Ekf::controlAirDataFusion() // If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates const bool airspeed_timed_out = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6); - const bool sideslip_timed_out = isTimedOut(_time_last_beta_fuse, (uint64_t)10e6); + const bool sideslip_timed_out = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6); if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & SensorFusionMask::USE_DRAG))) { _control_status.flags.wind = false; @@ -701,21 +701,26 @@ void Ekf::controlBetaFusion() } // Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fuson had been enabled externally: - const bool beta_fusion_time_triggered = isTimedOut(_time_last_beta_fuse, _params.beta_avg_ft_us); + const bool beta_fusion_time_triggered = isTimedOut(_aid_src_sideslip.time_last_fuse, _params.beta_avg_ft_us); if (beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air) { + updateSideslip(_aid_src_sideslip); + _innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected; + // If starting wind state estimation, reset the wind states and covariances before fusing any data if (!_control_status.flags.wind) { // activate the wind states _control_status.flags.wind = true; // reset the timeout timers to prevent repeated resets - _time_last_beta_fuse = _imu_sample_delayed.time_us; + _aid_src_airspeed.time_last_fuse = _imu_sample_delayed.time_us; resetWind(); } - fuseSideslip(); + if (Vector2f(Vector2f(_state.vel) - _state.wind_vel).longerThan(7.f)) { + fuseSideslip(_aid_src_sideslip); + } } } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 49a459f3ea..4bffdf4a23 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -171,9 +171,9 @@ public: void getAirspeedInnovVar(float &airspeed_innov_var) const { airspeed_innov_var = _aid_src_airspeed.innovation_variance; } void getAirspeedInnovRatio(float &airspeed_innov_ratio) const { airspeed_innov_ratio = _aid_src_airspeed.test_ratio; } - void getBetaInnov(float &beta_innov) const { beta_innov = _beta_innov; } - void getBetaInnovVar(float &beta_innov_var) const { beta_innov_var = _beta_innov_var; } - void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _beta_test_ratio; } + void getBetaInnov(float &beta_innov) const { beta_innov = _aid_src_sideslip.innovation; } + void getBetaInnovVar(float &beta_innov_var) const { beta_innov_var = _aid_src_sideslip.innovation_variance; } + void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _aid_src_sideslip.test_ratio; } void getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; } void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; } @@ -414,6 +414,7 @@ public: const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); } const auto &aid_src_airspeed() const { return _aid_src_airspeed; } + const auto &aid_src_sideslip() const { return _aid_src_sideslip; } const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; } const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; } @@ -494,7 +495,6 @@ private: uint64_t _time_last_of_fuse{0}; ///< time the last fusion of optical flow measurements were performed (uSec) uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec) - uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec) uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec) uint64_t _time_last_healthy_rng_data{0}; uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source @@ -543,9 +543,6 @@ private: Vector2f _drag_innov{}; ///< multirotor drag measurement innovation (m/sec**2) Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2) - float _beta_innov{0.0f}; ///< synthetic sideslip measurement innovation (rad) - float _beta_innov_var{0.0f}; ///< synthetic sideslip measurement innovation variance (rad**2) - float _hagl_innov{0.0f}; ///< innovation of the last height above terrain measurement (m) float _hagl_innov_var{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2) @@ -566,6 +563,7 @@ private: estimator_aid_source_1d_s _aid_src_baro_hgt{}; estimator_aid_source_1d_s _aid_src_rng_hgt{}; estimator_aid_source_1d_s _aid_src_airspeed{}; + estimator_aid_source_1d_s _aid_src_sideslip{}; estimator_aid_source_2d_s _aid_src_fake_pos{}; estimator_aid_source_1d_s _aid_src_fake_hgt{}; @@ -692,7 +690,8 @@ private: void fuseAirspeed(estimator_aid_source_1d_s &airspeed); // fuse synthetic zero sideslip measurement - void fuseSideslip(); + void updateSideslip(estimator_aid_source_1d_s &_aid_src_sideslip) const; + void fuseSideslip(estimator_aid_source_1d_s &_aid_src_sideslip); // fuse body frame drag specific forces for multi-rotor wind estimation void fuseDrag(const dragSample &drag_sample); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index edf423549b..a1cdc447c1 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -957,7 +957,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f hagl = sqrtf(_hagl_test_ratio); // return the synthetic sideslip innovation test ratio - beta = sqrtf(_beta_test_ratio); + beta = sqrtf(_aid_src_sideslip.test_ratio); } // return a bitmask integer that describes which state estimates are valid @@ -1032,7 +1032,7 @@ void Ekf::updateHorizontalDeadReckoningstatus() const bool airDataAiding = _control_status.flags.wind && isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max) && - isRecent(_time_last_beta_fuse, _params.no_aid_timeout_max); + isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max); _control_status.flags.wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding; _control_status.flags.inertial_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding; diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 511f607c46..dcd323c28b 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -345,7 +345,6 @@ protected: Vector2f _ev_pos_test_ratio{}; // EV position innovation consistency check ratios float _optflow_test_ratio{}; // Optical flow innovation consistency check ratio float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio - float _beta_test_ratio{}; // sideslip innovation consistency check ratio Vector2f _drag_test_ratio{}; // drag innovation consistency check ratio innovation_fault_status_u _innov_check_fail_status{}; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 66bf54f980..ebb1289704 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -98,9 +98,58 @@ def compute_airspeed_h_and_k( airspeed_pred = vel_rel.norm(epsilon=epsilon) H = sf.V1(airspeed_pred).jacobian(state) - K = P * H.T / sm.Max(innov_var, epsilon) + K = P * H.T / sf.Max(innov_var, epsilon) + + return (H.T, K) + +def predict_sideslip( + state: VState, + epsilon: sf.Scalar +) -> (sf.Scalar): + + vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz]) + q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) + relative_wind_body = quat_to_rot(q_att).T * vel_rel + + # Small angle approximation of side slip model + # Protect division by zero using epsilon + sideslip_pred = add_epsilon_sign(relative_wind_body[1] / relative_wind_body[0], relative_wind_body[0], epsilon) + + return sideslip_pred + +def compute_sideslip_innov_and_innov_var( + state: VState, + P: MState, + R: sf.Scalar, + epsilon: sf.Scalar +) -> (sf.Scalar, sf.Scalar, sf.Scalar): + + sideslip_pred = predict_sideslip(state, epsilon); + + innov = 0.0 - sideslip_pred + + H = sf.V1(sideslip_pred).jacobian(state) + innov_var = (H * P * H.T + R)[0,0] + + return (innov, innov_var) + +def compute_sideslip_h_and_k( + state: VState, + P: MState, + innov_var: sf.Scalar, + epsilon: sf.Scalar +) -> (VState, VState): + + sideslip_pred = predict_sideslip(state, epsilon); + + H = sf.V1(sideslip_pred).jacobian(state) + + K = P * H.T / sf.Max(innov_var, epsilon) return (H.T, K) generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"]) + +generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"]) +generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h new file mode 100644 index 0000000000..ee1c190df4 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h @@ -0,0 +1,181 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// backends/cpp/templates/function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_sideslip_h_and_k + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * innov_var: Scalar + * epsilon: Scalar + * + * Outputs: + * H: Matrix24_1 + * K: Matrix24_1 + */ +template +void ComputeSideslipHAndK(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar innov_var, + const Scalar epsilon, matrix::Matrix* const H = nullptr, + matrix::Matrix* const K = nullptr) { + // Total ops: 539 + + // Input arrays + + // Intermediate terms (44) + const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp3 = -_tmp0 + _tmp1 + _tmp2; + const Scalar _tmp4 = -state(22, 0) + state(4, 0); + const Scalar _tmp5 = state(0, 0) * state(3, 0); + const Scalar _tmp6 = state(1, 0) * state(2, 0); + const Scalar _tmp7 = _tmp5 + _tmp6; + const Scalar _tmp8 = -state(23, 0) + state(5, 0); + const Scalar _tmp9 = 2 * _tmp8; + const Scalar _tmp10 = state(0, 0) * state(2, 0); + const Scalar _tmp11 = state(1, 0) * state(3, 0); + const Scalar _tmp12 = 2 * state(6, 0); + const Scalar _tmp13 = _tmp12 * (-_tmp10 + _tmp11) + _tmp3 * _tmp4 + _tmp7 * _tmp9; + const Scalar _tmp14 = + _tmp13 + epsilon * (2 * math::min(0, (((_tmp13) > 0) - ((_tmp13) < 0))) + 1); + const Scalar _tmp15 = Scalar(1.0) / (_tmp14); + const Scalar _tmp16 = 2 * _tmp4; + const Scalar _tmp17 = _tmp12 * state(1, 0) - _tmp16 * state(3, 0) + _tmp9 * state(0, 0); + const Scalar _tmp18 = _tmp16 * state(0, 0); + const Scalar _tmp19 = _tmp9 * state(3, 0); + const Scalar _tmp20 = _tmp12 * state(2, 0); + const Scalar _tmp21 = _tmp0 - _tmp1 + _tmp2; + const Scalar _tmp22 = -2 * _tmp5 + 2 * _tmp6; + const Scalar _tmp23 = state(2, 0) * state(3, 0); + const Scalar _tmp24 = state(0, 0) * state(1, 0); + const Scalar _tmp25 = + (_tmp12 * (_tmp23 + _tmp24) + _tmp21 * _tmp8 + _tmp22 * _tmp4) / std::pow(_tmp14, Scalar(2)); + const Scalar _tmp26 = _tmp15 * _tmp17 - _tmp25 * (_tmp18 + _tmp19 - _tmp20); + const Scalar _tmp27 = _tmp9 * state(1, 0); + const Scalar _tmp28 = _tmp16 * state(2, 0); + const Scalar _tmp29 = _tmp12 * state(0, 0); + const Scalar _tmp30 = _tmp12 * state(3, 0) + _tmp16 * state(1, 0) + _tmp9 * state(2, 0); + const Scalar _tmp31 = _tmp15 * (-_tmp27 + _tmp28 + _tmp29) - _tmp25 * _tmp30; + const Scalar _tmp32 = _tmp15 * _tmp30 - _tmp25 * (_tmp27 - _tmp28 - _tmp29); + const Scalar _tmp33 = _tmp15 * (-_tmp18 - _tmp19 + _tmp20) - _tmp17 * _tmp25; + const Scalar _tmp34 = _tmp25 * _tmp3; + const Scalar _tmp35 = 2 * _tmp5; + const Scalar _tmp36 = 2 * _tmp6; + const Scalar _tmp37 = _tmp15 * (-_tmp35 + _tmp36) - _tmp34; + const Scalar _tmp38 = _tmp15 * _tmp21; + const Scalar _tmp39 = -_tmp25 * (_tmp35 + _tmp36) + _tmp38; + const Scalar _tmp40 = _tmp15 * (2 * _tmp23 + 2 * _tmp24) - _tmp25 * (-2 * _tmp10 + 2 * _tmp11); + const Scalar _tmp41 = -_tmp15 * _tmp22 + _tmp34; + const Scalar _tmp42 = 2 * _tmp25 * _tmp7 - _tmp38; + const Scalar _tmp43 = Scalar(1.0) / (math::max(epsilon, innov_var)); + + // Output terms (2) + if (H != nullptr) { + matrix::Matrix& _H = (*H); + + _H.setZero(); + + _H(0, 0) = _tmp26; + _H(1, 0) = _tmp31; + _H(2, 0) = _tmp32; + _H(3, 0) = _tmp33; + _H(4, 0) = _tmp37; + _H(5, 0) = _tmp39; + _H(6, 0) = _tmp40; + _H(22, 0) = _tmp41; + _H(23, 0) = _tmp42; + } + + if (K != nullptr) { + matrix::Matrix& _K = (*K); + + _K(0, 0) = _tmp43 * (P(0, 0) * _tmp26 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 + + P(0, 22) * _tmp41 + P(0, 23) * _tmp42 + P(0, 3) * _tmp33 + + P(0, 4) * _tmp37 + P(0, 5) * _tmp39 + P(0, 6) * _tmp40); + _K(1, 0) = _tmp43 * (P(1, 0) * _tmp26 + P(1, 1) * _tmp31 + P(1, 2) * _tmp32 + + P(1, 22) * _tmp41 + P(1, 23) * _tmp42 + P(1, 3) * _tmp33 + + P(1, 4) * _tmp37 + P(1, 5) * _tmp39 + P(1, 6) * _tmp40); + _K(2, 0) = _tmp43 * (P(2, 0) * _tmp26 + P(2, 1) * _tmp31 + P(2, 2) * _tmp32 + + P(2, 22) * _tmp41 + P(2, 23) * _tmp42 + P(2, 3) * _tmp33 + + P(2, 4) * _tmp37 + P(2, 5) * _tmp39 + P(2, 6) * _tmp40); + _K(3, 0) = _tmp43 * (P(3, 0) * _tmp26 + P(3, 1) * _tmp31 + P(3, 2) * _tmp32 + + P(3, 22) * _tmp41 + P(3, 23) * _tmp42 + P(3, 3) * _tmp33 + + P(3, 4) * _tmp37 + P(3, 5) * _tmp39 + P(3, 6) * _tmp40); + _K(4, 0) = _tmp43 * (P(4, 0) * _tmp26 + P(4, 1) * _tmp31 + P(4, 2) * _tmp32 + + P(4, 22) * _tmp41 + P(4, 23) * _tmp42 + P(4, 3) * _tmp33 + + P(4, 4) * _tmp37 + P(4, 5) * _tmp39 + P(4, 6) * _tmp40); + _K(5, 0) = _tmp43 * (P(5, 0) * _tmp26 + P(5, 1) * _tmp31 + P(5, 2) * _tmp32 + + P(5, 22) * _tmp41 + P(5, 23) * _tmp42 + P(5, 3) * _tmp33 + + P(5, 4) * _tmp37 + P(5, 5) * _tmp39 + P(5, 6) * _tmp40); + _K(6, 0) = _tmp43 * (P(6, 0) * _tmp26 + P(6, 1) * _tmp31 + P(6, 2) * _tmp32 + + P(6, 22) * _tmp41 + P(6, 23) * _tmp42 + P(6, 3) * _tmp33 + + P(6, 4) * _tmp37 + P(6, 5) * _tmp39 + P(6, 6) * _tmp40); + _K(7, 0) = _tmp43 * (P(7, 0) * _tmp26 + P(7, 1) * _tmp31 + P(7, 2) * _tmp32 + + P(7, 22) * _tmp41 + P(7, 23) * _tmp42 + P(7, 3) * _tmp33 + + P(7, 4) * _tmp37 + P(7, 5) * _tmp39 + P(7, 6) * _tmp40); + _K(8, 0) = _tmp43 * (P(8, 0) * _tmp26 + P(8, 1) * _tmp31 + P(8, 2) * _tmp32 + + P(8, 22) * _tmp41 + P(8, 23) * _tmp42 + P(8, 3) * _tmp33 + + P(8, 4) * _tmp37 + P(8, 5) * _tmp39 + P(8, 6) * _tmp40); + _K(9, 0) = _tmp43 * (P(9, 0) * _tmp26 + P(9, 1) * _tmp31 + P(9, 2) * _tmp32 + + P(9, 22) * _tmp41 + P(9, 23) * _tmp42 + P(9, 3) * _tmp33 + + P(9, 4) * _tmp37 + P(9, 5) * _tmp39 + P(9, 6) * _tmp40); + _K(10, 0) = _tmp43 * (P(10, 0) * _tmp26 + P(10, 1) * _tmp31 + P(10, 2) * _tmp32 + + P(10, 22) * _tmp41 + P(10, 23) * _tmp42 + P(10, 3) * _tmp33 + + P(10, 4) * _tmp37 + P(10, 5) * _tmp39 + P(10, 6) * _tmp40); + _K(11, 0) = _tmp43 * (P(11, 0) * _tmp26 + P(11, 1) * _tmp31 + P(11, 2) * _tmp32 + + P(11, 22) * _tmp41 + P(11, 23) * _tmp42 + P(11, 3) * _tmp33 + + P(11, 4) * _tmp37 + P(11, 5) * _tmp39 + P(11, 6) * _tmp40); + _K(12, 0) = _tmp43 * (P(12, 0) * _tmp26 + P(12, 1) * _tmp31 + P(12, 2) * _tmp32 + + P(12, 22) * _tmp41 + P(12, 23) * _tmp42 + P(12, 3) * _tmp33 + + P(12, 4) * _tmp37 + P(12, 5) * _tmp39 + P(12, 6) * _tmp40); + _K(13, 0) = _tmp43 * (P(13, 0) * _tmp26 + P(13, 1) * _tmp31 + P(13, 2) * _tmp32 + + P(13, 22) * _tmp41 + P(13, 23) * _tmp42 + P(13, 3) * _tmp33 + + P(13, 4) * _tmp37 + P(13, 5) * _tmp39 + P(13, 6) * _tmp40); + _K(14, 0) = _tmp43 * (P(14, 0) * _tmp26 + P(14, 1) * _tmp31 + P(14, 2) * _tmp32 + + P(14, 22) * _tmp41 + P(14, 23) * _tmp42 + P(14, 3) * _tmp33 + + P(14, 4) * _tmp37 + P(14, 5) * _tmp39 + P(14, 6) * _tmp40); + _K(15, 0) = _tmp43 * (P(15, 0) * _tmp26 + P(15, 1) * _tmp31 + P(15, 2) * _tmp32 + + P(15, 22) * _tmp41 + P(15, 23) * _tmp42 + P(15, 3) * _tmp33 + + P(15, 4) * _tmp37 + P(15, 5) * _tmp39 + P(15, 6) * _tmp40); + _K(16, 0) = _tmp43 * (P(16, 0) * _tmp26 + P(16, 1) * _tmp31 + P(16, 2) * _tmp32 + + P(16, 22) * _tmp41 + P(16, 23) * _tmp42 + P(16, 3) * _tmp33 + + P(16, 4) * _tmp37 + P(16, 5) * _tmp39 + P(16, 6) * _tmp40); + _K(17, 0) = _tmp43 * (P(17, 0) * _tmp26 + P(17, 1) * _tmp31 + P(17, 2) * _tmp32 + + P(17, 22) * _tmp41 + P(17, 23) * _tmp42 + P(17, 3) * _tmp33 + + P(17, 4) * _tmp37 + P(17, 5) * _tmp39 + P(17, 6) * _tmp40); + _K(18, 0) = _tmp43 * (P(18, 0) * _tmp26 + P(18, 1) * _tmp31 + P(18, 2) * _tmp32 + + P(18, 22) * _tmp41 + P(18, 23) * _tmp42 + P(18, 3) * _tmp33 + + P(18, 4) * _tmp37 + P(18, 5) * _tmp39 + P(18, 6) * _tmp40); + _K(19, 0) = _tmp43 * (P(19, 0) * _tmp26 + P(19, 1) * _tmp31 + P(19, 2) * _tmp32 + + P(19, 22) * _tmp41 + P(19, 23) * _tmp42 + P(19, 3) * _tmp33 + + P(19, 4) * _tmp37 + P(19, 5) * _tmp39 + P(19, 6) * _tmp40); + _K(20, 0) = _tmp43 * (P(20, 0) * _tmp26 + P(20, 1) * _tmp31 + P(20, 2) * _tmp32 + + P(20, 22) * _tmp41 + P(20, 23) * _tmp42 + P(20, 3) * _tmp33 + + P(20, 4) * _tmp37 + P(20, 5) * _tmp39 + P(20, 6) * _tmp40); + _K(21, 0) = _tmp43 * (P(21, 0) * _tmp26 + P(21, 1) * _tmp31 + P(21, 2) * _tmp32 + + P(21, 22) * _tmp41 + P(21, 23) * _tmp42 + P(21, 3) * _tmp33 + + P(21, 4) * _tmp37 + P(21, 5) * _tmp39 + P(21, 6) * _tmp40); + _K(22, 0) = _tmp43 * (P(22, 0) * _tmp26 + P(22, 1) * _tmp31 + P(22, 2) * _tmp32 + + P(22, 22) * _tmp41 + P(22, 23) * _tmp42 + P(22, 3) * _tmp33 + + P(22, 4) * _tmp37 + P(22, 5) * _tmp39 + P(22, 6) * _tmp40); + _K(23, 0) = _tmp43 * (P(23, 0) * _tmp26 + P(23, 1) * _tmp31 + P(23, 2) * _tmp32 + + P(23, 22) * _tmp41 + P(23, 23) * _tmp42 + P(23, 3) * _tmp33 + + P(23, 4) * _tmp37 + P(23, 5) * _tmp39 + P(23, 6) * _tmp40); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h new file mode 100644 index 0000000000..ee96db3c05 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h @@ -0,0 +1,126 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// backends/cpp/templates/function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_sideslip_innov_and_innov_var + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * innov: Scalar + * innov_var: Scalar + */ +template +void ComputeSideslipInnovAndInnovVar(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, + const Scalar epsilon, Scalar* const innov = nullptr, + Scalar* const innov_var = nullptr) { + // Total ops: 277 + + // Input arrays + + // Intermediate terms (44) + const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp3 = -_tmp0 + _tmp1 + _tmp2; + const Scalar _tmp4 = -state(22, 0) + state(4, 0); + const Scalar _tmp5 = state(0, 0) * state(3, 0); + const Scalar _tmp6 = state(1, 0) * state(2, 0); + const Scalar _tmp7 = _tmp5 + _tmp6; + const Scalar _tmp8 = -state(23, 0) + state(5, 0); + const Scalar _tmp9 = 2 * _tmp8; + const Scalar _tmp10 = state(0, 0) * state(2, 0); + const Scalar _tmp11 = state(1, 0) * state(3, 0); + const Scalar _tmp12 = 2 * state(6, 0); + const Scalar _tmp13 = _tmp12 * (-_tmp10 + _tmp11) + _tmp3 * _tmp4 + _tmp7 * _tmp9; + const Scalar _tmp14 = + _tmp13 + epsilon * (2 * math::min(0, (((_tmp13) > 0) - ((_tmp13) < 0))) + 1); + const Scalar _tmp15 = Scalar(1.0) / (_tmp14); + const Scalar _tmp16 = _tmp0 - _tmp1 + _tmp2; + const Scalar _tmp17 = -_tmp5 + _tmp6; + const Scalar _tmp18 = 2 * _tmp4; + const Scalar _tmp19 = state(2, 0) * state(3, 0); + const Scalar _tmp20 = state(0, 0) * state(1, 0); + const Scalar _tmp21 = _tmp12 * (_tmp19 + _tmp20) + _tmp16 * _tmp8 + _tmp17 * _tmp18; + const Scalar _tmp22 = _tmp9 * state(1, 0); + const Scalar _tmp23 = _tmp18 * state(2, 0); + const Scalar _tmp24 = _tmp12 * state(0, 0); + const Scalar _tmp25 = _tmp12 * state(3, 0) + _tmp18 * state(1, 0) + _tmp9 * state(2, 0); + const Scalar _tmp26 = _tmp21 / std::pow(_tmp14, Scalar(2)); + const Scalar _tmp27 = _tmp15 * (-_tmp22 + _tmp23 + _tmp24) - _tmp25 * _tmp26; + const Scalar _tmp28 = _tmp15 * _tmp16; + const Scalar _tmp29 = 2 * _tmp5; + const Scalar _tmp30 = 2 * _tmp6; + const Scalar _tmp31 = -_tmp26 * (_tmp29 + _tmp30) + _tmp28; + const Scalar _tmp32 = _tmp12 * state(1, 0) - _tmp18 * state(3, 0) + _tmp9 * state(0, 0); + const Scalar _tmp33 = _tmp18 * state(0, 0); + const Scalar _tmp34 = _tmp9 * state(3, 0); + const Scalar _tmp35 = _tmp12 * state(2, 0); + const Scalar _tmp36 = _tmp15 * _tmp32 - _tmp26 * (_tmp33 + _tmp34 - _tmp35); + const Scalar _tmp37 = 2 * _tmp26 * _tmp7 - _tmp28; + const Scalar _tmp38 = _tmp26 * _tmp3; + const Scalar _tmp39 = -2 * _tmp15 * _tmp17 + _tmp38; + const Scalar _tmp40 = _tmp15 * _tmp25 - _tmp26 * (_tmp22 - _tmp23 - _tmp24); + const Scalar _tmp41 = _tmp15 * (-_tmp33 - _tmp34 + _tmp35) - _tmp26 * _tmp32; + const Scalar _tmp42 = _tmp15 * (2 * _tmp19 + 2 * _tmp20) - _tmp26 * (-2 * _tmp10 + 2 * _tmp11); + const Scalar _tmp43 = _tmp15 * (-_tmp29 + _tmp30) - _tmp38; + + // Output terms (2) + if (innov != nullptr) { + Scalar& _innov = (*innov); + + _innov = -_tmp15 * _tmp21; + } + + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = R + + _tmp27 * (P(0, 1) * _tmp36 + P(1, 1) * _tmp27 + P(2, 1) * _tmp40 + + P(22, 1) * _tmp39 + P(23, 1) * _tmp37 + P(3, 1) * _tmp41 + + P(4, 1) * _tmp43 + P(5, 1) * _tmp31 + P(6, 1) * _tmp42) + + _tmp31 * (P(0, 5) * _tmp36 + P(1, 5) * _tmp27 + P(2, 5) * _tmp40 + + P(22, 5) * _tmp39 + P(23, 5) * _tmp37 + P(3, 5) * _tmp41 + + P(4, 5) * _tmp43 + P(5, 5) * _tmp31 + P(6, 5) * _tmp42) + + _tmp36 * (P(0, 0) * _tmp36 + P(1, 0) * _tmp27 + P(2, 0) * _tmp40 + + P(22, 0) * _tmp39 + P(23, 0) * _tmp37 + P(3, 0) * _tmp41 + + P(4, 0) * _tmp43 + P(5, 0) * _tmp31 + P(6, 0) * _tmp42) + + _tmp37 * (P(0, 23) * _tmp36 + P(1, 23) * _tmp27 + P(2, 23) * _tmp40 + + P(22, 23) * _tmp39 + P(23, 23) * _tmp37 + P(3, 23) * _tmp41 + + P(4, 23) * _tmp43 + P(5, 23) * _tmp31 + P(6, 23) * _tmp42) + + _tmp39 * (P(0, 22) * _tmp36 + P(1, 22) * _tmp27 + P(2, 22) * _tmp40 + + P(22, 22) * _tmp39 + P(23, 22) * _tmp37 + P(3, 22) * _tmp41 + + P(4, 22) * _tmp43 + P(5, 22) * _tmp31 + P(6, 22) * _tmp42) + + _tmp40 * (P(0, 2) * _tmp36 + P(1, 2) * _tmp27 + P(2, 2) * _tmp40 + + P(22, 2) * _tmp39 + P(23, 2) * _tmp37 + P(3, 2) * _tmp41 + + P(4, 2) * _tmp43 + P(5, 2) * _tmp31 + P(6, 2) * _tmp42) + + _tmp41 * (P(0, 3) * _tmp36 + P(1, 3) * _tmp27 + P(2, 3) * _tmp40 + + P(22, 3) * _tmp39 + P(23, 3) * _tmp37 + P(3, 3) * _tmp41 + + P(4, 3) * _tmp43 + P(5, 3) * _tmp31 + P(6, 3) * _tmp42) + + _tmp42 * (P(0, 6) * _tmp36 + P(1, 6) * _tmp27 + P(2, 6) * _tmp40 + + P(22, 6) * _tmp39 + P(23, 6) * _tmp37 + P(3, 6) * _tmp41 + + P(4, 6) * _tmp43 + P(5, 6) * _tmp31 + P(6, 6) * _tmp42) + + _tmp43 * (P(0, 4) * _tmp36 + P(1, 4) * _tmp27 + P(2, 4) * _tmp40 + + P(22, 4) * _tmp39 + P(23, 4) * _tmp37 + P(3, 4) * _tmp41 + + P(4, 4) * _tmp43 + P(5, 4) * _tmp31 + P(6, 4) * _tmp42); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index b8cf037095..dae7518e4d 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/sideslip_fusion.cpp @@ -42,176 +42,87 @@ */ #include "ekf.h" +#include "python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h" +#include "python/ekf_derivation/generated/compute_sideslip_h_and_k.h" #include -void Ekf::fuseSideslip() +void Ekf::updateSideslip(estimator_aid_source_1d_s &sideslip) const { - // get latest estimated orientation - const float q0 = _state.quat_nominal(0); - const float q1 = _state.quat_nominal(1); - const float q2 = _state.quat_nominal(2); - const float q3 = _state.quat_nominal(3); + // reset flags + resetEstimatorAidStatusFlags(sideslip); - // get latest velocity in earth frame - const float vn = _state.vel(0); - const float ve = _state.vel(1); - const float vd = _state.vel(2); + const float R = sq(_params.beta_noise); // observation noise variance - // get latest wind velocity in earth frame - const float vwn = _state.wind_vel(0); - const float vwe = _state.wind_vel(1); + float innov = 0.f; + float innov_var = 0.f; + sym::ComputeSideslipInnovAndInnovVar(getStateAtFusionHorizonAsVector(), P, R, FLT_EPSILON, &innov, &innov_var); - // calculate relative wind velocity in earth frame and rotate into body frame - const Vector3f rel_wind_earth(vn - vwn, ve - vwe, vd); - const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal); - const Vector3f rel_wind_body = earth_to_body * rel_wind_earth; + sideslip.observation = 0.f; + sideslip.observation_variance = R; + sideslip.innovation = innov; + sideslip.innovation_variance = innov_var; - // perform fusion of assumed sideslip = 0 - if (rel_wind_body.norm() > 7.0f) { - const float R_BETA = sq(_params.beta_noise); // observation noise variance + sideslip.fusion_enabled = _control_status.flags.fuse_aspd; - // determine if we need the sideslip fusion to correct states other than wind - bool update_wind_only = !_control_status.flags.wind_dead_reckoning; + sideslip.timestamp_sample = _imu_sample_delayed.time_us; - // Intermediate Values - const float HK0 = vn - vwn; - const float HK1 = ve - vwe; - const float HK2 = HK0*q0 + HK1*q3 - q2*vd; - const float HK3 = q0*q2 - q1*q3; - const float HK4 = 2*vd; - const float HK5 = q0*q3; - const float HK6 = q1*q2; - const float HK7 = 2*HK5 + 2*HK6; - const float HK8 = ecl::powf(q0, 2); - const float HK9 = ecl::powf(q3, 2); - const float HK10 = HK8 - HK9; - const float HK11 = ecl::powf(q1, 2); - const float HK12 = ecl::powf(q2, 2); - const float HK13 = HK11 - HK12; - const float HK14 = HK10 + HK13; - const float HK15 = HK0*HK14 + HK1*HK7 - HK3*HK4; - const float HK16 = 1.0F/HK15; - const float HK17 = q0*q1 + q2*q3; - const float HK18 = HK10 - HK11 + HK12; - const float HK19 = HK16*(-2*HK0*(HK5 - HK6) + HK1*HK18 + HK17*HK4); - const float HK20 = -HK0*q3 + HK1*q0 + q1*vd; - const float HK21 = -HK19*HK2 + HK20; - const float HK22 = 2*HK16; - const float HK23 = HK0*q1 + HK1*q2 + q3*vd; - const float HK24 = HK0*q2 - HK1*q1 + q0*vd; - const float HK25 = -HK19*HK23 + HK24; - const float HK26 = HK19*HK24 + HK23; - const float HK27 = HK19*HK20 + HK2; - const float HK28 = HK14*HK19 + 2*HK5 - 2*HK6; - const float HK29 = HK16*HK28; - const float HK30 = HK19*HK7; - const float HK31 = HK17 + HK19*HK3; - const float HK32 = HK13 + HK30 - HK8 + HK9; - const float HK33 = 2*HK31; - const float HK34 = 2*HK26; - const float HK35 = 2*HK25; - const float HK36 = 2*HK27; - const float HK37 = 2*HK21; - const float HK38 = HK28*P(0,22) - HK28*P(0,4) + HK32*P(0,23) - HK32*P(0,5) + HK33*P(0,6) + HK34*P(0,2) + HK35*P(0,1) - HK36*P(0,3) + HK37*P(0,0); - const float HK39 = ecl::powf(HK15, -2); - const float HK40 = -HK28*P(4,6) + HK28*P(6,22) - HK32*P(5,6) + HK32*P(6,23) + HK33*P(6,6) + HK34*P(2,6) + HK35*P(1,6) - HK36*P(3,6) + HK37*P(0,6); - const float HK41 = HK32*P(5,23); - const float HK42 = HK28*P(22,23) - HK28*P(4,23) + HK32*P(23,23) + HK33*P(6,23) + HK34*P(2,23) + HK35*P(1,23) - HK36*P(3,23) + HK37*P(0,23) - HK41; - const float HK43 = HK32*HK39; - const float HK44 = HK28*P(4,22); - const float HK45 = HK28*P(22,22) + HK32*P(22,23) - HK32*P(5,22) + HK33*P(6,22) + HK34*P(2,22) + HK35*P(1,22) - HK36*P(3,22) + HK37*P(0,22) - HK44; - const float HK46 = HK28*HK39; - const float HK47 = -HK28*P(4,5) + HK28*P(5,22) - HK32*P(5,5) + HK33*P(5,6) + HK34*P(2,5) + HK35*P(1,5) - HK36*P(3,5) + HK37*P(0,5) + HK41; - const float HK48 = -HK28*P(4,4) + HK32*P(4,23) - HK32*P(4,5) + HK33*P(4,6) + HK34*P(2,4) + HK35*P(1,4) - HK36*P(3,4) + HK37*P(0,4) + HK44; - const float HK49 = HK28*P(2,22) - HK28*P(2,4) + HK32*P(2,23) - HK32*P(2,5) + HK33*P(2,6) + HK34*P(2,2) + HK35*P(1,2) - HK36*P(2,3) + HK37*P(0,2); - const float HK50 = HK28*P(1,22) - HK28*P(1,4) + HK32*P(1,23) - HK32*P(1,5) + HK33*P(1,6) + HK34*P(1,2) + HK35*P(1,1) - HK36*P(1,3) + HK37*P(0,1); - const float HK51 = HK28*P(3,22) - HK28*P(3,4) + HK32*P(3,23) - HK32*P(3,5) + HK33*P(3,6) + HK34*P(2,3) + HK35*P(1,3) - HK36*P(3,3) + HK37*P(0,3); - //const float HK52 = HK16/(HK33*HK39*HK40 + HK34*HK39*HK49 + HK35*HK39*HK50 - HK36*HK39*HK51 + HK37*HK38*HK39 + HK42*HK43 - HK43*HK47 + HK45*HK46 - HK46*HK48 + R_BETA); + const float innov_gate = fmaxf(_params.beta_innov_gate, 1.f); + setEstimatorAidStatusTestRatio(sideslip, innov_gate); +} - // innovation variance - _beta_innov_var = (HK33*HK39*HK40 + HK34*HK39*HK49 + HK35*HK39*HK50 - HK36*HK39*HK51 + HK37*HK38*HK39 + HK42*HK43 - HK43*HK47 + HK45*HK46 - HK46*HK48 + R_BETA); +void Ekf::fuseSideslip(estimator_aid_source_1d_s &sideslip) +{ + if (sideslip.innovation_rejected) { + return; + } + // determine if we need the sideslip fusion to correct states other than wind + bool update_wind_only = !_control_status.flags.wind_dead_reckoning; - // Reset covariance and states if the calculation is badly conditioned - if (_beta_innov_var < R_BETA) { - _fault_status.flags.bad_sideslip = true; + // Reset covariance and states if the calculation is badly conditioned + if ((sideslip.innovation_variance < sideslip.observation_variance) + || (sideslip.innovation_variance < FLT_EPSILON)) { + _fault_status.flags.bad_sideslip = true; - // if we are getting aiding from other sources, warn and reset the wind states and covariances only - const char *action_string = nullptr; + // if we are getting aiding from other sources, warn and reset the wind states and covariances only + const char *action_string = nullptr; - if (update_wind_only) { - resetWind(); - action_string = "wind"; - - } else { - initialiseCovariance(); - _state.wind_vel.setZero(); - action_string = "full"; - } - - ECL_ERR("sideslip badly conditioned - %s covariance reset", action_string); - - return; - } - - _fault_status.flags.bad_sideslip = false; - const float HK52 = HK16 / _beta_innov_var; - - // Calculate predicted sideslip angle and innovation using small angle approximation - _beta_innov = rel_wind_body(1) / rel_wind_body(0); - - // Compute the ratio of innovation to gate size - _beta_test_ratio = sq(_beta_innov) / (sq(fmaxf(_params.beta_innov_gate, 1.0f)) * _beta_innov_var); - - // if the innovation consistency check fails then don't fuse the sample and indicate bad beta health - if (_beta_test_ratio > 1.0f) { - _innov_check_fail_status.flags.reject_sideslip = true; - return; + if (update_wind_only) { + resetWind(); + action_string = "wind"; } else { - _innov_check_fail_status.flags.reject_sideslip = false; + initialiseCovariance(); + _state.wind_vel.setZero(); + action_string = "full"; } - // Observation Jacobians - SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion; - Hfusion.at<0>() = HK21*HK22; - Hfusion.at<1>() = HK22*HK25; - Hfusion.at<2>() = HK22*HK26; - Hfusion.at<3>() = -HK22*HK27; - Hfusion.at<4>() = -HK29; - Hfusion.at<5>() = HK16*(HK18 - HK30); - Hfusion.at<6>() = HK22*HK31; - Hfusion.at<22>() = HK29; - Hfusion.at<23>() = HK16*HK32; + ECL_ERR("sideslip badly conditioned - %s covariance reset", action_string); - // Calculate Kalman gains - Vector24f Kfusion; + return; + } - if (!update_wind_only) { + _fault_status.flags.bad_sideslip = false; - Kfusion(0) = HK38*HK52; - Kfusion(1) = HK50*HK52; - Kfusion(2) = HK49*HK52; - Kfusion(3) = HK51*HK52; - Kfusion(4) = HK48*HK52; - Kfusion(5) = HK47*HK52; - Kfusion(6) = HK40*HK52; + Vector24f H; // Observation jacobian + Vector24f K; // Kalman gain vector - for (unsigned row = 7; row <= 21; row++) { - Kfusion(row) = HK52*(HK28*P(row,22) - HK28*P(4,row) + HK32*P(row,23) - HK32*P(5,row) + HK33*P(6,row) + HK34*P(2,row) + HK35*P(1,row) - HK36*P(3,row) + HK37*P(0,row)); - } + sym::ComputeSideslipHAndK(getStateAtFusionHorizonAsVector(), P, sideslip.innovation_variance, FLT_EPSILON, &H, &K); - } + SparseVector24f<0,1,2,3,4,5,6,22,23> H_sparse(H); - Kfusion(22) = HK45*HK52; - Kfusion(23) = HK42*HK52; - - const bool is_fused = measurementUpdate(Kfusion, Hfusion, _beta_innov); - - _fault_status.flags.bad_sideslip = !is_fused; - - if (is_fused) { - _time_last_beta_fuse = _imu_sample_delayed.time_us; + if (update_wind_only) { + for (unsigned row = 0; row <= 21; row++) { + K(row) = 0.f; } } + + const bool is_fused = measurementUpdate(K, H_sparse, sideslip.innovation); + + sideslip.fused = is_fused; + _fault_status.flags.bad_sideslip = !is_fused; + + if (is_fused) { + sideslip.time_last_fuse = _imu_sample_delayed.time_us; + } } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index dc49593078..2809c95135 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -714,6 +714,9 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) // airspeed PublishAidSourceStatus(_ekf.aid_src_airspeed(), _status_airspeed_pub_last, _estimator_aid_src_airspeed_pub); + // sideslip + PublishAidSourceStatus(_ekf.aid_src_sideslip(), _status_sideslip_pub_last, _estimator_aid_src_sideslip_pub); + // baro height PublishAidSourceStatus(_ekf.aid_src_baro_hgt(), _status_baro_hgt_pub_last, _estimator_aid_src_baro_hgt_pub); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 81f559e003..120caab1a4 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -262,6 +262,7 @@ private: hrt_abstime _last_gps_status_published{0}; hrt_abstime _status_airspeed_pub_last{0}; + hrt_abstime _status_sideslip_pub_last{0}; hrt_abstime _status_baro_hgt_pub_last{0}; hrt_abstime _status_rng_hgt_pub_last{0}; @@ -349,6 +350,7 @@ private: uORB::PublicationMulti _estimator_aid_src_airspeed_pub{ORB_ID(estimator_aid_src_airspeed)}; uORB::PublicationMulti _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)}; uORB::PublicationMulti _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)}; + uORB::PublicationMulti _estimator_aid_src_sideslip_pub{ORB_ID(estimator_aid_src_sideslip)}; uORB::PublicationMulti _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)}; uORB::PublicationMulti _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index a835ddfeba..9639654ba3 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -273,6 +273,7 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("estimator_aid_src_mag_heading", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_aid_src_mag", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_aid_src_ev_yaw", 0, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_sideslip", 0, MAX_ESTIMATOR_INSTANCES); #endif /* CONFIG_ARCH_BOARD_PX4_SITL */ }