Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             : 
      15             :    This class inherits from AC_AttitudeControl_Multi and provides functionality
      16             :    specific to tailsitter quadplanes.
      17             :    1) "body-frame" roll control mode for all tailsitters
      18             :    2) a relax_attitude_controller method needed for coping with vectored tailsitters
      19             :  */
      20             : #include "AC_AttitudeControl_TS.h"
      21             : 
      22      318754 : void AC_AttitudeControl_TS::relax_attitude_controllers(bool exclude_pitch)
      23             : {
      24             :     // If exclude_pitch: relax roll and yaw rate controller outputs only,
      25             :     // leaving pitch controller active to let TVBS motors tilt up while in throttle_wait
      26      318754 :     if (exclude_pitch) {
      27             :         // Get the current attitude quaternion
      28           0 :         Quaternion current_attitude;
      29           0 :         _ahrs.get_quat_body_to_ned(current_attitude);
      30             : 
      31           0 :         Vector3f current_eulers;
      32           0 :         current_attitude.to_euler(current_eulers.x, current_eulers.y, current_eulers.z);
      33             : 
      34             :         // set target attitude to zero pitch with (approximate) current roll and yaw
      35             :         // by rotating the current_attitude quaternion by the error in desired pitch
      36           0 :         Quaternion pitch_rotation;
      37           0 :         pitch_rotation.from_axis_angle(Vector3f{0, -1, 0}, current_eulers.y);
      38           0 :         _attitude_target = current_attitude * pitch_rotation;
      39           0 :         _attitude_target.normalize();
      40           0 :         _attitude_target.to_euler(_euler_angle_target_rad.x, _euler_angle_target_rad.y, _euler_angle_target_rad.z);
      41           0 :         _attitude_ang_error = current_attitude.inverse() * _attitude_target;
      42             : 
      43             :         // Initialize the roll and yaw angular rate variables to the current rate
      44           0 :         _ang_vel_target_rads = _ahrs.get_gyro();
      45           0 :         _ang_vel_body_rads.x = _ang_vel_target_rads.x;
      46           0 :         _ang_vel_body_rads.z = _ang_vel_target_rads.z;
      47           0 :         body_to_euler_derivative(_attitude_target, _ang_vel_target_rads, _euler_rate_target_rads);
      48             : 
      49             :         // Reset the roll and yaw I terms
      50           0 :         get_rate_roll_pid().reset_I();
      51           0 :         get_rate_yaw_pid().reset_I();
      52             :     } else {
      53             :         // relax all attitude controllers
      54      318754 :         AC_AttitudeControl::relax_attitude_controllers();
      55             :     }
      56      318754 : }
      57             : 
      58             : // Commands a body-frame roll angle (in centidegrees), an euler pitch angle (in centidegrees), and a yaw rate (in centidegrees/s).
      59             : // See input_euler_rate_yaw_euler_angle_pitch_bf_roll_rad() for full details.
      60           0 : void AC_AttitudeControl_TS::input_euler_rate_yaw_euler_angle_pitch_bf_roll_cd(bool plane_controls, float body_roll_cd, float euler_pitch_cd, float euler_yaw_rate_cds)
      61             : {
      62             :     // Convert from centidegrees on public interface to radians
      63           0 :     float euler_yaw_rate_rads = cd_to_rad(euler_yaw_rate_cds);
      64           0 :     float euler_pitch_rad    = cd_to_rad(euler_pitch_cd);
      65           0 :     float body_roll_rad      = cd_to_rad(body_roll_cd);
      66             : 
      67           0 :     input_euler_rate_yaw_euler_angle_pitch_bf_roll_rad(plane_controls, body_roll_rad, euler_pitch_rad, euler_yaw_rate_rads);
      68           0 : }
      69             : 
      70             : // Commands a body-frame roll angle (in radians), an euler pitch angle (in radians), and a yaw rate (in radians/s).
      71             : // Used by tailsitter quadplanes. Optionally swaps roll and yaw effects as pitch nears 90° if plane_controls is true.
      72           0 : void AC_AttitudeControl_TS::input_euler_rate_yaw_euler_angle_pitch_bf_roll_rad(bool plane_controls, float body_roll_rad, float euler_pitch_rad, float euler_yaw_rate_rads)
      73             : {
      74           0 :     euler_pitch_rad    = constrain_float(euler_pitch_rad, -radians(90.0), radians(90.0));
      75           0 :     body_roll_rad      = -body_roll_rad;
      76             : 
      77           0 :     const float cpitch = cosf(euler_pitch_rad);
      78           0 :     const float spitch = fabsf(sinf(euler_pitch_rad));
      79             : 
      80             :     // Compute attitude error
      81           0 :     Quaternion attitude_body;
      82           0 :     Quaternion error_quat;
      83           0 :     _ahrs.get_quat_body_to_ned(attitude_body);
      84           0 :     error_quat = attitude_body.inverse() * _attitude_target;
      85           0 :     Vector3f att_error;
      86           0 :     error_quat.to_axis_angle(att_error);
      87             : 
      88             :     // update heading
      89           0 :     float yaw_rate_rads = euler_yaw_rate_rads;
      90           0 :     if (plane_controls) {
      91           0 :         yaw_rate_rads = (euler_yaw_rate_rads * spitch) + (body_roll_rad * cpitch);
      92             :     }
      93             :     // limit yaw error
      94           0 :     float yaw_error = fabsf(att_error.z);
      95           0 :     float error_ratio = yaw_error / M_PI_2;
      96           0 :     if (error_ratio > 1) {
      97           0 :         yaw_rate_rads /= (error_ratio * error_ratio);
      98             :     }
      99           0 :     _euler_angle_target_rad.z = wrap_PI(_euler_angle_target_rad.z + yaw_rate_rads * _dt_s);
     100             : 
     101             :     // init attitude target to desired euler yaw and pitch with zero roll
     102           0 :     _attitude_target.from_euler(0, euler_pitch_rad, _euler_angle_target_rad.z);
     103             : 
     104             :     // apply body-frame yaw/roll (this is roll/yaw for a tailsitter in forward flight)
     105             :     // rotate body_roll_rad axis by |sin(pitch angle)|
     106           0 :     Quaternion bf_roll_Q;
     107           0 :     bf_roll_Q.from_axis_angle(Vector3f{0, 0, spitch * body_roll_rad});
     108             : 
     109             :     // rotate body_yaw axis by cos(pitch angle)
     110           0 :     Quaternion bf_yaw_Q;
     111           0 :     if (plane_controls) {
     112           0 :         bf_yaw_Q.from_axis_angle(Vector3f{cpitch, 0, 0}, euler_yaw_rate_rads);
     113             :     } else {
     114           0 :         bf_yaw_Q.from_axis_angle(Vector3f{-cpitch * body_roll_rad, 0, 0});
     115             :     }
     116           0 :     _attitude_target = _attitude_target * bf_roll_Q * bf_yaw_Q;
     117             : 
     118             :     // _euler_angle_target_rad roll and pitch: Note: roll/yaw will be indeterminate when pitch is near +/-90
     119             :     // These should be used only for logging target eulers, with the caveat noted above.
     120             :     // Also note that _attitude_target.from_euler() should only be used in special circumstances
     121             :     // such as when attitude is specified directly in terms of Euler angles.
     122             :     //    _euler_angle_target_rad.x = _attitude_target.get_euler_roll();
     123             :     //    _euler_angle_target_rad.y = euler_pitch_rad;
     124             : 
     125             :     // Set rate feedforward requests to zero
     126           0 :     _euler_rate_target_rads.zero();
     127           0 :     _ang_accel_target_rads.zero();
     128           0 :     _ang_vel_target_rads.zero();
     129             : 
     130             :     // Compute attitude error
     131           0 :     error_quat = attitude_body.inverse() * _attitude_target;
     132           0 :     error_quat.to_axis_angle(att_error);
     133             : 
     134             :     // Compute the angular velocity target from the attitude error
     135           0 :     _ang_vel_body_rads = update_ang_vel_target_from_att_error(att_error);
     136           0 : }