ArduPlane/mode_acro.cpp

   1 #include "mode.h"
   2 #include "Plane.h"
   3
   4 bool ModeAcro::_enter()
   5 {
   6     acro_state.locked_roll = false;
   7     acro_state.locked_pitch = false;
   8     IGNORE_RETURN(ahrs.get_quaternion(acro_state.q));
   9     return true;
  10 }
  11
  12 void ModeAcro::update()
  13 {
  14     // handle locked/unlocked control
  15     if (acro_state.locked_roll) {
  16         plane.nav_roll_cd = acro_state.locked_roll_err;
  17     } else {
  18         plane.nav_roll_cd = ahrs.roll_sensor;
  19     }
  20     if (acro_state.locked_pitch) {
  21         plane.nav_pitch_cd = acro_state.locked_pitch_cd;
  22     } else {
  23         plane.nav_pitch_cd = ahrs.pitch_sensor;
  24     }
  25 }
  26
  27 void ModeAcro::run()
  28 {
  29     output_pilot_throttle();
  30
  31     if (plane.g.acro_locking == 2 && plane.g.acro_yaw_rate > 0 &&
  32         plane.yawController.rate_control_enabled()) {
  33         // we can do 3D acro locking
  34         stabilize_quaternion();
  35         return;
  36     }
  37
  38     // Normal acro
  39     stabilize();
  40 }
  41
  42 /*
  43   this is the ACRO mode stabilization function. It does rate
  44   stabilization on roll and pitch axes
  45  */
  46 void ModeAcro::stabilize()
  47 {
  48     const float speed_scaler = plane.get_speed_scaler();
  49     const float rexpo = plane.roll_in_expo(true);
  50     const float pexpo = plane.pitch_in_expo(true);
  51     float roll_rate = (rexpo/SERVO_MAX) * plane.g.acro_roll_rate;
  52     float pitch_rate = (pexpo/SERVO_MAX) * plane.g.acro_pitch_rate;
  53
  54     IGNORE_RETURN(ahrs.get_quaternion(acro_state.q));
  55
  56     /*
  57       check for special roll handling near the pitch poles
  58      */
  59     if (plane.g.acro_locking && is_zero(roll_rate)) {
  60         /*
  61           we have no roll stick input, so we will enter "roll locked"
  62           mode, and hold the roll we had when the stick was released
  63          */
  64         if (!acro_state.locked_roll) {
  65             acro_state.locked_roll = true;
  66             acro_state.locked_roll_err = 0;
  67         } else {
  68             acro_state.locked_roll_err += ahrs.get_gyro().x * plane.G_Dt;
  69         }
  70         int32_t roll_error_cd = -ToDeg(acro_state.locked_roll_err)*100;
  71         plane.nav_roll_cd = ahrs.roll_sensor + roll_error_cd;
  72         // try to reduce the integrated angular error to zero. We set
  73         // 'stabilize' to true, which disables the roll integrator
  74         SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_servo_out(roll_error_cd,
  75                                                                                              speed_scaler,
  76                                                                                              true, false));
  77     } else {
  78         /*
  79           aileron stick is non-zero, use pure rate control until the
  80           user releases the stick
  81          */
  82         acro_state.locked_roll = false;
  83         SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_rate_out(roll_rate,  speed_scaler));
  84     }
  85
  86     if (plane.g.acro_locking && is_zero(pitch_rate)) {
  87         /*
  88           user has zero pitch stick input, so we lock pitch at the
  89           point they release the stick
  90          */
  91         if (!acro_state.locked_pitch) {
  92             acro_state.locked_pitch = true;
  93             acro_state.locked_pitch_cd = ahrs.pitch_sensor;
  94         }
  95         // try to hold the locked pitch. Note that we have the pitch
  96         // integrator enabled, which helps with inverted flight
  97         plane.nav_pitch_cd = acro_state.locked_pitch_cd;
  98         SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitchController.get_servo_out(plane.nav_pitch_cd - ahrs.pitch_sensor,
  99                                                                                                speed_scaler,
 100                                                                                                false, false));
 101     } else {
 102         /*
 103           user has non-zero pitch input, use a pure rate controller
 104          */
 105         acro_state.locked_pitch = false;
 106         SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitchController.get_rate_out(pitch_rate, speed_scaler));
 107     }
 108
 109     float rudder_output;
 110     if (plane.g.acro_yaw_rate > 0 && plane.yawController.rate_control_enabled()) {
 111         // user has asked for yaw rate control with yaw rate scaled by ACRO_YAW_RATE
 112         const float rudd_expo = plane.rudder_in_expo(true);
 113         const float yaw_rate = (rudd_expo/SERVO_MAX) * plane.g.acro_yaw_rate;
 114         rudder_output = plane.yawController.get_rate_out(yaw_rate,  speed_scaler, false);
 115     } else if (plane.flight_option_enabled(FlightOptions::ACRO_YAW_DAMPER)) {
 116         // use yaw controller
 117         rudder_output = plane.calc_nav_yaw_coordinated();
 118     } else {
 119         /*
 120           manual rudder
 121         */
 122         rudder_output = plane.rudder_input();
 123     }
 124
 125     output_rudder_and_steering(rudder_output);
 126
 127 }
 128
 129 /*
 130   quaternion based acro stabilization with continuous locking. Enabled with ACRO_LOCKING=2
 131  */
 132 void ModeAcro::stabilize_quaternion()
 133 {
 134     const float speed_scaler = plane.get_speed_scaler();
 135     auto &q = acro_state.q;
 136     const float rexpo = plane.roll_in_expo(true);
 137     const float pexpo = plane.pitch_in_expo(true);
 138     const float yexpo = plane.rudder_in_expo(true);
 139
 140     // get pilot desired rates
 141     float roll_rate = (rexpo/SERVO_MAX) * plane.g.acro_roll_rate;
 142     float pitch_rate = (pexpo/SERVO_MAX) * plane.g.acro_pitch_rate;
 143     float yaw_rate = (yexpo/SERVO_MAX) * plane.g.acro_yaw_rate;
 144     bool roll_active = !is_zero(roll_rate);
 145     bool pitch_active = !is_zero(pitch_rate);
 146     bool yaw_active = !is_zero(yaw_rate);
 147
 148     // integrate target attitude
 149     Vector3f r{ float(radians(roll_rate)), float(radians(pitch_rate)), float(radians(yaw_rate)) };
 150     r *= plane.G_Dt;
 151     q.rotate_fast(r);
 152     q.normalize();
 153
 154     // fill in target roll/pitch for GCS/logs
 155     plane.nav_roll_cd = degrees(q.get_euler_roll())*100;
 156     plane.nav_pitch_cd = degrees(q.get_euler_pitch())*100;
 157
 158     // get AHRS attitude
 159     Quaternion ahrs_q;
 160     IGNORE_RETURN(ahrs.get_quaternion(ahrs_q));
 161
 162     // zero target if not flying, no stick input and zero throttle
 163     if (is_zero(plane.get_throttle_input()) &&
 164         !plane.is_flying() &&
 165         is_zero(roll_rate) &&
 166         is_zero(pitch_rate) &&
 167         is_zero(yaw_rate)) {
 168         // cope with sitting on the ground with neutral sticks, no throttle
 169         q = ahrs_q;
 170     }
 171
 172     // get error in attitude
 173     Quaternion error_quat = ahrs_q.inverse() * q;
 174     Vector3f error_angle1;
 175     error_quat.to_axis_angle(error_angle1);
 176
 177     // don't let too much error build up, limit to 0.2s
 178     const float max_error_t = 0.2;
 179     float max_err_roll_rad  = radians(plane.g.acro_roll_rate*max_error_t);
 180     float max_err_pitch_rad = radians(plane.g.acro_pitch_rate*max_error_t);
 181     float max_err_yaw_rad   = radians(plane.g.acro_yaw_rate*max_error_t);
 182
 183     if (!roll_active && acro_state.roll_active_last) {
 184         max_err_roll_rad = 0;
 185     }
 186     if (!pitch_active && acro_state.pitch_active_last) {
 187         max_err_pitch_rad = 0;
 188     }
 189     if (!yaw_active && acro_state.yaw_active_last) {
 190         max_err_yaw_rad = 0;
 191     }
 192
 193     Vector3f desired_rates = error_angle1;
 194     desired_rates.x = constrain_float(desired_rates.x, -max_err_roll_rad, max_err_roll_rad);
 195     desired_rates.y = constrain_float(desired_rates.y, -max_err_pitch_rad, max_err_pitch_rad);
 196     desired_rates.z = constrain_float(desired_rates.z, -max_err_yaw_rad, max_err_yaw_rad);
 197
 198     // correct target based on max error
 199     q.rotate_fast(desired_rates - error_angle1);
 200     q.normalize();
 201
 202     // convert to desired body rates
 203     desired_rates.x /= plane.rollController.tau();
 204     desired_rates.y /= plane.pitchController.tau();
 205     desired_rates.z /= plane.pitchController.tau(); // no yaw tau parameter, use pitch
 206
 207     desired_rates *= degrees(1.0);
 208
 209     if (roll_active) {
 210         desired_rates.x = roll_rate;
 211     }
 212     if (pitch_active) {
 213         desired_rates.y = pitch_rate;
 214     }
 215     if (yaw_active) {
 216         desired_rates.z = yaw_rate;
 217     }
 218
 219     // call to rate controllers
 220     SRV_Channels::set_output_scaled(SRV_Channel::k_aileron,  plane.rollController.get_rate_out(desired_rates.x, speed_scaler));
 221     SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitchController.get_rate_out(desired_rates.y, speed_scaler));
 222     output_rudder_and_steering(plane.yawController.get_rate_out(desired_rates.z,  speed_scaler, false));
 223
 224     acro_state.roll_active_last = roll_active;
 225     acro_state.pitch_active_last = pitch_active;
 226     acro_state.yaw_active_last = yaw_active;
 227 }