diff --git a/msg/VehicleCommand.msg b/msg/VehicleCommand.msg index b147bef092..0c1ce85d5f 100644 --- a/msg/VehicleCommand.msg +++ b/msg/VehicleCommand.msg @@ -102,6 +102,7 @@ uint16 VEHICLE_CMD_FIXED_MAG_CAL_YAW = 42006 # Magnetometer calibrati uint16 VEHICLE_CMD_DO_WINCH = 42600 # Command to operate winch. uint16 VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE = 43003 # external reset of estimator global position when deadreckoning +uint16 VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE = 43004 # PX4 vehicle commands (beyond 16 bit mavlink commands) uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # start of PX4 internal only vehicle commands (> UINT16_MAX) diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index a71c1b06e6..35c6a7b779 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -214,6 +214,10 @@ if(CONFIG_EKF2_TERRAIN) list(APPEND EKF_SRCS EKF/terrain_control.cpp) endif() +if(CONFIG_EKF2_WIND) + list(APPEND EKF_SRCS EKF/wind.cpp) +endif () + add_subdirectory(EKF) px4_add_module( diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index f404817ef4..954123b250 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -138,6 +138,10 @@ if(CONFIG_EKF2_TERRAIN) list(APPEND EKF_SRCS terrain_control.cpp) endif() +if(CONFIG_EKF2_WIND) + list(APPEND EKF_SRCS wind.cpp) +endif () + include_directories(${CMAKE_CURRENT_SOURCE_DIR}) add_library(ecl_EKF ${EKF_SRCS} diff --git a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp index 46dc33e491..5cfb121079 100644 --- a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp @@ -62,6 +62,10 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) _control_status.flags.wind = false; } + if (_control_status.flags.wind && _external_wind_init) { + _external_wind_init = false; + } + #if defined(CONFIG_EKF2_GNSS) // clear yaw estimator airspeed (updated later with true airspeed if airspeed fusion is active) @@ -128,10 +132,11 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) if (_control_status.flags.inertial_dead_reckoning && !is_airspeed_consistent) { resetVelUsingAirspeed(airspeed_sample); - } else if (!_control_status.flags.wind || getWindVelocityVariance().longerThan(_params.initial_wind_uncertainty)) { - // If starting wind state estimation, reset the wind states and covariances before fusing any data - // Also catch the case where sideslip fusion enabled wind estimation recently and didn't converge yet. + } else if (!_external_wind_init + && (!_control_status.flags.wind + || getWindVelocityVariance().longerThan(sq(_params.initial_wind_uncertainty)))) { resetWindUsingAirspeed(airspeed_sample); + _aid_src_airspeed.time_last_fuse = _time_delayed_us; } _control_status.flags.wind = true; diff --git a/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp index 439cbdb446..41c7dc2f12 100644 --- a/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp @@ -48,9 +48,11 @@ void Ekf::controlDragFusion(const imuSample &imu_delayed) if ((_params.drag_ctrl > 0) && _drag_buffer) { if (!_control_status.flags.wind && !_control_status.flags.fake_pos && _control_status.flags.in_air) { - // reset the wind states and covariances when starting drag accel fusion _control_status.flags.wind = true; - resetWindToZero(); + + if (!_external_wind_init) { + resetWindCov(); + } } dragSample drag_sample; diff --git a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp index f7365b7333..193af436f3 100644 --- a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp @@ -64,16 +64,12 @@ void Ekf::controlBetaFusion(const imuSample &imu_delayed) 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 + if (fuseSideslip(_aid_src_sideslip)) { _control_status.flags.wind = true; - // reset the timeout timers to prevent repeated resets - _aid_src_sideslip.time_last_fuse = imu_delayed.time_us; - resetWindToZero(); - } - fuseSideslip(_aid_src_sideslip); + } else if (!_external_wind_init && !_control_status.flags.wind) { + resetWindCov(); + } } } } @@ -96,10 +92,10 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &aid_src) const math::max(_params.beta_innov_gate, 1.f)); // innovation gate } -void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) +bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) { if (sideslip.innovation_rejected) { - return; + return false; } // determine if we need the sideslip fusion to correct states other than wind @@ -114,7 +110,7 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) const char *action_string = nullptr; if (update_wind_only) { - resetWind(); + resetWindCov(); action_string = "wind"; } else { @@ -125,7 +121,7 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) ECL_ERR("sideslip badly conditioned - %s covariance reset", action_string); - return; + return false; } _fault_status.flags.bad_sideslip = false; @@ -150,4 +146,6 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) if (is_fused) { sideslip.time_last_fuse = _time_delayed_us; } + + return is_fused; } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 2e317cd71f..6294310bbf 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -640,6 +640,7 @@ bool yaw_aligned_to_imu_gps : bool reset_hgt_to_ev : 1; ///< 16 - true when the vertical position state is reset to the ev measurement bool reset_pos_to_ext_obs : 1; ///< 17 - true when horizontal position was reset to an external observation while deadreckoning + bool reset_wind_to_ext_obs : 1; ///< 18 - true when wind states were reset to an external observation } flags; uint32_t value; }; diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 58e83b3bd7..28decbc0d1 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -200,15 +200,15 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) #if defined(CONFIG_EKF2_WIND) - if (_control_status.flags.wind) { - // wind vel: add process noise - float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, - 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf)); + // wind vel: add process noise + if (!_external_wind_init) { + float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f) + * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf)); float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt; for (unsigned index = 0; index < State::wind_vel.dof; index++) { const unsigned i = State::wind_vel.idx + index; - P(i, i) += wind_vel_process_noise; + P(i, i) = fminf(P(i, i) + wind_vel_process_noise, sq(_params.initial_wind_uncertainty)); } } @@ -348,11 +348,3 @@ void Ekf::resetMagCov() P.uncorrelateCovarianceSetVariance(State::mag_B.idx, sq(_params.mag_noise)); } #endif // CONFIG_EKF2_MAGNETOMETER - -#if defined(CONFIG_EKF2_WIND) -void Ekf::resetWindCov() -{ - // start with a small initial uncertainty to improve the initial estimate - P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, sq(_params.initial_wind_uncertainty)); -} -#endif // CONFIG_EKF2_WIND diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 51212ebef1..f3f055434e 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -526,6 +526,18 @@ public: bool resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation); + /** + * @brief Resets the wind states to an external observation + * + * @param wind_speed The wind speed in m/s + * @param wind_direction The azimuth (from true north) to where the wind is heading in radians + * @param wind_speed_accuracy The 1 sigma accuracy of the wind speed estimate in m/s + * @param wind_direction_accuracy The 1 sigma accuracy of the wind direction estimate in radians + */ + void resetWindToExternalObservation(float wind_speed, float wind_direction, float wind_speed_accuracy, + float wind_direction_accuracy); + bool _external_wind_init{false}; + void updateParameters(); friend class AuxGlobalPosition; @@ -782,7 +794,7 @@ private: // fuse synthetic zero sideslip measurement void updateSideslip(estimator_aid_source1d_s &_aid_src_sideslip) const; - void fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip); + bool fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip); #endif // CONFIG_EKF2_SIDESLIP #if defined(CONFIG_EKF2_DRAG_FUSION) @@ -808,6 +820,8 @@ private: void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var); void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const float pos_var = NAN) { resetHorizontalPositionTo(new_horz_pos, Vector2f(pos_var, pos_var)); } + void resetWindTo(const Vector2f &wind, const Vector2f &wind_var); + bool isHeightResetRequired() const; void resetVerticalPositionTo(float new_vert_pos, float new_vert_pos_var = NAN); @@ -1095,8 +1109,6 @@ private: #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_WIND) - // perform a reset of the wind states and related covariances - void resetWind(); void resetWindCov(); void resetWindToZero(); #endif // CONFIG_EKF2_WIND diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 8c2a2c5511..d88f7c400d 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -864,32 +864,6 @@ void Ekf::updateGroundEffect() } #endif // CONFIG_EKF2_BAROMETER -#if defined(CONFIG_EKF2_WIND) -void Ekf::resetWind() -{ -#if defined(CONFIG_EKF2_AIRSPEED) - - if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) { - resetWindUsingAirspeed(_airspeed_sample_delayed); - return; - } - -#endif // CONFIG_EKF2_AIRSPEED - - resetWindToZero(); -} - -void Ekf::resetWindToZero() -{ - ECL_INFO("reset wind to zero"); - - // If we don't have an airspeed measurement, then assume the wind is zero - _state.wind_vel.setZero(); - - resetWindCov(); -} - -#endif // CONFIG_EKF2_WIND void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) { diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index a206ea6295..df08b048ee 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -314,6 +314,20 @@ def compute_wind_init_and_cov_from_airspeed( wind = wind.subs({sideslip: 0.0}) return (wind, P) +def compute_wind_init_and_cov_from_wind_speed_and_direction( + wind_speed: sf.Scalar, + wind_direction: sf.Scalar, + wind_speed_var: sf.Scalar, + wind_direction_var: sf.Scalar +)-> (sf.V2, sf.V2): + wind = sf.V2(wind_speed * sf.cos(wind_direction), wind_speed * sf.sin(wind_direction)) + H = wind.jacobian([wind_speed, wind_direction]) + R = sf.Matrix.diag([wind_speed_var, wind_direction_var]) + + P = H * R * H.T + P_diag = sf.V2(P[0,0], P[1,1]) + return (wind, P_diag) + def predict_sideslip( state: State, epsilon: sf.Scalar @@ -721,6 +735,7 @@ if not args.disable_wind: generate_px4_function(compute_sideslip_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_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"]) + generate_px4_function(compute_wind_init_and_cov_from_wind_speed_and_direction, output_names=["wind", "P_wind"]) generate_px4_function(compute_yaw_innov_var_and_h, output_names=["innov_var", "H"]) generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_wind_init_and_cov_from_wind_speed_and_direction.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_wind_init_and_cov_from_wind_speed_and_direction.h new file mode 100644 index 0000000000..d0af79dc7a --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_wind_init_and_cov_from_wind_speed_and_direction.h @@ -0,0 +1,61 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// 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_wind_init_and_cov_from_wind_speed_and_direction + * + * Args: + * wind_speed: Scalar + * wind_direction: Scalar + * wind_speed_var: Scalar + * wind_direction_var: Scalar + * + * Outputs: + * wind: Matrix21 + * P_wind: Matrix21 + */ +template +void ComputeWindInitAndCovFromWindSpeedAndDirection( + const Scalar wind_speed, const Scalar wind_direction, const Scalar wind_speed_var, + const Scalar wind_direction_var, matrix::Matrix* const wind = nullptr, + matrix::Matrix* const P_wind = nullptr) { + // Total ops: 14 + + // Input arrays + + // Intermediate terms (5) + const Scalar _tmp0 = std::cos(wind_direction); + const Scalar _tmp1 = std::sin(wind_direction); + const Scalar _tmp2 = std::pow(_tmp0, Scalar(2)); + const Scalar _tmp3 = std::pow(_tmp1, Scalar(2)); + const Scalar _tmp4 = wind_direction_var * std::pow(wind_speed, Scalar(2)); + + // Output terms (2) + if (wind != nullptr) { + matrix::Matrix& _wind = (*wind); + + _wind(0, 0) = _tmp0 * wind_speed; + _wind(1, 0) = _tmp1 * wind_speed; + } + + if (P_wind != nullptr) { + matrix::Matrix& _p_wind = (*P_wind); + + _p_wind(0, 0) = _tmp2 * wind_speed_var + _tmp3 * _tmp4; + _p_wind(1, 0) = _tmp2 * _tmp4 + _tmp3 * wind_speed_var; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/wind.cpp b/src/modules/ekf2/EKF/wind.cpp new file mode 100644 index 0000000000..ca3e8abc8b --- /dev/null +++ b/src/modules/ekf2/EKF/wind.cpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 wind.cpp + * Helper functions for wind states + */ + +#include "ekf.h" +#include + +void Ekf::resetWindToExternalObservation(float wind_speed, float wind_direction, float wind_speed_accuracy, + float wind_direction_accuracy) +{ + if (!_control_status.flags.in_air) { + + const float wind_speed_constrained = math::max(wind_speed, 0.0f); + const float wind_direction_var = sq(wind_direction_accuracy); + const float wind_speed_var = sq(wind_speed_accuracy); + + Vector2f wind; + Vector2f wind_var; + + sym::ComputeWindInitAndCovFromWindSpeedAndDirection(wind_speed_constrained, wind_direction, wind_speed_var, + wind_direction_var, &wind, &wind_var); + + ECL_INFO("reset wind states to external observation"); + _information_events.flags.reset_wind_to_ext_obs = true; + _external_wind_init = true; + + resetWindTo(wind, wind_var); + + } +} + +void Ekf::resetWindTo(const Vector2f &wind, const Vector2f &wind_var) +{ + _state.wind_vel = wind; + + if (PX4_ISFINITE(wind_var(0))) { + P.uncorrelateCovarianceSetVariance<1>(State::wind_vel.idx, + math::min(sq(_params.initial_wind_uncertainty), wind_var(0))); + } + + if (PX4_ISFINITE(wind_var(1))) { + P.uncorrelateCovarianceSetVariance<1>(State::wind_vel.idx + 1, + math::min(sq(_params.initial_wind_uncertainty), wind_var(1))); + } +} + +void Ekf::resetWindCov() +{ + // start with a small initial uncertainty to improve the initial estimate + P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, sq(_params.initial_wind_uncertainty)); +} + +void Ekf::resetWindToZero() +{ + ECL_INFO("reset wind to zero"); + _state.wind_vel.setZero(); + resetWindCov(); +} diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index b5d5dda1ce..ac98b9584f 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -561,6 +561,17 @@ void EKF2::Run() command_ack.timestamp = hrt_absolute_time(); _vehicle_command_ack_pub.publish(command_ack); } + + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE) { +#if defined(CONFIG_EKF2_WIND) + // wind direction is given as azimuth where wind blows FROM + // PX4 backend expects direction where wind blows TO + const float wind_direction_rad = wrap_pi(math::radians(vehicle_command.param3) + M_PI_F); + const float wind_direction_accuracy_rad = math::radians(vehicle_command.param4); + _ekf.resetWindToExternalObservation(vehicle_command.param1, wind_direction_rad, vehicle_command.param2, + wind_direction_accuracy_rad); +#endif // CONFIG_EKF2_WIND + } } }