diff --git a/msg/airspeed_wind.msg b/msg/airspeed_wind.msg index 03033bbbdd..a9dff0fe63 100644 --- a/msg/airspeed_wind.msg +++ b/msg/airspeed_wind.msg @@ -9,7 +9,9 @@ float32 variance_east # Wind estimate error variance in east / Y direction (m/s float32 tas_innov # True airspeed innovation float32 tas_innov_var # True airspeed innovation variance + float32 tas_scale # Estimated true airspeed scale factor +float32 tas_scale_var # True airspeed scale factor variance float32 beta_innov # Sideslip measurement innovation float32 beta_innov_var # Sideslip measurement innovation variance diff --git a/src/lib/wind_estimator/WindEstimator.cpp b/src/lib/wind_estimator/WindEstimator.cpp index df840080f0..2b6b828059 100644 --- a/src/lib/wind_estimator/WindEstimator.cpp +++ b/src/lib/wind_estimator/WindEstimator.cpp @@ -56,7 +56,7 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f & // initilaise wind states assuming zero side slip and horizontal flight _state(INDEX_W_N) = velI(INDEX_W_N) - tas_meas * cosf(heading_est); _state(INDEX_W_E) = velI(INDEX_W_E) - tas_meas * sinf(heading_est); - _state(INDEX_TAS_SCALE) = 1.0f; + _state(INDEX_TAS_SCALE) = _scale_init; // compute jacobian of states wrt north/each earth velocity states and true airspeed measurement float L0 = v_e * v_e; @@ -116,7 +116,7 @@ WindEstimator::update(uint64_t time_now) _time_last_update = time_now; float q_w = _wind_p_var; - float q_k_tas = _tas_scale_p_var; + float q_k_tas = _disable_tas_scale_estimate ? 0.f : _tas_scale_p_var; float SPP0 = dt * dt; float SPP1 = SPP0 * q_w; @@ -160,28 +160,32 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const const float v_e = velI(1); const float v_d = velI(2); - const float k_tas = _state(INDEX_TAS_SCALE); + // calculate airspeed from ground speed and wind states (without scale) + const float airspeed_predicted_raw = sqrtf((v_n - _state(INDEX_W_N)) * (v_n - _state(INDEX_W_N)) + + (v_e - _state(INDEX_W_E)) * (v_e - _state(INDEX_W_E)) + v_d * v_d); - // compute kalman gain K - const float HH0 = sqrtf(v_d * v_d + (v_e - _state(INDEX_W_E)) * (v_e - _state(INDEX_W_E)) + (v_n - _state( - INDEX_W_N)) * (v_n - _state(INDEX_W_N))); - const float HH1 = k_tas / HH0; + // compute state observation matrix H + const float HH0 = airspeed_predicted_raw; + const float HH1 = _state(INDEX_TAS_SCALE) / HH0; matrix::Matrix H_tas; H_tas(0, 0) = HH1 * (-v_n + _state(INDEX_W_N)); H_tas(0, 1) = HH1 * (-v_e + _state(INDEX_W_E)); H_tas(0, 2) = HH0; - matrix::Matrix K = _P * H_tas.transpose(); - + // compute innovation covariance S const matrix::Matrix S = H_tas * _P * H_tas.transpose() + _tas_var; + // compute Kalman gain + matrix::Matrix K = _P * H_tas.transpose(); K /= S(0, 0); - // compute innovation - const float airspeed_pred = k_tas * sqrtf((v_n - _state(INDEX_W_N)) * (v_n - _state(INDEX_W_N)) + (v_e - _state( - INDEX_W_E)) * - (v_e - _state(INDEX_W_E)) + v_d * v_d); + if (_disable_tas_scale_estimate) { + K(2, 0) = 0.f; + } + + // compute innovation + const float airspeed_pred = _state(INDEX_TAS_SCALE) * airspeed_predicted_raw; _tas_innov = true_airspeed - airspeed_pred; // innovation variance @@ -259,13 +263,17 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const H_beta(0, 1) = HB15 * (HB12 - HB7 + HB8) + HB16 * HB5; H_beta(0, 2) = 0; - // compute kalman gain - matrix::Matrix K = _P * H_beta.transpose(); - + // compute innovation covariance S const matrix::Matrix S = H_beta * _P * H_beta.transpose() + _beta_var; + // compute Kalman gain + matrix::Matrix K = _P * H_beta.transpose(); K /= S(0, 0); + if (_disable_tas_scale_estimate) { + K(2, 0) = 0.f; + } + // compute predicted side slip angle matrix::Vector3f rel_wind(velI(0) - _state(INDEX_W_N), velI(1) - _state(INDEX_W_E), velI(2)); matrix::Dcmf R_body_to_earth(q_att); @@ -333,16 +341,6 @@ WindEstimator::run_sanity_checks() return; } - // check if we should inhibit learning of airspeed scale factor and rather use a pre-set value. - // airspeed scale factor errors arise from sensor installation which does not change and only needs - // to be computed once for a perticular installation. - if (_enforced_airspeed_scale < 0) { - _state(INDEX_TAS_SCALE) = math::max(0.0f, _state(INDEX_TAS_SCALE)); - - } else { - _state(INDEX_TAS_SCALE) = _enforced_airspeed_scale; - } - // attain symmetry for (unsigned row = 0; row < 3; row++) { for (unsigned column = 0; column < row; column++) { diff --git a/src/lib/wind_estimator/WindEstimator.hpp b/src/lib/wind_estimator/WindEstimator.hpp index d90e242937..e81539adfe 100644 --- a/src/lib/wind_estimator/WindEstimator.hpp +++ b/src/lib/wind_estimator/WindEstimator.hpp @@ -70,7 +70,9 @@ public: bool check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size, uint64_t &time_meas_rejected, bool &reinit_filter); - float get_tas_scale() { return _state(INDEX_TAS_SCALE); } + // invert scale (CAS = IAS * scale), protect agains division by 0, constrain to [0.1, 10] + float get_tas_scale() { return math::constrain(1.f / (_state(INDEX_TAS_SCALE) + 0.0001f), 0.1f, 10.f); } + float get_tas_scale_var() { return _P(2, 2); } float get_tas_innov() { return _tas_innov; } float get_tas_innov_var() { return _tas_innov_var; } float get_beta_innov() { return _beta_innov; } @@ -88,10 +90,8 @@ public: void set_beta_noise(float beta_var) { _beta_var = beta_var * beta_var; } void set_tas_gate(uint8_t gate_size) {_tas_gate = gate_size; } void set_beta_gate(uint8_t gate_size) {_beta_gate = gate_size; } - - // Inhibit learning of the airspeed scale factor and force the estimator to use _enforced_airspeed_scale as scale factor. - // Negative input values enable learning of the airspeed scale factor. - void enforce_airspeed_scale(float scale) {_enforced_airspeed_scale = scale; } + void set_scale_init(float scale_init) {_scale_init = math::constrain(1.f / (scale_init + 0.0001f), 0.1f, 10.f); } + void set_disable_tas_scale_estimate(bool disable_tas_scale_estimate) {_disable_tas_scale_estimate = disable_tas_scale_estimate; } private: enum { @@ -100,7 +100,7 @@ private: INDEX_TAS_SCALE }; ///< enum which can be used to access state. - matrix::Vector3f _state; ///< state vector + matrix::Vector3f _state{0.f, 0.f, 1.f}; matrix::Matrix3f _P; ///< state covariance matrix float _tas_innov{0.0f}; ///< true airspeed innovation @@ -118,15 +118,18 @@ private: uint8_t _tas_gate{3}; ///< airspeed fusion gate size uint8_t _beta_gate{1}; ///< sideslip fusion gate size + float _scale_init{1.f}; + uint64_t _time_last_airspeed_fuse = 0; ///< timestamp of last airspeed fusion - uint64_t _time_last_beta_fuse = 0; ///< timestamp of last sideslip fusion - uint64_t _time_last_update = 0; ///< timestamp of last covariance prediction - uint64_t _time_rejected_beta = 0; ///< timestamp of when sideslip measurements have consistently started to be rejected + uint64_t _time_last_beta_fuse = 0; ///< timestamp of last sideslip fusion + uint64_t _time_last_update = 0; ///< timestamp of last covariance prediction + uint64_t _time_rejected_beta = 0; ///< timestamp of when sideslip measurements have consistently started to be rejected uint64_t _time_rejected_tas = - 0; /// 100_s) { + reset_CAS_scale_check(); + } + + const float course_over_ground_rad = matrix::wrap_2pi(atan2f(vy, vx)); + const int segment_index = int(SCALE_CHECK_SAMPLES * course_over_ground_rad / (2.f * M_PI_F)); + + _scale_check_groundspeed(segment_index) = sqrt(vx * vx + vy * vy + vz * vz); + _scale_check_TAS(segment_index) = _TAS; + + // run check if all segments are filled + if (PX4_ISFINITE(_scale_check_groundspeed.norm_squared())) { + + float ground_speed_sum = 0.f; + float TAS_sum = 0.f; + + for (int i = 0; i < SCALE_CHECK_SAMPLES; i++) { + ground_speed_sum += _scale_check_groundspeed(i); + TAS_sum += _scale_check_TAS(i); + } + + const float TAS_to_grounspeed_error_current = ground_speed_sum - TAS_sum * _CAS_scale_estimated; + const float TAS_to_grounspeed_error_new = ground_speed_sum - TAS_sum * _wind_estimator.get_tas_scale(); + + // check passes if the average airspeed with the scale applied is closer to groundspeed than without + if (fabsf(TAS_to_grounspeed_error_new) < fabsf(TAS_to_grounspeed_error_current)) { + + // constrain the scale update to max 0.01 at a time + const float new_scale_constrained = math::constrain(_wind_estimator.get_tas_scale(), _CAS_scale_estimated - 0.01f, + _CAS_scale_estimated + 0.01f); + + // PX4_INFO("_CAS_scale_estimated updated: %.2f --> %.2f", (double)_CAS_scale_estimated, + // (double)new_scale_constrained); + + _CAS_scale_estimated = new_scale_constrained; + } + + reset_CAS_scale_check(); + } +} + +void +AirspeedValidator::reset_CAS_scale_check() { - _airspeed_scale_manual = airspeed_scale_manual; - _wind_estimator.enforce_airspeed_scale(1.0f / airspeed_scale_manual); // scale is inverted inside the wind estimator + + _scale_check_groundspeed.setAll(NAN); + _scale_check_TAS.setAll(NAN); + + _begin_current_scale_check = hrt_absolute_time(); } void -AirspeedValidator::update_CAS_scale() +AirspeedValidator::update_CAS_scale_applied() { - if (_wind_estimator.is_estimate_valid()) { - _CAS_scale = 1.0f / math::constrain(_wind_estimator.get_tas_scale(), 0.5f, 2.0f); + switch (_tas_scale_apply) { + default: - } else { - _CAS_scale = _airspeed_scale_manual; + /* fallthrough */ + case 0: + + /* fallthrough */ + case 1: + + /* fallthrough */ + case 2: + _CAS_scale_applied = _tas_scale_init; + break; + + case 3: + _CAS_scale_applied = _CAS_scale_estimated; + break; } } void AirspeedValidator::update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius) { - _CAS = calc_CAS_from_IAS(_IAS, _CAS_scale); + _CAS = calc_CAS_from_IAS(_IAS, _CAS_scale_applied); _TAS = calc_TAS_from_CAS(_CAS, air_pressure_pa, air_temperature_celsius); } +void +AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now) +{ + // data stuck test: trigger when IAS is not changing for DATA_STUCK_TIMEOUT (2s) + + if (fabsf(_IAS - _IAS_prev) > FLT_EPSILON || _time_last_unequal_data == 0) { + _time_last_unequal_data = time_now; + _IAS_prev = _IAS; + } + + _data_stuck_test_failed = hrt_elapsed_time(&_time_last_unequal_data) > DATA_STUCK_TIMEOUT; +} + void AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_status_vel_test_ratio, float estimator_status_mag_test_ratio) @@ -205,12 +289,12 @@ AirspeedValidator::check_load_factor(float accel_z) void AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp) { - if (_innovations_check_failed || _load_factor_check_failed) { - // either innovation or load factor check failed, so record timestamp + if (_data_stuck_test_failed || _innovations_check_failed || _load_factor_check_failed) { + // at least one check (data stuck, innovation or load factor) failed, so record timestamp _time_checks_failed = timestamp; - } else if (!_innovations_check_failed && !_load_factor_check_failed) { - // both innovation or load factor checks must pass to declare airspeed good + } else if (! _data_stuck_test_failed && !_innovations_check_failed && !_load_factor_check_failed) { + // all checks(data stuck, innovation and load factor) must pass to declare airspeed good _time_checks_passed = timestamp; } @@ -223,7 +307,7 @@ AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp) // a timeout period is applied. const bool single_check_fail_timeout = (timestamp - _time_checks_passed) > _checks_fail_delay * 1_s; - if (both_checks_failed || single_check_fail_timeout) { + if (both_checks_failed || single_check_fail_timeout || _data_stuck_test_failed) { _airspeed_valid = false; } diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index 0fc5e1ac9d..fcf1ad04da 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2021 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 @@ -73,7 +73,7 @@ struct airspeed_validator_update_data { class AirspeedValidator { public: - AirspeedValidator() = default; + AirspeedValidator(); ~AirspeedValidator() = default; void update_airspeed_validator(const airspeed_validator_update_data &input_data); @@ -84,13 +84,18 @@ public: float get_CAS() { return _CAS; } float get_TAS() { return _TAS; } bool get_airspeed_valid() { return _airspeed_valid; } - float get_CAS_scale() {return _CAS_scale;} + float get_CAS_scale_estimated() {return _CAS_scale_estimated;} airspeed_wind_s get_wind_estimator_states(uint64_t timestamp); // setters wind estimator parameters void set_wind_estimator_wind_p_noise(float wind_sigma) { _wind_estimator.set_wind_p_noise(wind_sigma); } void set_wind_estimator_tas_scale_p_noise(float tas_scale_sigma) { _wind_estimator.set_tas_scale_p_noise(tas_scale_sigma); } + void set_wind_estimator_tas_scale_init(float tas_scale_init) + { + _tas_scale_init = tas_scale_init; + } + void set_wind_estimator_tas_noise(float tas_sigma) { _wind_estimator.set_tas_noise(tas_sigma); } void set_wind_estimator_beta_noise(float beta_var) { _wind_estimator.set_beta_noise(beta_var); } void set_wind_estimator_tas_gate(uint8_t gate_size) @@ -100,9 +105,6 @@ public: } void set_wind_estimator_beta_gate(uint8_t gate_size) { _wind_estimator.set_beta_gate(gate_size); } - void set_wind_estimator_scale_estimation_on(bool scale_estimation_on) { _wind_estimator_scale_estimation_on = scale_estimation_on;} - - void set_airspeed_scale_manual(float airspeed_scale_manual); // setters for failure detection tuning parameters void set_tas_innov_threshold(float tas_innov_threshold) { _tas_innov_threshold = tas_innov_threshold; } @@ -112,20 +114,31 @@ public: void set_airspeed_stall(float airspeed_stall) { _airspeed_stall = airspeed_stall; } + void set_tas_scale_apply(int tas_scale_apply) { _tas_scale_apply = tas_scale_apply; } + void set_CAS_scale_estimated(float scale) { _CAS_scale_estimated = scale; } + void set_scale_init(float scale) { _wind_estimator.set_scale_init(scale); } + void set_disable_tas_scale_estimate(bool disable_scale_est) {_wind_estimator.set_disable_tas_scale_estimate(disable_scale_est); } + private: WindEstimator _wind_estimator{}; ///< wind estimator instance running in this particular airspeedValidator - // wind estimator parameter - bool _wind_estimator_scale_estimation_on{false}; ///< online scale estimation (IAS-->CAS) is on - float _airspeed_scale_manual{1.0f}; ///< manually entered airspeed scale + // airspeed scale validity check + static constexpr int SCALE_CHECK_SAMPLES = 12; ///< take samples from 12 segments (every 360/12=30°) // general states bool _in_fixed_wing_flight{false}; ///< variable to bypass innovation and load factor checks float _IAS{0.0f}; ///< indicated airsped in m/s float _CAS{0.0f}; ///< calibrated airspeed in m/s float _TAS{0.0f}; ///< true airspeed in m/s - float _CAS_scale{1.0f}; ///< scale factor from IAS to CAS + float _CAS_scale_applied{1.0f}; ///< scale factor from IAS to CAS (currently applied value) + float _CAS_scale_estimated{1.0f}; ///< scale factor from IAS to CAS (currently estimated value) + + // data stuck check + uint64_t _time_last_unequal_data{0}; + bool _data_stuck_test_failed{false}; + float _IAS_prev{0.f}; + static constexpr uint64_t DATA_STUCK_TIMEOUT{2_s}; ///< timeout after which data stuck check triggers when data is flat // states of innovation check float _tas_gate{1.0f}; ///< gate size of airspeed innovation (to calculate tas_test_ratio) @@ -146,22 +159,33 @@ private: // states of airspeed valid declaration bool _airspeed_valid{true}; ///< airspeed valid (pitot or groundspeed-windspeed) int _checks_fail_delay{3}; ///< delay for airspeed invalid declaration after single check failure (Sec) - int _checks_clear_delay{3}; ///< delay for airspeed valid declaration after all checks passed again (Sec) + int _checks_clear_delay{-1}; ///< delay for airspeed valid declaration after all checks passed again (Sec) uint64_t _time_checks_passed{0}; ///< time the checks have last passed (uSec) uint64_t _time_checks_failed{0}; ///< time the checks have last not passed (uSec) + hrt_abstime _begin_current_scale_check{0}; + + int _tas_scale_apply{0}; + float _tas_scale_init{1.f}; + + matrix::Vector _scale_check_TAS {}; + matrix::Vector _scale_check_groundspeed {}; + void update_in_fixed_wing_flight(bool in_fixed_wing_flight) { _in_fixed_wing_flight = in_fixed_wing_flight; } void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool lpos_valid, float lpos_vx, float lpos_vy, float lpos_vz, float lpos_evh, float lpos_evv, const float att_q[4]); - void update_CAS_scale(); + void update_CAS_scale_estimated(bool lpos_valid, float vx, float vy, float vz); + void update_CAS_scale_applied(); void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius); + void check_airspeed_data_stuck(uint64_t timestamp); void check_airspeed_innovation(uint64_t timestamp, float estimator_status_vel_test_ratio, float estimator_status_mag_test_ratio); void check_load_factor(float accel_z); void update_airspeed_valid_status(const uint64_t timestamp); void reset(); + void reset_CAS_scale_check(); }; diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 0688926094..e6f0e22aec 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2021 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 @@ -149,13 +149,14 @@ private: bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */ float _ground_minus_wind_TAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */ float _ground_minus_wind_CAS{0.0f}; /**< calibrated airspeed from groundspeed minus windspeed */ - - bool _scale_estimation_previously_on{false}; /**< scale_estimation was on in the last cycle */ + bool _armed_prev{false}; hrt_abstime _time_last_airspeed_update[MAX_NUM_AIRSPEED_SENSORS] {}; perf_counter_t _perf_elapsed{}; + float _param_airspeed_scale[MAX_NUM_AIRSPEED_SENSORS] {}; /** array to save the airspeed scale params in */ + DEFINE_PARAMETERS( (ParamFloat) _param_west_w_p_noise, (ParamFloat) _param_west_sc_p_noise, @@ -163,8 +164,10 @@ private: (ParamFloat) _param_west_beta_noise, (ParamInt) _param_west_tas_gate, (ParamInt) _param_west_beta_gate, - (ParamInt) _param_west_scale_estimation_on, - (ParamFloat) _param_west_airspeed_scale, + (ParamInt) _param_aspd_scale_apply, + (ParamFloat) _param_airspeed_scale_1, + (ParamFloat) _param_airspeed_scale_2, + (ParamFloat) _param_airspeed_scale_3, (ParamInt) _param_airspeed_primary_index, (ParamInt) _param_airspeed_checks_on, (ParamInt) _param_airspeed_fallback_gw, @@ -291,6 +294,12 @@ AirspeedModule::Run() if (!_initialized) { init(); // initialize airspeed validator instances + + for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) { + _airspeed_validator[i].set_CAS_scale_estimated(_param_airspeed_scale[i]); + _airspeed_validator[i].set_scale_init(_param_airspeed_scale[i]); + } + _initialized = true; } @@ -300,7 +309,7 @@ AirspeedModule::Run() update_params(); } - bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + const bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); // check for new connected airspeed sensors as long as we're disarmed if (!armed) { @@ -375,11 +384,44 @@ AirspeedModule::Run() _airspeed_validator[i].reset_airspeed_to_invalid(_time_now_usec); } + + // save estimated airspeed scale after disarm + if (!armed && _armed_prev) { + if (_param_aspd_scale_apply.get() > 1) { + if (fabsf(_airspeed_validator[i].get_CAS_scale_estimated() - _param_airspeed_scale[i]) > 0.01f) { + // apply the new scale if changed more than 0.01 + mavlink_log_info(&_mavlink_log_pub, "Airspeed sensor Nr. %d ASPD_SCALE updated: %.2f --> %.2f", i + 1, + (double)_param_airspeed_scale[i], + (double)_airspeed_validator[i].get_CAS_scale_estimated()); + + switch (i) { + case 0: + _param_airspeed_scale_1.set(_airspeed_validator[i].get_CAS_scale_estimated()); + _param_airspeed_scale_1.commit_no_notification(); + break; + + case 1: + _param_airspeed_scale_2.set(_airspeed_validator[i].get_CAS_scale_estimated()); + _param_airspeed_scale_2.commit_no_notification(); + break; + + case 2: + _param_airspeed_scale_3.set(_airspeed_validator[i].get_CAS_scale_estimated()); + _param_airspeed_scale_3.commit_no_notification(); + break; + } + } + } + + _airspeed_validator[i].set_scale_init(_param_airspeed_scale[i]); + } } } select_airspeed_and_publish(); + _armed_prev = armed; + perf_end(_perf_elapsed); if (should_exit()) { @@ -391,6 +433,10 @@ void AirspeedModule::update_params() { updateParams(); + _param_airspeed_scale[0] = _param_airspeed_scale_1.get(); + _param_airspeed_scale[1] = _param_airspeed_scale_2.get(); + _param_airspeed_scale[2] = _param_airspeed_scale_3.get(); + _wind_estimator_sideslip.set_wind_p_noise(_param_west_w_p_noise.get()); _wind_estimator_sideslip.set_tas_scale_p_noise(_param_west_sc_p_noise.get()); _wind_estimator_sideslip.set_tas_noise(_param_west_tas_noise.get()); @@ -398,62 +444,24 @@ void AirspeedModule::update_params() _wind_estimator_sideslip.set_tas_gate(_param_west_tas_gate.get()); _wind_estimator_sideslip.set_beta_gate(_param_west_beta_gate.get()); - for (int i = 0; i < _number_of_airspeed_sensors; i++) { + for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) { _airspeed_validator[i].set_wind_estimator_wind_p_noise(_param_west_w_p_noise.get()); _airspeed_validator[i].set_wind_estimator_tas_scale_p_noise(_param_west_sc_p_noise.get()); _airspeed_validator[i].set_wind_estimator_tas_noise(_param_west_tas_noise.get()); _airspeed_validator[i].set_wind_estimator_beta_noise(_param_west_beta_noise.get()); _airspeed_validator[i].set_wind_estimator_tas_gate(_param_west_tas_gate.get()); _airspeed_validator[i].set_wind_estimator_beta_gate(_param_west_beta_gate.get()); - _airspeed_validator[i].set_wind_estimator_scale_estimation_on(_param_west_scale_estimation_on.get()); - // only apply manual entered airspeed scale to first airspeed measurement - // TODO: enable multiple airspeed sensors - _airspeed_validator[0].set_airspeed_scale_manual(_param_west_airspeed_scale.get()); + _airspeed_validator[i].set_tas_scale_apply(_param_aspd_scale_apply.get()); + _airspeed_validator[i].set_wind_estimator_tas_scale_init(_param_airspeed_scale[i]); _airspeed_validator[i].set_tas_innov_threshold(_tas_innov_threshold.get()); _airspeed_validator[i].set_tas_innov_integ_threshold(_tas_innov_integ_threshold.get()); _airspeed_validator[i].set_checks_fail_delay(_checks_fail_delay.get()); _airspeed_validator[i].set_checks_clear_delay(_checks_clear_delay.get()); _airspeed_validator[i].set_airspeed_stall(_param_fw_airspd_stall.get()); + _airspeed_validator[i].set_disable_tas_scale_estimate(_param_aspd_scale_apply.get() == 0); } - - // if the airspeed scale estimation is enabled and the airspeed is valid, - // then set the scale inside the wind estimator to -1 such that it starts to estimate it - if (!_scale_estimation_previously_on && _param_west_scale_estimation_on.get()) { - if (_valid_airspeed_index > 0) { - // set it to a negative value to start estimation inside wind estimator - _airspeed_validator[0].set_airspeed_scale_manual(-1.0f); - - } else { - mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.\t"); - events::send(events::ID("airspeed_selector_cannot_est_scale"), events::Log::Error, - "Airspeed: cannot estimate scale as there is no valid sensor"); - _param_west_scale_estimation_on.set(0); // reset this param to 0 as estimation was not turned on - _param_west_scale_estimation_on.commit_no_notification(); - } - - } else if (_scale_estimation_previously_on && !_param_west_scale_estimation_on.get()) { - if (_valid_airspeed_index > 0) { - - _param_west_airspeed_scale.set(_airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale()); - _param_west_airspeed_scale.commit_no_notification(); - _airspeed_validator[_valid_airspeed_index - 1].set_airspeed_scale_manual(_param_west_airspeed_scale.get()); - - mavlink_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ASPD_SCALE): %0.2f\t", - (double)_airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale()); - events::send(events::ID("airspeed_selector_scale"), events::Log::Info, - "Airspeed: estimated scale (ASPD_SCALE): {1:.2}", _airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale()); - - } else { - mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.\t"); - events::send(events::ID("airspeed_selector_cannot_est_scale2"), events::Log::Error, - "Airspeed: cannot estimate scale as there is no valid sensor"); - } - } - - _scale_estimation_previously_on = _param_west_scale_estimation_on.get(); - } void AirspeedModule::poll_topics() @@ -505,11 +513,12 @@ void AirspeedModule::update_wind_estimator_sideslip() _wind_estimator_sideslip.get_wind_var(wind_cov); _wind_estimate_sideslip.variance_north = wind_cov[0]; _wind_estimate_sideslip.variance_east = wind_cov[1]; - _wind_estimate_sideslip.tas_innov = _wind_estimator_sideslip.get_tas_innov(); - _wind_estimate_sideslip.tas_innov_var = _wind_estimator_sideslip.get_tas_innov_var(); + _wind_estimate_sideslip.tas_innov = NAN; + _wind_estimate_sideslip.tas_innov_var = NAN; _wind_estimate_sideslip.beta_innov = _wind_estimator_sideslip.get_beta_innov(); _wind_estimate_sideslip.beta_innov_var = _wind_estimator_sideslip.get_beta_innov_var(); - _wind_estimate_sideslip.tas_scale = _wind_estimator_sideslip.get_tas_scale(); + _wind_estimate_sideslip.tas_scale = NAN; + _wind_estimate_sideslip.tas_scale_var = NAN; _wind_estimate_sideslip.source = airspeed_wind_s::SOURCE_AS_BETA_ONLY; } diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c index dac73ae16d..159ba45683 100644 --- a/src/modules/airspeed_selector/airspeed_selector_params.c +++ b/src/modules/airspeed_selector/airspeed_selector_params.c @@ -7,6 +7,7 @@ * @min 0 * @max 1 * @unit m/s^2 + * @decimal 2 * @group Airspeed Validator */ PARAM_DEFINE_FLOAT(ASPD_W_P_NOISE, 0.1f); @@ -19,9 +20,10 @@ PARAM_DEFINE_FLOAT(ASPD_W_P_NOISE, 0.1f); * @min 0 * @max 0.1 * @unit Hz + * @decimal 5 * @group Airspeed Validator */ -PARAM_DEFINE_FLOAT(ASPD_SC_P_NOISE, 0.0001); +PARAM_DEFINE_FLOAT(ASPD_SC_P_NOISE, 0.0001f); /** * Airspeed Selector: Wind estimator true airspeed measurement noise @@ -31,9 +33,10 @@ PARAM_DEFINE_FLOAT(ASPD_SC_P_NOISE, 0.0001); * @min 0 * @max 4 * @unit m/s + * @decimal 1 * @group Airspeed Validator */ -PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4); +PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4f); /** * Airspeed Selector: Wind estimator sideslip measurement noise @@ -43,9 +46,10 @@ PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4); * @min 0 * @max 1 * @unit rad + * @decimal 3 * @group Airspeed Validator */ -PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.3); +PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.3f); /** * Airspeed Selector: Gate size for true airspeed fusion @@ -72,27 +76,57 @@ PARAM_DEFINE_INT32(ASPD_TAS_GATE, 3); PARAM_DEFINE_INT32(ASPD_BETA_GATE, 1); /** - * Automatic airspeed scale estimation on + * Controls when to apply the new esstimated airspeed scale * - * Turns the automatic airspeed scale (scale from IAS to CAS) on or off. It is recommended to fly level - * altitude while performing the estimation. Set to 1 to start estimation (best when already flying). - * Set to 0 to end scale estimation. The estimated scale is then saved using the ASPD_SCALE parameter. + * @value 0 Disable airspeed scale estimation completely + * @value 1 Do not apply the new gains (logging and inside wind estimator) + * @value 2 Apply the new scale after disarm + * @value 3 Apply the new gains in air + * @group Airspeed Validator + */ +PARAM_DEFINE_INT32(ASPD_SCALE_APPLY, 2); + +/** + * Scale of airspeed sensor 1 * - * @boolean + * This is the scale IAS --> CAS of the first airspeed sensor instance + * + * @min 0.5 + * @max 2.0 + * @decimal 2 + * @reboot_required true * @group Airspeed Validator + * @volatile */ -PARAM_DEFINE_INT32(ASPD_SCALE_EST, 0); +PARAM_DEFINE_FLOAT(ASPD_SCALE_1, 1.0f); /** - * Airspeed scale (scale from IAS to CAS) + * Scale of airspeed sensor 2 * - * Scale can either be entered manually, or estimated in-flight by setting ASPD_SCALE_EST to 1. + * This is the scale IAS --> CAS of the second airspeed sensor instance * * @min 0.5 - * @max 1.5 + * @max 2.0 + * @decimal 2 + * @reboot_required true + * @group Airspeed Validator + * @volatile + */ +PARAM_DEFINE_FLOAT(ASPD_SCALE_2, 1.0f); + +/** + * Scale of airspeed sensor 3 + * + * This is the scale IAS --> CAS of the third airspeed sensor instance + * + * @min 0.5 + * @max 2.0 + * @decimal 2 + * @reboot_required true * @group Airspeed Validator + * @volatile */ -PARAM_DEFINE_FLOAT(ASPD_SCALE, 1.0f); +PARAM_DEFINE_FLOAT(ASPD_SCALE_3, 1.0f); /** * Index or primary airspeed measurement source @@ -142,6 +176,7 @@ PARAM_DEFINE_INT32(ASPD_FALLBACK_GW, 0); * * @min 0.5 * @max 3.0 + * @decimal 1 * @group Airspeed Validator */ PARAM_DEFINE_FLOAT(ASPD_FS_INNOV, 1.0f); @@ -156,6 +191,7 @@ PARAM_DEFINE_FLOAT(ASPD_FS_INNOV, 1.0f); * * @unit s * @max 30.0 + * @decimal 1 * @group Airspeed Validator */ PARAM_DEFINE_FLOAT(ASPD_FS_INTEG, 5.0f);