diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index dd72dd75e9..71db6b9c4f 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -54,7 +54,7 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat 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, - input_data.ground_velocity, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q); + 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); @@ -72,20 +72,18 @@ 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, - const matrix::Vector3f &vI, float lpos_evh, float lpos_evv, const float att_q[4]) + 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) { - Quatf q(att_q); - // airspeed fusion (with raw TAS) const float hor_vel_variance = lpos_evh * lpos_evh; - _wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, hor_vel_variance, q); + _wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, hor_vel_variance, q_att); // sideslip fusion - _wind_estimator.fuse_beta(time_now_usec, vI, hor_vel_variance, q); + _wind_estimator.fuse_beta(time_now_usec, vI, hor_vel_variance, q_att); } } diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index f0eebd819e..69985fc448 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -59,7 +59,7 @@ struct airspeed_validator_update_data { bool lpos_valid; float lpos_evh; float lpos_evv; - float att_q[4]; + matrix::Quatf q_att; float air_pressure_pa; float air_temperature_celsius; float accel_z; @@ -179,7 +179,7 @@ private: void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool lpos_valid, const matrix::Vector3f &vI, - float lpos_evh, float lpos_evv, const float att_q[4]); + 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_applied(); void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius); diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 5098a0bb08..2cd37691f0 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -128,7 +128,6 @@ private: estimator_status_s _estimator_status {}; vehicle_acceleration_s _accel {}; vehicle_air_data_s _vehicle_air_data {}; - vehicle_attitude_s _vehicle_attitude {}; vehicle_land_detected_s _vehicle_land_detected {}; vehicle_local_position_s _vehicle_local_position {}; vehicle_status_s _vehicle_status {}; @@ -142,6 +141,7 @@ private: int32_t _prev_number_of_airspeed_sensors{0}; /**< number of airspeed sensors in previous loop (to detect a new added sensor)*/ AirspeedValidator _airspeed_validator[MAX_NUM_AIRSPEED_SENSORS] {}; /**< airspeedValidator instances (one for each sensor) */ + matrix::Quatf _q_att; hrt_abstime _time_now_usec{0}; int _valid_airspeed_index{-2}; /**< index of currently chosen (valid) airspeed sensor */ int _prev_airspeed_index{-2}; /**< previously chosen airspeed sensor index */ @@ -350,10 +350,7 @@ AirspeedModule::Run() input_data.lpos_valid = _vehicle_local_position_valid; input_data.lpos_evh = _vehicle_local_position.evh; input_data.lpos_evv = _vehicle_local_position.evv; - input_data.att_q[0] = _vehicle_attitude.q[0]; - input_data.att_q[1] = _vehicle_attitude.q[1]; - input_data.att_q[2] = _vehicle_attitude.q[2]; - input_data.att_q[3] = _vehicle_attitude.q[3]; + input_data.q_att = _q_att; input_data.air_pressure_pa = _vehicle_air_data.baro_pressure_pa; input_data.accel_z = _accel.xyz[2]; input_data.vel_test_ratio = _estimator_status.vel_test_ratio; @@ -498,13 +495,25 @@ void AirspeedModule::poll_topics() _estimator_status_sub.update(&_estimator_status); _vehicle_acceleration_sub.update(&_accel); _vehicle_air_data_sub.update(&_vehicle_air_data); - _vehicle_attitude_sub.update(&_vehicle_attitude); _vehicle_land_detected_sub.update(&_vehicle_land_detected); _vehicle_status_sub.update(&_vehicle_status); _vtol_vehicle_status_sub.update(&_vtol_vehicle_status); _vehicle_local_position_sub.update(&_vehicle_local_position); _position_setpoint_sub.update(&_position_setpoint); + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude; + _vehicle_attitude_sub.update(&vehicle_attitude); + + if (_vehicle_status.is_vtol_tailsitter && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + + // if the vehicle is a tailsitter we have to rotate the attitude by 90° to get to the airspeed frame + _q_att = Quatf(vehicle_attitude.q) * Quatf(matrix::Eulerf(0.f, M_PI_2_F, 0.f)); + + } else { + _q_att = Quatf(vehicle_attitude.q); + } + } _vehicle_local_position_valid = (_time_now_usec - _vehicle_local_position.timestamp < 1_s) && (_vehicle_local_position.timestamp > 0) @@ -521,11 +530,9 @@ void AirspeedModule::update_wind_estimator_sideslip() && _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); - Quatf q(_vehicle_attitude.q); const float hor_vel_variance = _vehicle_local_position.evh * _vehicle_local_position.evh; - - _wind_estimator_sideslip.fuse_beta(_time_now_usec, vI, hor_vel_variance, q); + _wind_estimator_sideslip.fuse_beta(_time_now_usec, vI, hor_vel_variance, _q_att); } _wind_estimate_sideslip.timestamp = _time_now_usec;