diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index b40dd06a3d..aa883ee5c6 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -50,7 +50,7 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat // get indicated airspeed from input data (raw airspeed) _IAS = input_data.airspeed_indicated_raw; - update_CAS_scale_estimated(input_data.lpos_valid, input_data.ground_velocity, input_data.airspeed_true_raw); + update_CAS_scale_validated(input_data.lpos_valid, input_data.ground_velocity, input_data.airspeed_true_raw); update_CAS_scale_applied(); update_CAS_TAS(input_data.air_pressure_pa, input_data.air_temperature_celsius); update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid, @@ -106,14 +106,14 @@ AirspeedValidator::get_wind_estimator_states(uint64_t timestamp) wind_est.beta_innov_var = _wind_estimator.get_beta_innov_var(); wind_est.tas_scale_raw = _wind_estimator.get_tas_scale(); wind_est.tas_scale_raw_var = _wind_estimator.get_tas_scale_var(); - wind_est.tas_scale_validated = _CAS_scale_estimated; + wind_est.tas_scale_validated = _CAS_scale_validated; return wind_est; } void -AirspeedValidator::update_CAS_scale_estimated(bool lpos_valid, matrix::Vector3f vI, float airspeed_true_raw) +AirspeedValidator::update_CAS_scale_validated(bool lpos_valid, matrix::Vector3f vI, float airspeed_true_raw) { - if (!_in_fixed_wing_flight) { + if (!_in_fixed_wing_flight || !lpos_valid) { return; } @@ -139,20 +139,17 @@ AirspeedValidator::update_CAS_scale_estimated(bool lpos_valid, matrix::Vector3f 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_current = ground_speed_sum - TAS_sum * _CAS_scale_validated; 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); + const float new_scale_constrained = math::constrain(_wind_estimator.get_tas_scale(), _CAS_scale_validated - 0.01f, + _CAS_scale_validated + 0.01f); - // PX4_INFO("_CAS_scale_estimated updated: %.2f --> %.2f", (double)_CAS_scale_estimated, - // (double)new_scale_constrained); - - _CAS_scale_estimated = new_scale_constrained; + _CAS_scale_validated = new_scale_constrained; } reset_CAS_scale_check(); @@ -187,7 +184,7 @@ AirspeedValidator::update_CAS_scale_applied() break; case 3: - _CAS_scale_applied = _CAS_scale_estimated; + _CAS_scale_applied = _CAS_scale_validated; break; } } @@ -241,7 +238,7 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ const float dt_s = math::constrain((time_now - _time_last_aspd_innov_check) / 1e6f, 0.01f, 0.2f); // limit to [5,100] Hz matrix::Vector2f wind_2d(_wind_estimator.get_wind()); matrix::Vector3f air_vel = vI - matrix::Vector3f {wind_2d(0), wind_2d(1), 0.f}; - const float tas_innov = abs(_TAS - air_vel.norm()); + const float tas_innov = fabsf(_TAS - air_vel.norm()); if (tas_innov > _tas_innov_threshold) { _apsd_innov_integ_state += dt_s * (tas_innov - _tas_innov_threshold); // integrate exceedance diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index 280fd6510e..0add7fd15b 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -82,7 +82,7 @@ public: float get_CAS() { return _CAS; } float get_TAS() { return _TAS; } bool get_airspeed_valid() { return _airspeed_valid; } - float get_CAS_scale_estimated() {return _CAS_scale_estimated;} + float get_CAS_scale_validated() {return _CAS_scale_validated;} airspeed_wind_s get_wind_estimator_states(uint64_t timestamp); @@ -113,7 +113,7 @@ 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_CAS_scale_validated(float scale) { _CAS_scale_validated = 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); } @@ -130,7 +130,7 @@ private: float _CAS{0.0f}; ///< calibrated airspeed in m/s float _TAS{0.0f}; ///< true airspeed in m/s 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) + float _CAS_scale_validated{1.0f}; ///< scale factor from IAS to CAS (currently estimated value) // data stuck check uint64_t _time_last_unequal_data{0}; @@ -173,7 +173,7 @@ private: void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool lpos_valid, matrix::Vector3f vI, float lpos_evh, float lpos_evv, const float att_q[4]); - void update_CAS_scale_estimated(bool lpos_valid, matrix::Vector3f vI, float airspeed_true_raw); + void update_CAS_scale_validated(bool lpos_valid, matrix::Vector3f vI, float airspeed_true_raw); 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); diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 0e04a1dbc3..ca33b20395 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -296,7 +296,7 @@ AirspeedModule::Run() 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_CAS_scale_validated(_param_airspeed_scale[i]); _airspeed_validator[i].set_scale_init(_param_airspeed_scale[i]); } @@ -388,25 +388,25 @@ AirspeedModule::Run() // 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) { + if (fabsf(_airspeed_validator[i].get_CAS_scale_validated() - _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()); + (double)_airspeed_validator[i].get_CAS_scale_validated()); switch (i) { case 0: - _param_airspeed_scale_1.set(_airspeed_validator[i].get_CAS_scale_estimated()); + _param_airspeed_scale_1.set(_airspeed_validator[i].get_CAS_scale_validated()); _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.set(_airspeed_validator[i].get_CAS_scale_validated()); _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.set(_airspeed_validator[i].get_CAS_scale_validated()); _param_airspeed_scale_3.commit_no_notification(); break; } diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c index c6c40afa10..9dc172cb42 100644 --- a/src/modules/airspeed_selector/airspeed_selector_params.c +++ b/src/modules/airspeed_selector/airspeed_selector_params.c @@ -76,7 +76,7 @@ PARAM_DEFINE_INT32(ASPD_TAS_GATE, 3); PARAM_DEFINE_INT32(ASPD_BETA_GATE, 1); /** - * Controls when to apply the new esstimated airspeed scale + * Controls when to apply the new estimated airspeed scale * * @value 0 Disable airspeed scale estimation completely * @value 1 Do not apply the new gains (logging and inside wind estimator)