dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

AirspeedSelector: fix attitude rotation for tailsitter 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-03-16 15:23:22 +01:00
parent 6bee0893de
commit edc570adff
3 changed files with 22 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/airspeed_selector/AirspeedValidator.cpp
View File

@ -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);
}
}

4
src/modules/airspeed_selector/AirspeedValidator.hpp
View File

@ -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);

25
src/modules/airspeed_selector/airspeed_selector_main.cpp
View File

@ -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;