Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : #if MODE_DRIFT_ENABLED
       4             : 
       5             : /*
       6             :  * Drift flight mode — meters/second and radians version
       7             :  */
       8             : //   Converted: 8 [cd/(cm/s)] * (π/18000 rad/cd) * (100 cm/m) = 0.13962634 rad/(m/s)
       9             : #ifndef DRIFT_SPEEDGAIN_RAD
      10             :  # define DRIFT_SPEEDGAIN_RAD 0.13962634f
      11             : #endif
      12             : #ifdef DRIFT_SPEEDGAIN
      13             :     #error please convert to radians and use DRIFT_SPEEDGAIN_RAD
      14             : #endif
      15             : 
      16             : #ifndef DRIFT_SPEEDLIMIT_MS
      17             :  # define DRIFT_SPEEDLIMIT_MS 5.60f
      18             : #endif
      19             : #ifdef DRIFT_SPEEDLIMIT
      20             :     #error please convert to meters per second and use DRIFT_SPEEDLIMIT_MS
      21             : #endif
      22             : 
      23             : #ifndef DRIFT_VEL_FORWARD_MIN_MS
      24             :  # define DRIFT_VEL_FORWARD_MIN_MS 20.0f
      25             : #endif
      26             : #ifdef DRIFT_VEL_FORWARD_MIN
      27             :     #error please convert to meters per second and use DRIFT_VEL_FORWARD_MIN_MS
      28             : #endif
      29             : 
      30             : #ifndef DRIFT_THR_ASSIST_GAIN_MS
      31             :  # define DRIFT_THR_ASSIST_GAIN_MS 0.18f
      32             : #endif
      33             : #ifdef DRIFT_THR_ASSIST_GAIN
      34             :     #error please convert to meters per second and use DRIFT_THR_ASSIST_GAIN_MS
      35             : #endif
      36             : 
      37             : #ifndef DRIFT_THR_ASSIST_MAX
      38             :  # define DRIFT_THR_ASSIST_MAX  0.3f      // maximum assistance throttle assist will provide
      39             : #endif
      40             : 
      41             : #ifndef DRIFT_THR_MIN
      42             :  # define DRIFT_THR_MIN         0.213f
      43             : #endif
      44             : #ifndef DRIFT_THR_MAX
      45             :  # define DRIFT_THR_MAX         0.787f
      46             : #endif
      47             : 
      48             : // drift_init - initialise drift controller
      49           2 : bool ModeDrift::init(bool ignore_checks)
      50             : {
      51           2 :     return true;
      52             : }
      53             : 
      54             : // drift_run - runs the drift controller
      55             : // should be called at 100hz or more
      56       37839 : void ModeDrift::run()
      57             : {
      58             :     static float braker = 0.0f;
      59             :     static float roll_input_rad = 0.0f;
      60             : 
      61             :     // convert pilot input to lean angles (already radians)
      62             :     float target_roll_rad, target_pitch_rad;
      63       37839 :     get_pilot_desired_lean_angles_rad(target_roll_rad, target_pitch_rad, attitude_control->lean_angle_max_rad(), attitude_control->get_althold_lean_angle_max_rad());
      64             : 
      65             :     // Grab inertial velocity (already m/s)
      66       37839 :     const Vector3f& vel_ned_ms = pos_control->get_vel_estimate_NED_ms();
      67             : 
      68             :     // rotate roll, pitch input from north facing to vehicle's perspective
      69             :     // body-frame components in m/s
      70       37839 :     float vel_right_ms   =  vel_ned_ms.y * ahrs.cos_yaw() - vel_ned_ms.x * ahrs.sin_yaw(); // body roll axis
      71       37839 :     float vel_forward_ms =  vel_ned_ms.y * ahrs.sin_yaw() + vel_ned_ms.x * ahrs.cos_yaw(); // body pitch axis
      72             : 
      73             :     // gain scheduling for yaw
      74       37839 :     const float vel_forward_2_ms = MIN(fabsf(vel_forward_ms), DRIFT_VEL_FORWARD_MIN_MS);
      75             : 
      76             :     // yaw-rate schedule:
      77       37839 :     const float yaw_rate_max_rads = radians(g2.command_model_acro_y.get_rate());
      78       37839 :     const float target_yaw_rate_rads = (target_roll_rad / radians(45.0f)) * yaw_rate_max_rads * (1.0f - (vel_forward_2_ms / 50.0f));
      79             : 
      80             :     // Constrain body velocities
      81       37839 :     vel_right_ms   = constrain_float(vel_right_ms,   -DRIFT_SPEEDLIMIT_MS, DRIFT_SPEEDLIMIT_MS);
      82       37839 :     vel_forward_ms = constrain_float(vel_forward_ms, -DRIFT_SPEEDLIMIT_MS, DRIFT_SPEEDLIMIT_MS);
      83             : 
      84             :     // roll_input from yaw stick (convert centidegrees -> radians before LP)
      85             :     // (channel_yaw->get_control_in() returns centidegrees)
      86       37839 :     const float yaw_stick_rad = cd_to_rad((float)channel_yaw->get_control_in());
      87       37839 :     roll_input_rad = roll_input_rad * 0.96f + yaw_stick_rad * 0.04f;
      88             : 
      89             :     // convert user input into desired roll velocity term (m/s equivalent)
      90       37839 :     float roll_vel_error_ms = vel_right_ms - (roll_input_rad / DRIFT_SPEEDGAIN_RAD);
      91             : 
      92             :     // roll velocity is fed into roll angle to minimize slip
      93       37839 :     target_roll_rad = roll_vel_error_ms * -DRIFT_SPEEDGAIN_RAD;
      94             : 
      95             :     // constrain to ±45 deg
      96       37839 :     target_roll_rad = constrain_float(target_roll_rad, -radians(45.0f), radians(45.0f));
      97             : 
      98             :     // If we let go of sticks, bring us to a stop
      99       37839 :     if (is_zero(target_pitch_rad)) {
     100             :         // Clamp to the same coupling constant, now in rad/(m/s)
     101       37839 :         braker += 0.03f;
     102       37839 :         braker = MIN(braker, DRIFT_SPEEDGAIN_RAD);
     103       37839 :         target_pitch_rad = vel_forward_ms * braker;
     104             :     } else {
     105           0 :         braker = 0.0f;
     106             :     }
     107             : 
     108             :     // Determine desired spool state based on pilot throttle input.
     109             :     // The setter enforces that disarmed aircraft are held at SHUT_DOWN until armed.
     110       37839 :     if (copter.ap.throttle_zero) {
     111       37839 :         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
     112             :     } else {
     113           0 :         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     114             :     }
     115             : 
     116       37839 :     switch (motors->get_spool_state()) {
     117       37041 :     case AP_Motors::SpoolState::SHUT_DOWN:
     118       37041 :         attitude_control->reset_yaw_target_and_rate(false);
     119       37041 :         attitude_control->reset_rate_controller_I_terms();
     120       37041 :         break;
     121             : 
     122         798 :     case AP_Motors::SpoolState::GROUND_IDLE:
     123         798 :         attitude_control->reset_yaw_target_and_rate();
     124         798 :         attitude_control->reset_rate_controller_I_terms_smoothly();
     125         798 :         break;
     126             : 
     127           0 :     case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
     128           0 :         if (!motors->limit.throttle_lower) {
     129           0 :             set_land_complete(false);
     130             :         }
     131           0 :         break;
     132             : 
     133           0 :     case AP_Motors::SpoolState::SPOOLING_UP:
     134             :     case AP_Motors::SpoolState::SPOOLING_DOWN:
     135           0 :         break;
     136             :     }
     137             : 
     138             :     // call attitude controller (already expects radians)
     139       37839 :     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(
     140             :         target_roll_rad, target_pitch_rad, target_yaw_rate_rads);
     141             : 
     142             :     // output pilot's throttle with angle boost (velz now m/s)
     143       37839 :     const float assisted_throttle = get_throttle_assist(vel_ned_ms.z, get_pilot_desired_throttle());
     144       37839 :     attitude_control->set_throttle_out(assisted_throttle, true, g.throttle_filt);
     145       37839 : }
     146             : 
     147             : // get_throttle_assist - return throttle output (range 0 ~ 1) based on pilot input and D-axis velocity (positive down)
     148       37839 : float ModeDrift::get_throttle_assist(float vel_d_ms, float pilot_throttle_scaled)
     149             : {
     150             :     // throttle assist - adjusts throttle to slow the vehicle's vertical velocity
     151             :     //      Only active when pilot's throttle is between 0.213 ~ 0.787
     152             :     //      Assistance is strongest at mid, drops linearly to no assistance at 0.213 and 0.787
     153       37839 :     float thr_assist = 0.0f;
     154       37839 :     if (pilot_throttle_scaled > DRIFT_THR_MIN && pilot_throttle_scaled < DRIFT_THR_MAX) {
     155             :         // calculate throttle assist gain
     156           0 :         thr_assist = 1.2f - ((float)fabsf(pilot_throttle_scaled - 0.5f) / 0.24f);
     157           0 :         thr_assist = constrain_float(thr_assist, 0.0f, 1.0f) * DRIFT_THR_ASSIST_GAIN_MS * vel_d_ms;
     158             : 
     159             :         // ensure throttle assist never adjusts the throttle by more than 0.3 (≈300 pwm)
     160           0 :         thr_assist = constrain_float(thr_assist, -DRIFT_THR_ASSIST_MAX, DRIFT_THR_ASSIST_MAX);
     161             :     }
     162             : 
     163       37839 :     return constrain_float(pilot_throttle_scaled + thr_assist, 0.0f, 1.0f);
     164             : }
     165             : #endif