Line data    Source code

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