From 4e9b6d1919f4df72cc2e5d83b88d7b36c2945413 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 15 Mar 2019 21:32:47 -0600 Subject: [PATCH] AC_AttitudeControl: improve tailsitter bodyframe roll control --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 33 +++++++------------ .../AC_AttitudeControl/AC_AttitudeControl.h | 5 --- 2 files changed, 11 insertions(+), 27 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index aeee0580aa..4a1789f536 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -366,32 +366,21 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll(float eu float euler_pitch = radians(euler_pitch_cd*0.01f); float body_roll = radians(body_roll_cd*0.01f); - // back out the body roll to get current euler_yaw - Quaternion bf_roll_Q; - bf_roll_Q.from_axis_angle(Vector3f(0, 0, -_last_body_roll)); - Quaternion base_att_Q = _attitude_target_quat * bf_roll_Q; - - // avoid Euler singularities - if (_last_euler_pitch > M_PI_4) { - base_att_Q.rotate(Vector3f(0,-M_PI_2,0)); - } else if (_last_euler_pitch < -M_PI_4) { - base_att_Q.rotate(Vector3f(0,M_PI_2,0)); - } - - // current heading - float heading = base_att_Q.get_euler_yaw(); - // new heading - heading = wrap_PI(heading + euler_yaw_rate * _dt); + _attitude_target_euler_angle.z = wrap_PI(_attitude_target_euler_angle.z + euler_yaw_rate * _dt); // init attitude target to desired euler yaw and pitch with zero roll - _attitude_target_quat.from_euler(0, euler_pitch, heading); - _last_euler_pitch = euler_pitch; + _attitude_target_quat.from_euler(0, euler_pitch, _attitude_target_euler_angle.z); + + // apply body-frame yaw/roll (this is roll/yaw for a tailsitter in forward flight) + // rotate body_roll axis by |sin(pitch angle)| + Quaternion bf_roll_Q; + bf_roll_Q.from_axis_angle(Vector3f(0, 0, fabsf(sinf(euler_pitch)) * body_roll)); - // apply body-frame yaw (this is roll for a tailsitter in forward flight) - bf_roll_Q.from_axis_angle(Vector3f(0, 0, body_roll)); - _last_body_roll = body_roll; - _attitude_target_quat = _attitude_target_quat * bf_roll_Q; + // rotate body_yaw axis by cos(pitch angle) + Quaternion bf_yaw_Q; + bf_yaw_Q.from_axis_angle(Vector3f(-cosf(euler_pitch), 0, 0), body_roll); + _attitude_target_quat = _attitude_target_quat * bf_roll_Q * bf_yaw_Q; // Set rate feedforward requests to zero _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 5e8ef292ac..e4e55dceac 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -434,11 +434,6 @@ protected: // true in inverted flight mode bool _inverted_flight; - // state for input_euler_rate_yaw_euler_angle_pitch_bf_roll() - // (would be expensive to compute from _attitude_target_quat) - float _last_body_roll; - float _last_euler_pitch; - public: // log a CTRL message void control_monitor_log(void);