diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index 63343b4023..b83d419b5f 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -50,16 +50,16 @@ 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_validated(input_data.lpos_valid, input_data.ground_velocity, input_data.airspeed_true_raw); + update_CAS_scale_validated(input_data.gnss_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, + update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.gnss_valid, input_data.ground_velocity, input_data.lpos_evh, input_data.lpos_evv, input_data.q_att); update_in_fixed_wing_flight(input_data.in_fixed_wing_flight); check_airspeed_data_stuck(input_data.timestamp); check_load_factor(input_data.accel_z); check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio, - input_data.ground_velocity, input_data.lpos_valid); + input_data.ground_velocity, input_data.gnss_valid); update_airspeed_valid_status(input_data.timestamp); } @@ -71,12 +71,12 @@ AirspeedValidator::reset_airspeed_to_invalid(const uint64_t timestamp) } void -AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float airspeed_true_raw, bool lpos_valid, +AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float airspeed_true_raw, bool gnss_valid, const matrix::Vector3f &vI, float lpos_evh, float lpos_evv, const Quatf &q_att) { _wind_estimator.update(time_now_usec); - if (lpos_valid && _in_fixed_wing_flight) { + if (gnss_valid && _in_fixed_wing_flight) { // airspeed fusion (with raw TAS) const float hor_vel_variance = lpos_evh * lpos_evh; @@ -109,9 +109,9 @@ AirspeedValidator::get_wind_estimator_states(uint64_t timestamp) } void -AirspeedValidator::update_CAS_scale_validated(bool lpos_valid, const matrix::Vector3f &vI, float airspeed_true_raw) +AirspeedValidator::update_CAS_scale_validated(bool gnss_valid, const matrix::Vector3f &vI, float airspeed_true_raw) { - if (!_in_fixed_wing_flight || !lpos_valid) { + if (!_in_fixed_wing_flight || !gnss_valid) { return; } @@ -212,7 +212,7 @@ AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now) void AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_status_vel_test_ratio, - float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool lpos_valid) + float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool gnss_valid) { // Check normalised innovation levels with requirement for continuous data and use of hysteresis // to prevent false triggering. @@ -226,7 +226,7 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ _innovations_check_failed = false; _aspd_innov_integ_state = 0.f; - } else if (!lpos_valid || estimator_status_vel_test_ratio > 1.f || estimator_status_mag_test_ratio > 1.f) { + } else if (!gnss_valid || estimator_status_vel_test_ratio > 1.f || estimator_status_mag_test_ratio > 1.f) { //nav velocity data is likely not good //don't run the test but don't reset the check if it had previously failed when nav velocity data was still likely good _aspd_innov_integ_state = 0.f; diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index ed7008a5c6..58f79e0009 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -56,7 +56,7 @@ struct airspeed_validator_update_data { float airspeed_true_raw; uint64_t airspeed_timestamp; matrix::Vector3f ground_velocity; - bool lpos_valid; + bool gnss_valid; float lpos_evh; float lpos_evv; matrix::Quatf q_att; @@ -175,15 +175,15 @@ private: 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, + void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool gnss_valid, const matrix::Vector3f &vI, float lpos_evh, float lpos_evv, const Quatf &q_att); - void update_CAS_scale_validated(bool lpos_valid, const matrix::Vector3f &vI, float airspeed_true_raw); + void update_CAS_scale_validated(bool gnss_valid, const 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); void check_airspeed_innovation(uint64_t timestamp, float estimator_status_vel_test_ratio, - float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool lpos_valid); + float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool gnss_valid); void check_load_factor(float accel_z); void update_airspeed_valid_status(const uint64_t timestamp); void reset(); diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index a25e2d4768..28fb87c2b6 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -146,7 +146,7 @@ private: int _valid_airspeed_index{-2}; /**< index of currently chosen (valid) airspeed sensor */ int _prev_airspeed_index{-2}; /**< previously chosen airspeed sensor index */ bool _initialized{false}; /**< module initialized*/ - bool _vehicle_local_position_valid{false}; /**< local position (from GPS) valid */ + bool _gnss_lpos_valid{false}; /**< local position (from GNSS) valid */ bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */ float _ground_minus_wind_TAS{NAN}; /**< true airspeed from groundspeed minus windspeed */ float _ground_minus_wind_CAS{NAN}; /**< calibrated airspeed from groundspeed minus windspeed */ @@ -347,7 +347,7 @@ AirspeedModule::Run() struct airspeed_validator_update_data input_data = {}; input_data.timestamp = _time_now_usec; input_data.ground_velocity = vI; - input_data.lpos_valid = _vehicle_local_position_valid; + input_data.gnss_valid = _gnss_lpos_valid; input_data.lpos_evh = _vehicle_local_position.evh; input_data.lpos_evv = _vehicle_local_position.evv; input_data.q_att = _q_att; @@ -515,10 +515,10 @@ void AirspeedModule::poll_topics() } } - _vehicle_local_position_valid = (_time_now_usec - _vehicle_local_position.timestamp < 1_s) - && (_vehicle_local_position.timestamp > 0) - && _vehicle_local_position.v_xy_valid - && !_vehicle_local_position.dead_reckoning; + _gnss_lpos_valid = (_time_now_usec - _vehicle_local_position.timestamp < 1_s) + && (_vehicle_local_position.timestamp > 0) + && _vehicle_local_position.v_xy_valid + && _estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS); } void AirspeedModule::update_wind_estimator_sideslip() @@ -526,7 +526,7 @@ void AirspeedModule::update_wind_estimator_sideslip() // update wind and airspeed estimator _wind_estimator_sideslip.update(_time_now_usec); - if (_vehicle_local_position_valid + if (_gnss_lpos_valid && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_vehicle_land_detected.landed) { Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz); @@ -603,8 +603,8 @@ void AirspeedModule::select_airspeed_and_publish() if (_valid_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX) { - // _vehicle_local_position_valid determines if ground-wind estimate is valid - if (_vehicle_local_position_valid && + // _gnss_lpos_valid determines if ground-wind estimate is valid + if (_gnss_lpos_valid && (_param_airspeed_fallback_gw.get() || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) { _valid_airspeed_index = airspeed_index::GROUND_MINUS_WIND_INDEX;