From ccab93e68bf7590d5218f9bd3c0e153501298013 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 12 Oct 2021 20:14:30 +0200 Subject: [PATCH] AirspeedSelector: use Vector3f Signed-off-by: Silvan Fuhrer --- .../airspeed_selector/AirspeedValidator.cpp | 17 +++++++---------- .../airspeed_selector/AirspeedValidator.hpp | 10 +++------- .../airspeed_selector_main.cpp | 6 +++--- 3 files changed, 13 insertions(+), 20 deletions(-) diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index cf3a5f07f2..0a630006f2 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -50,12 +50,11 @@ 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.lpos_vx, input_data.lpos_vy, input_data.lpos_vz); + update_CAS_scale_estimated(input_data.lpos_valid, input_data.ground_velocity); 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.lpos_vx, - input_data.lpos_vy, - input_data.lpos_vz, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q); + 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); update_in_fixed_wing_flight(input_data.in_fixed_wing_flight); check_airspeed_data_stuck(input_data.timestamp); check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio); @@ -72,14 +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, - float lpos_vx, float lpos_vy, - float lpos_vz, float lpos_evh, float lpos_evv, const float att_q[4]) + matrix::Vector3f vI, float lpos_evh, float lpos_evv, const float att_q[4]) { _wind_estimator.update(time_now_usec); if (lpos_valid && _in_fixed_wing_flight) { - Vector3f vI(lpos_vx, lpos_vy, lpos_vz); Quatf q(att_q); // airspeed fusion (with raw TAS) @@ -117,7 +114,7 @@ AirspeedValidator::get_wind_estimator_states(uint64_t timestamp) } void -AirspeedValidator::update_CAS_scale_estimated(bool lpos_valid, float vx, float vy, float vz) +AirspeedValidator::update_CAS_scale_estimated(bool lpos_valid, matrix::Vector3f vI) { if (!_in_fixed_wing_flight) { return; @@ -128,10 +125,10 @@ AirspeedValidator::update_CAS_scale_estimated(bool lpos_valid, float vx, float v reset_CAS_scale_check(); } - const float course_over_ground_rad = matrix::wrap_2pi(atan2f(vy, vx)); + const float course_over_ground_rad = matrix::wrap_2pi(atan2f(vI(0), vI(1))); 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_groundspeed(segment_index) = vI.norm(); _scale_check_TAS(segment_index) = _TAS; // run check if all segments are filled diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index fcf1ad04da..3cda6bfb5f 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -55,9 +55,7 @@ struct airspeed_validator_update_data { float airspeed_indicated_raw; float airspeed_true_raw; uint64_t airspeed_timestamp; - float lpos_vx; - float lpos_vy; - float lpos_vz; + matrix::Vector3f ground_velocity; bool lpos_valid; float lpos_evh; float lpos_evv; @@ -173,11 +171,9 @@ 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, float lpos_vx, - float lpos_vy, - float lpos_vz, + 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, float vx, float vy, float vz); + void update_CAS_scale_estimated(bool lpos_valid, matrix::Vector3f vI); 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 0b3525522f..8f85d7bb9a 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -329,12 +329,12 @@ AirspeedModule::Run() _position_setpoint.type != position_setpoint_s::SETPOINT_TYPE_LAND && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + const matrix::Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz); + // Prepare data for airspeed_validator struct airspeed_validator_update_data input_data = {}; input_data.timestamp = _time_now_usec; - input_data.lpos_vx = _vehicle_local_position.vx; - input_data.lpos_vy = _vehicle_local_position.vy; - input_data.lpos_vz = _vehicle_local_position.vz; + input_data.ground_velocity = vI; input_data.lpos_valid = _vehicle_local_position_valid; input_data.lpos_evh = _vehicle_local_position.evh; input_data.lpos_evv = _vehicle_local_position.evv;