From cc2c631d23ee77d42cf69fef104c92ec4bd6c276 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 2 Sep 2020 20:45:06 -0600 Subject: [PATCH] AC_AttitudeControl: add AC_AttitudeControl_TS move tailsitter body-frame roll input method to new subclass override relax_attitude_controllers in AttitudeControl_TS --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 70 ---------- .../AC_AttitudeControl/AC_AttitudeControl.h | 10 +- .../AC_AttitudeControl_TS.cpp | 132 ++++++++++++++++++ .../AC_AttitudeControl_TS.h | 21 +++ 4 files changed, 160 insertions(+), 73 deletions(-) create mode 100644 libraries/AC_AttitudeControl/AC_AttitudeControl_TS.cpp create mode 100644 libraries/AC_AttitudeControl/AC_AttitudeControl_TS.h diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 1419494f12..b0e0b401ab 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -344,76 +344,6 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle attitude_controller_run_quat(); } -// Command euler yaw rate and pitch angle with roll angle specified in body frame -// (used only by tailsitter quadplanes) -// If plane_controls is true, swap the effects of roll and yaw as euler pitch approaches 90 degrees -void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float body_roll_cd, float euler_pitch_cd, float euler_yaw_rate_cds) -{ - // Convert from centidegrees on public interface to radians - float euler_yaw_rate = radians(euler_yaw_rate_cds*0.01f); - float euler_pitch = radians(constrain_float(euler_pitch_cd * 0.01f, -90.0f, 90.0f)); - float body_roll = radians(-body_roll_cd * 0.01f); - - const float cpitch = cosf(euler_pitch); - const float spitch = fabsf(sinf(euler_pitch)); - - // Compute attitude error - Quaternion attitude_vehicle_quat; - Quaternion error_quat; - attitude_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned()); - error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat; - Vector3f att_error; - error_quat.to_axis_angle(att_error); - - // update heading - float yaw_rate = euler_yaw_rate; - if (plane_controls) { - yaw_rate = (euler_yaw_rate * spitch) + (body_roll * cpitch); - } - // limit yaw error - float yaw_error = fabsf(att_error.z); - float error_ratio = yaw_error / M_PI_2; - if (error_ratio > 1) { - yaw_rate /= (error_ratio * error_ratio); - } - _attitude_target_euler_angle.z = wrap_PI(_attitude_target_euler_angle.z + yaw_rate * _dt); - - // init attitude target to desired euler yaw and pitch with zero roll - _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, spitch * body_roll)); - - // rotate body_yaw axis by cos(pitch angle) - Quaternion bf_yaw_Q; - if (plane_controls) { - bf_yaw_Q.from_axis_angle(Vector3f(cpitch, 0, 0), euler_yaw_rate); - } else { - bf_yaw_Q.from_axis_angle(Vector3f(-cpitch * body_roll, 0, 0)); - } - _attitude_target_quat = _attitude_target_quat * bf_roll_Q * bf_yaw_Q; - - // _attitude_target_euler_angle roll and pitch: Note: roll/yaw will be indeterminate when pitch is near +/-90 - // These should be used only for logging target eulers, with the caveat noted above. - // Also note that _attitude_target_quat.from_euler() should only be used in special circumstances - // such as when attitude is specified directly in terms of Euler angles. - // _attitude_target_euler_angle.x = _attitude_target_quat.get_euler_roll(); - // _attitude_target_euler_angle.y = euler_pitch; - - // Set rate feedforward requests to zero - _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f); - _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f); - - // Compute attitude error - error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat; - error_quat.to_axis_angle(att_error); - - // Compute the angular velocity target from the attitude error - _rate_target_ang_vel = update_ang_vel_target_from_att_error(att_error); -} - // Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds) { diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 4c4552fa92..a901722731 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -109,7 +109,10 @@ public: void set_input_tc(float input_tc) { _input_tc = constrain_float(input_tc, 0.0f, 1.0f); } // Ensure attitude controller have zero errors to relax rate controller output - void relax_attitude_controllers(); + virtual void relax_attitude_controllers(); + + // Used by child class AC_AttitudeControl_TS to change behavior for tailsitter quadplanes + virtual void relax_attitude_controllers(bool exclude_pitch) { relax_attitude_controllers(); } // reset rate controller I terms void reset_rate_controller_I_terms(); @@ -136,8 +139,9 @@ public: virtual void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw); // Command euler yaw rate and pitch angle with roll angle specified in body frame - // (used only by tailsitter quadplanes) - virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds); + // (implemented only in AC_AttitudeControl_TS for tailsitter quadplanes) + virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float euler_roll_angle_cd, + float euler_pitch_angle_cd, float euler_yaw_rate_cds) {} // Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing void input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_TS.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_TS.cpp new file mode 100644 index 0000000000..4b1dfdecd9 --- /dev/null +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_TS.cpp @@ -0,0 +1,132 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + This class inherits from AC_AttitudeControl_Multi and provides functionality + specific to tailsitter quadplanes. + 1) "body-frame" roll control mode for all tailsitters + 2) a relax_attitude_controller method needed for coping with vectored tailsitters + */ +#include "AC_AttitudeControl_TS.h" + + +AC_AttitudeControl_TS::AC_AttitudeControl_TS(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt) : + AC_AttitudeControl_Multi(ahrs, aparm, motors, dt) +{ +} + +void AC_AttitudeControl_TS::relax_attitude_controllers(bool exclude_pitch) +{ + // If exclude_pitch: relax roll and yaw rate controller outputs only, + // leaving pitch controller active to let TVBS motors tilt up while in throttle_wait + if (exclude_pitch) { + // Get the current attitude quaternion + Quaternion current_attitude; + _ahrs.get_quat_body_to_ned(current_attitude); + + Vector3f current_eulers; + current_attitude.to_euler(current_eulers.x, current_eulers.y, current_eulers.z); + + // set target attitude to zero pitch with (approximate) current roll and yaw + // by rotating the current_attitude quaternion by the error in desired pitch + Quaternion pitch_rotation; + pitch_rotation.from_axis_angle(Vector3f(0, -1, 0), current_eulers.y); + _attitude_target_quat = current_attitude * pitch_rotation; + _attitude_target_quat.normalize(); + _attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z); + _attitude_ang_error = current_attitude.inverse() * _attitude_target_quat; + + // Initialize the roll and yaw angular rate variables to the current rate + _attitude_target_ang_vel = _ahrs.get_gyro(); + ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate); + _rate_target_ang_vel.x = _ahrs.get_gyro().x; + _rate_target_ang_vel.z = _ahrs.get_gyro().z; + + // Reset the roll and yaw I terms + get_rate_roll_pid().reset_I(); + get_rate_yaw_pid().reset_I(); + } else { + // relax all attitude controllers + AC_AttitudeControl::relax_attitude_controllers(); + } +} + +// Command euler yaw rate and pitch angle with roll angle specified in body frame +// (used only by tailsitter quadplanes) +// If plane_controls is true, swap the effects of roll and yaw as euler pitch approaches 90 degrees +void AC_AttitudeControl_TS::input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float body_roll_cd, float euler_pitch_cd, float euler_yaw_rate_cds) +{ + // Convert from centidegrees on public interface to radians + float euler_yaw_rate = radians(euler_yaw_rate_cds*0.01f); + float euler_pitch = radians(constrain_float(euler_pitch_cd * 0.01f, -90.0f, 90.0f)); + float body_roll = radians(-body_roll_cd * 0.01f); + + const float cpitch = cosf(euler_pitch); + const float spitch = fabsf(sinf(euler_pitch)); + + // Compute attitude error + Quaternion attitude_vehicle_quat; + Quaternion error_quat; + _ahrs.get_quat_body_to_ned(attitude_vehicle_quat); + error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat; + Vector3f att_error; + error_quat.to_axis_angle(att_error); + + // update heading + float yaw_rate = euler_yaw_rate; + if (plane_controls) { + yaw_rate = (euler_yaw_rate * spitch) + (body_roll * cpitch); + } + // limit yaw error + float yaw_error = fabsf(att_error.z); + float error_ratio = yaw_error / M_PI_2; + if (error_ratio > 1) { + yaw_rate /= (error_ratio * error_ratio); + } + _attitude_target_euler_angle.z = wrap_PI(_attitude_target_euler_angle.z + yaw_rate * _dt); + + // init attitude target to desired euler yaw and pitch with zero roll + _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, spitch * body_roll)); + + // rotate body_yaw axis by cos(pitch angle) + Quaternion bf_yaw_Q; + if (plane_controls) { + bf_yaw_Q.from_axis_angle(Vector3f(cpitch, 0, 0), euler_yaw_rate); + } else { + bf_yaw_Q.from_axis_angle(Vector3f(-cpitch * body_roll, 0, 0)); + } + _attitude_target_quat = _attitude_target_quat * bf_roll_Q * bf_yaw_Q; + + // _attitude_target_euler_angle roll and pitch: Note: roll/yaw will be indeterminate when pitch is near +/-90 + // These should be used only for logging target eulers, with the caveat noted above. + // Also note that _attitude_target_quat.from_euler() should only be used in special circumstances + // such as when attitude is specified directly in terms of Euler angles. + // _attitude_target_euler_angle.x = _attitude_target_quat.get_euler_roll(); + // _attitude_target_euler_angle.y = euler_pitch; + + // Set rate feedforward requests to zero + _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f); + _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f); + + // Compute attitude error + error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat; + error_quat.to_axis_angle(att_error); + + // Compute the angular velocity target from the attitude error + _rate_target_ang_vel = update_ang_vel_target_from_att_error(att_error); +} diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_TS.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_TS.h new file mode 100644 index 0000000000..f300131e6d --- /dev/null +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_TS.h @@ -0,0 +1,21 @@ +#pragma once + +/// @file AC_AttitudeControl_TVBS.h +/// @brief ArduCopter attitude control library + +#include "AC_AttitudeControl_Multi.h" + + +class AC_AttitudeControl_TS : public AC_AttitudeControl_Multi +{ +public: + AC_AttitudeControl_TS(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt); + + // empty destructor to suppress compiler warning + virtual ~AC_AttitudeControl_TS() {} + + // Ensure attitude controllers have zero errors to relax rate controller output + // Relax only the roll and yaw rate controllers if exclude_pitch is true + virtual void relax_attitude_controllers(bool exclude_pitch) override; + virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float body_roll_cd, float euler_pitch_cd, float euler_yaw_rate_cds) override; +};