Line data    Source code

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