fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 03:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

AirspeedSelector: fix attitude rotation for tailsitter

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-03-22 10:05:32 +01:00
parent 6bee0893de
commit edc570adff
3 changed files with 22 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/airspeed_selector/airspeed_selector_main.cpp
+16 -9
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;