ArduCopter/mode_turtle.cpp

   1 #include "Copter.h"
   2
   3 #if MODE_TURTLE_ENABLED
   4
   5 #define CRASH_FLIP_EXPO 35.0f
   6 #define CRASH_FLIP_STICK_MINF 0.15f
   7 #define power3(x) ((x) * (x) * (x))
   8
   9 bool ModeTurtle::init(bool ignore_checks)
  10 {
  11     // do not enter the mode when already armed or when flying
  12     if (motors->armed() || SRV_Channels::get_dshot_esc_type() == 0) {
  13         return false;
  14     }
  15
  16     // perform minimal arming checks
  17     if (!copter.mavlink_motor_control_check(*gcs().chan(0), true, "Turtle Mode")) {
  18         return false;
  19     }
  20
  21     // do not enter the mode if sticks are not centered or throttle is not at zero
  22     if (!is_zero(channel_pitch->norm_input_dz())
  23         || !is_zero(channel_roll->norm_input_dz())
  24         || !is_zero(channel_yaw->norm_input_dz())
  25         || !is_zero(channel_throttle->norm_input_dz())) {
  26         return false;
  27     }
  28
  29     // turn on notify leds
  30     AP_Notify::flags.esc_calibration = true;
  31
  32     return true;
  33 }
  34
  35 void ModeTurtle::arm_motors()
  36 {
  37     if (hal.util->get_soft_armed()) {
  38         return;
  39     }
  40
  41     // stop the spoolup block activating
  42     motors->set_spoolup_block(false);
  43
  44     // reverse the motors
  45     hal.rcout->disable_channel_mask_updates();
  46     change_motor_direction(true);
  47
  48     // disable throttle and gps failsafe
  49     g.failsafe_throttle.set(FS_THR_DISABLED);
  50     g.failsafe_gcs.set(FS_GCS_DISABLED);
  51     g.fs_ekf_action.set(0);
  52
  53     // arm
  54     motors->armed(true);
  55     hal.util->set_soft_armed(true);
  56 }
  57
  58 bool ModeTurtle::allows_arming(AP_Arming::Method method) const
  59 {
  60     return true;
  61 }
  62
  63 void ModeTurtle::exit()
  64 {
  65     disarm_motors();
  66
  67     // turn off notify leds
  68     AP_Notify::flags.esc_calibration = false;
  69 }
  70
  71 void ModeTurtle::disarm_motors()
  72 {
  73     if (!hal.util->get_soft_armed()) {
  74         return;
  75     }
  76
  77     // disarm
  78     motors->armed(false);
  79     hal.util->set_soft_armed(false);
  80
  81     // un-reverse the motors
  82     change_motor_direction(false);
  83     hal.rcout->enable_channel_mask_updates();
  84
  85     // re-enable failsafes
  86     g.failsafe_throttle.load();
  87     g.failsafe_gcs.load();
  88     g.fs_ekf_action.load();
  89 }
  90
  91 void ModeTurtle::change_motor_direction(bool reverse)
  92 {
  93     AP_HAL::RCOutput::BLHeliDshotCommand direction = reverse ? AP_HAL::RCOutput::DSHOT_REVERSE : AP_HAL::RCOutput::DSHOT_NORMAL;
  94     AP_HAL::RCOutput::BLHeliDshotCommand inverse_direction = reverse ? AP_HAL::RCOutput::DSHOT_NORMAL : AP_HAL::RCOutput::DSHOT_REVERSE;
  95
  96     if (!hal.rcout->get_reversed_mask()) {
  97         hal.rcout->send_dshot_command(direction, AP_HAL::RCOutput::ALL_CHANNELS, 0, 10, true);
  98     } else {
  99         for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; ++i) {
 100             if (!motors->is_motor_enabled(i)) {
 101                 continue;
 102             }
 103
 104             if ((hal.rcout->get_reversed_mask() & (1U << i)) == 0) {
 105                 hal.rcout->send_dshot_command(direction, i, 0, 10, true);
 106             } else {
 107                 hal.rcout->send_dshot_command(inverse_direction, i, 0, 10, true);
 108             }
 109         }
 110     }
 111 }
 112
 113 void ModeTurtle::run()
 114 {
 115     const float flip_power_factor = 1.0f - CRASH_FLIP_EXPO * 0.01f;
 116     const bool norc = copter.failsafe.radio || !rc().has_ever_seen_rc_input();
 117     const float stick_deflection_pitch = norc ? 0.0f : channel_pitch->norm_input_dz();
 118     const float stick_deflection_roll = norc ? 0.0f : channel_roll->norm_input_dz();
 119     const float stick_deflection_yaw = norc ? 0.0f : channel_yaw->norm_input_dz();
 120
 121     const float stick_deflection_pitch_abs = fabsf(stick_deflection_pitch);
 122     const float stick_deflection_roll_abs = fabsf(stick_deflection_roll);
 123     const float stick_deflection_yaw_abs = fabsf(stick_deflection_yaw);
 124
 125     const float stick_deflection_pitch_expo = flip_power_factor * stick_deflection_pitch_abs + power3(stick_deflection_pitch_abs) * (1 - flip_power_factor);
 126     const float stick_deflection_roll_expo = flip_power_factor * stick_deflection_roll_abs + power3(stick_deflection_roll_abs) * (1 - flip_power_factor);
 127     const float stick_deflection_yaw_expo = flip_power_factor * stick_deflection_yaw_abs + power3(stick_deflection_yaw_abs) * (1 - flip_power_factor);
 128
 129     float sign_pitch = stick_deflection_pitch < 0 ? -1 : 1;
 130     float sign_roll = stick_deflection_roll < 0 ? 1 : -1;
 131
 132     float stick_deflection_length = sqrtf(sq(stick_deflection_pitch_abs) + sq(stick_deflection_roll_abs));
 133     float stick_deflection_expo_length = sqrtf(sq(stick_deflection_pitch_expo) + sq(stick_deflection_roll_expo));
 134
 135     if (stick_deflection_yaw_abs > MAX(stick_deflection_pitch_abs, stick_deflection_roll_abs)) {
 136         // If yaw is the dominant, disable pitch and roll
 137         stick_deflection_length = stick_deflection_yaw_abs;
 138         stick_deflection_expo_length = stick_deflection_yaw_expo;
 139         sign_roll = 0;
 140         sign_pitch = 0;
 141     }
 142
 143     const float cos_phi = (stick_deflection_length > 0) ? (stick_deflection_pitch_abs + stick_deflection_roll_abs) / (sqrtf(2.0f) * stick_deflection_length) : 0;
 144     const float cos_threshold = sqrtf(3.0f) / 2.0f; // cos(PI/6.0f)
 145
 146     if (cos_phi < cos_threshold) {
 147         // Enforce either roll or pitch exclusively, if not on diagonal
 148         if (stick_deflection_roll_abs > stick_deflection_pitch_abs) {
 149             sign_pitch = 0;
 150         } else {
 151             sign_roll = 0;
 152         }
 153     }
 154
 155     // Apply a reasonable amount of stick deadband
 156     const float crash_flip_stick_min_expo = flip_power_factor * CRASH_FLIP_STICK_MINF + power3(CRASH_FLIP_STICK_MINF) * (1 - flip_power_factor);
 157     const float flip_stick_range = 1.0f - crash_flip_stick_min_expo;
 158     const float flip_power = MAX(0.0f, stick_deflection_expo_length - crash_flip_stick_min_expo) / flip_stick_range;
 159
 160     // at this point we have a power value in the range 0..1
 161
 162     // normalise the roll and pitch input to match the motors
 163     Vector2f input{sign_roll, sign_pitch};
 164     motors_input = input.normalized() * 0.5;
 165     // we bypass spin min and friends in the deadzone because we only want spin up when the sticks are moved
 166     motors_output = !is_zero(flip_power) ? motors->thr_lin.thrust_to_actuator(flip_power) : 0.0f;
 167 }
 168
 169 // actually write values to the motors
 170 void ModeTurtle::output_to_motors()
 171 {
 172     // throttle needs to be raised
 173     if (is_zero(channel_throttle->norm_input_dz())) {
 174         const uint32_t now = AP_HAL::millis();
 175         if (now - last_throttle_warning_output_ms > 5000) {
 176             gcs().send_text(MAV_SEVERITY_WARNING, "Turtle: raise throttle to arm");
 177             last_throttle_warning_output_ms = now;
 178         }
 179
 180         disarm_motors();
 181         return;
 182     }
 183
 184     arm_motors();
 185
 186     // check if motor are allowed to spin
 187     const bool allow_output = motors->armed() && motors->get_interlock();
 188
 189     for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; ++i) {
 190         if (!motors->is_motor_enabled(i)) {
 191             continue;
 192         }
 193
 194         const Vector2f output{motors->get_roll_factor(i), motors->get_pitch_factor(i)};
 195         // if output aligns with input then use this motor
 196         if (!allow_output || (motors_input - output).length() > 0.5) {
 197             motors->rc_write(i, motors->get_pwm_output_min());
 198             continue;
 199         }
 200
 201         int16_t pwm = motors->get_pwm_output_min() + (motors->get_pwm_output_max() - motors->get_pwm_output_min()) * motors_output;
 202
 203         motors->rc_write(i, pwm);
 204     }
 205 }
 206
 207 #endif