From 0f1c9504d57826a3bfacff0af214c9a3a7080af0 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 18 Apr 2018 15:31:07 +0200 Subject: [PATCH] FixedWingPositionControl: rotate attitude consistenly for tailsitters Signed-off-by: Roman --- .../FixedwingPositionControl.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 1f918f8445..45796ac151 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -412,6 +412,13 @@ FixedwingPositionControl::vehicle_attitude_poll() /* set rotation matrix and euler angles */ _R_nb = Quatf(_att.q); + // if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset + // between multirotor and fixed wing flight + if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { + Dcmf R_offset = Eulerf(0, M_PI_2_F, 0); + _R_nb = _R_nb * R_offset; + } + Eulerf euler_angles(_R_nb); _roll = euler_angles(0); _pitch = euler_angles(1); @@ -1878,14 +1885,6 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee /* Using tecs library */ float pitch_for_tecs = _pitch - _parameters.pitchsp_offset_rad; - // if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset - // between multirotor and fixed wing flight - if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { - Dcmf R_offset = Eulerf(0, M_PI_2_F, 0); - Eulerf euler = Eulerf(_R_nb * R_offset); - pitch_for_tecs = euler(1); - } - /* filter speed and altitude for controller */ Vector3f accel_body(_sub_sensors.get().accel_x, _sub_sensors.get().accel_y, _sub_sensors.get().accel_z);