From ab4731a99f8888062791c3d1feede47919c3943a Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 19 Aug 2016 17:30:20 +0200 Subject: [PATCH] fw_pos_control_l1: for tailsitters we need to rotate the body acceleration vector as it has it's reference in the mc frame Signed-off-by: Roman --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3fc327a33b..74b26ea3fb 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1285,6 +1285,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* filter speed and altitude for controller */ math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); + + // tailsitters use the multicopter frame as reference, in fixed wing + // we need to use the fixed wing frame + if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { + float tmp = accel_body(0); + accel_body(0) = -accel_body(2); + accel_body(2) = tmp; + } + math::Vector<3> accel_earth = _R_nb * accel_body; /* tell TECS to update its state, but let it know when it cannot actually control the plane */