Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 11:59:17 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

AirspeedSelector: use Vector3f

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-10-12 20:14:30 +02:00
parent 259b851ba7
commit ccab93e68b
3 changed files with 13 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/airspeed_selector/AirspeedValidator.cpp
+7 -10
View File
@@ -50,12 +50,11 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat
// get indicated airspeed from input data (raw airspeed) // get indicated airspeed from input data (raw airspeed)
_IAS = input_data.airspeed_indicated_raw; _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_scale_applied();
update_CAS_TAS(input_data.air_pressure_pa, input_data.air_temperature_celsius); 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, update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid,
input_data.lpos_vy, input_data.ground_velocity, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q);
input_data.lpos_vz, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q);
update_in_fixed_wing_flight(input_data.in_fixed_wing_flight); update_in_fixed_wing_flight(input_data.in_fixed_wing_flight);
check_airspeed_data_stuck(input_data.timestamp); check_airspeed_data_stuck(input_data.timestamp);
check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio); 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 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 lpos_valid,
float lpos_vx, float lpos_vy, matrix::Vector3f vI, float lpos_evh, float lpos_evv, const float att_q[4])
float lpos_vz, float lpos_evh, float lpos_evv, const float att_q[4])
{ {
_wind_estimator.update(time_now_usec); _wind_estimator.update(time_now_usec);
if (lpos_valid && _in_fixed_wing_flight) { if (lpos_valid && _in_fixed_wing_flight) {
Vector3f vI(lpos_vx, lpos_vy, lpos_vz);
Quatf q(att_q); Quatf q(att_q);
// airspeed fusion (with raw TAS) // airspeed fusion (with raw TAS)
@@ -117,7 +114,7 @@ AirspeedValidator::get_wind_estimator_states(uint64_t timestamp)
} }
void 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) { if (!_in_fixed_wing_flight) {
return; return;
@@ -128,10 +125,10 @@ AirspeedValidator::update_CAS_scale_estimated(bool lpos_valid, float vx, float v
reset_CAS_scale_check(); 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)); 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; _scale_check_TAS(segment_index) = _TAS;
// run check if all segments are filled // run check if all segments are filled
src/modules/airspeed_selector/AirspeedValidator.hpp
+3 -7
View File
@@ -55,9 +55,7 @@ struct airspeed_validator_update_data {
float airspeed_indicated_raw; float airspeed_indicated_raw;
float airspeed_true_raw; float airspeed_true_raw;
uint64_t airspeed_timestamp; uint64_t airspeed_timestamp;
float lpos_vx; matrix::Vector3f ground_velocity;
float lpos_vy;
float lpos_vz;
bool lpos_valid; bool lpos_valid;
float lpos_evh; float lpos_evh;
float lpos_evv; 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_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, void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool lpos_valid, matrix::Vector3f vI,
float lpos_vy,
float lpos_vz,
float lpos_evh, float lpos_evv, const float att_q[4]); 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_scale_applied();
void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius); void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius);
void check_airspeed_data_stuck(uint64_t timestamp); void check_airspeed_data_stuck(uint64_t timestamp);
src/modules/airspeed_selector/airspeed_selector_main.cpp
+3 -3
View File
@@ -329,12 +329,12 @@ AirspeedModule::Run()
_position_setpoint.type != position_setpoint_s::SETPOINT_TYPE_LAND && _position_setpoint.type != position_setpoint_s::SETPOINT_TYPE_LAND &&
_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; _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 // Prepare data for airspeed_validator
struct airspeed_validator_update_data input_data = {}; struct airspeed_validator_update_data input_data = {};
input_data.timestamp = _time_now_usec; input_data.timestamp = _time_now_usec;
input_data.lpos_vx = _vehicle_local_position.vx; input_data.ground_velocity = vI;
input_data.lpos_vy = _vehicle_local_position.vy;
input_data.lpos_vz = _vehicle_local_position.vz;
input_data.lpos_valid = _vehicle_local_position_valid; input_data.lpos_valid = _vehicle_local_position_valid;
input_data.lpos_evh = _vehicle_local_position.evh; input_data.lpos_evh = _vehicle_local_position.evh;
input_data.lpos_evv = _vehicle_local_position.evv; input_data.lpos_evv = _vehicle_local_position.evv;