Copter: add turtle mode

add turtle mode as an RC function
add turtle mode arming checks
re-use motortest arming checks for turtle mode

ArduCopter/Copter.h

@@ -220,6 +220,7 @@ public:
     friend class ModeThrow;
     friend class ModeZigZag;
     friend class ModeAutorotate;
+    friend class ModeTurtle;
 
     Copter(void);
 
@@ -816,7 +817,7 @@ private:
 
     // motor_test.cpp
     void motor_test_output();
-    bool mavlink_motor_test_check(const GCS_MAVLINK &gcs_chan, bool check_rc);
+    bool mavlink_motor_control_check(const GCS_MAVLINK &gcs_chan, bool check_rc, const char* mode);
     MAV_RESULT mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t motor_seq, uint8_t throttle_type, float throttle_value, float timeout_sec, uint8_t motor_count);
     void motor_test_stop();
 
@@ -982,6 +983,9 @@ private:
 #if MODE_AUTOROTATE_ENABLED == ENABLED
     ModeAutorotate mode_autorotate;
 #endif
+#if MODE_TURTLE_ENABLED == ENABLED
+    ModeTurtle mode_turtle;
+#endif
 
     // mode.cpp
     Mode *mode_from_mode_num(const Mode::Number mode);

ArduCopter/RC_Channel.cpp

@@ -89,6 +89,7 @@ void RC_Channel_Copter::init_aux_function(const aux_func_t ch_option, const AuxS
     case AUX_FUNC::ZIGZAG_SaveWP:
     case AUX_FUNC::ACRO:
     case AUX_FUNC::AUTO_RTL:
+    case AUX_FUNC::TURTLE:
         break;
     case AUX_FUNC::ACRO_TRAINER:
     case AUX_FUNC::ATTCON_ACCEL_LIM:
@@ -568,6 +569,11 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi
 #endif
             break;
 
+        case AUX_FUNC::TURTLE:
+#if MODE_TURTLE_ENABLED == ENABLED
+            do_aux_function_change_mode(Mode::Number::TURTLE, ch_flag);
+#endif
+            break;
 
     default:
         return RC_Channel::do_aux_function(ch_option, ch_flag);

ArduCopter/config.h

@@ -335,6 +335,12 @@
 # define MODE_ZIGZAG_ENABLED !HAL_MINIMIZE_FEATURES
 #endif
 
+//////////////////////////////////////////////////////////////////////////////
+// Turtle - allow vehicle to be flipped over after a crash
+#ifndef MODE_TURTLE_ENABLED
+# define MODE_TURTLE_ENABLED !HAL_MINIMIZE_FEATURES && !defined(DISABLE_DSHOT) && FRAME_CONFIG != HELI_FRAME
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 
 //////////////////////////////////////////////////////////////////////////////

ArduCopter/mode.cpp

@@ -169,6 +169,12 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode)
             break;
 #endif
 
+#if MODE_TURTLE_ENABLED == ENABLED
+        case Mode::Number::TURTLE:
+            ret = &mode_turtle;
+            break;
+#endif
+
         default:
             break;
     }

ArduCopter/mode.h

@@ -37,6 +37,7 @@ public:
         SYSTEMID  =    25,  // System ID mode produces automated system identification signals in the controllers
         AUTOROTATE =   26,  // Autonomous autorotation
         AUTO_RTL =     27,  // Auto RTL, this is not a true mode, AUTO will report as this mode if entered to perform a DO_LAND_START Landing sequence
+        TURTLE =       28,  // Flip over after crash
     };
 
     // constructor
@@ -1495,6 +1496,30 @@ private:
     float free_fall_start_velz;     // vertical velocity when free fall was detected
 };
 
+#if MODE_TURTLE_ENABLED == ENABLED
+class ModeTurtle : public Mode {
+
+public:
+    // inherit constructors
+    using Mode::Mode;
+    Number mode_number() const override { return Number::TURTLE; }
+
+    bool init(bool ignore_checks) override;
+    void run() override;
+    void exit() override;
+
+    bool requires_GPS() const override { return false; }
+    bool has_manual_throttle() const override { return true; }
+    bool allows_arming(AP_Arming::Method method) const override;
+    bool is_autopilot() const override { return false; }
+    void change_motor_direction(bool reverse);
+
+protected:
+    const char *name() const override { return "TURTLE"; }
+    const char *name4() const override { return "TRTL"; }
+};
+#endif
+
 // modes below rely on Guided mode so must be declared at the end (instead of in alphabetical order)
 
 class ModeAvoidADSB : public ModeGuided {

ArduCopter/mode_turtle.cpp

@@ -0,0 +1,165 @@
+#include "Copter.h"
+
+#if MODE_TURTLE_ENABLED == ENABLED
+
+#define CRASH_FLIP_EXPO 35.0f
+#define CRASH_FLIP_STICK_MINF 0.15f
+#define power3(x) ((x)*(x)*(x))
+
+bool ModeTurtle::init(bool ignore_checks)
+{
+    // do not enter the mode when already armed or when flying
+    if (motors->armed() || SRV_Channels::get_dshot_esc_type() == 0) {
+        return false;
+    }
+
+    // perform minimal arming checks
+    if (!copter.mavlink_motor_control_check(*gcs().chan(0), true, "Turtle Mode")) {
+        return false;
+    }
+
+    // do not enter the mode if sticks are not centered
+    if (!is_zero(channel_pitch->norm_input_dz())
+        || !is_zero(channel_roll->norm_input_dz())
+        || !is_zero(channel_yaw->norm_input_dz())) {
+            return false;
+    }
+    // reverse the motors
+    hal.rcout->disable_channel_mask_updates();
+    change_motor_direction(true);
+
+    // disable throttle and gps failsafe
+    g.failsafe_throttle = FS_THR_DISABLED;
+    g.failsafe_gcs = FS_GCS_DISABLED;
+    g.fs_ekf_action = 0;
+
+    // arm
+    motors->armed(true);
+    hal.util->set_soft_armed(true);
+
+    return true;
+}
+
+bool  ModeTurtle::allows_arming(AP_Arming::Method method) const
+{
+    return true;
+}
+
+void ModeTurtle::exit()
+{
+    // disarm
+    motors->armed(false);
+    hal.util->set_soft_armed(false);
+
+    // un-reverse the motors
+    change_motor_direction(false);
+    hal.rcout->enable_channel_mask_updates();
+
+    // re-enable failsafes
+    g.failsafe_throttle.load();
+    g.failsafe_gcs.load();
+    g.fs_ekf_action.load();
+}
+
+void ModeTurtle::change_motor_direction(bool reverse)
+{
+    AP_HAL::RCOutput::BLHeliDshotCommand direction = reverse ? AP_HAL::RCOutput::DSHOT_REVERSE : AP_HAL::RCOutput::DSHOT_NORMAL;
+    AP_HAL::RCOutput::BLHeliDshotCommand inverse_direction = reverse ? AP_HAL::RCOutput::DSHOT_NORMAL : AP_HAL::RCOutput::DSHOT_REVERSE;
+
+    if (!hal.rcout->get_reversed_mask()) {
+        hal.rcout->send_dshot_command(direction, AP_HAL::RCOutput::ALL_CHANNELS, 0, 10, true);
+    } else {
+        for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; ++i) {
+            if (!motors->is_motor_enabled(i)) {
+                continue;
+            }
+
+            if ((hal.rcout->get_reversed_mask() & (1U<<i)) == 0) {
+                hal.rcout->send_dshot_command(direction, i, 0, 10, true);
+            } else {
+                hal.rcout->send_dshot_command(inverse_direction, i, 0, 10, true);
+            }
+        }
+    }
+}
+
+void ModeTurtle::run()
+{
+    const float flipPowerFactor = 1.0f - CRASH_FLIP_EXPO / 100.0f;
+    const bool norc = copter.failsafe.radio || !copter.ap.rc_receiver_present;
+    const float stickDeflectionPitch =  norc ? 0.0f : channel_pitch->norm_input_dz();
+    const float stickDeflectionRoll = norc ? 0.0f : channel_roll->norm_input_dz();
+    const float stickDeflectionYaw = norc ? 0.0f : channel_yaw->norm_input_dz();
+
+    const float stickDeflectionPitchAbs =  fabsf(stickDeflectionPitch);
+    const float stickDeflectionRollAbs = fabsf(stickDeflectionRoll);
+    const float stickDeflectionYawAbs = fabsf(stickDeflectionYaw);
+
+    const float stickDeflectionPitchExpo = flipPowerFactor * stickDeflectionPitchAbs + power3(stickDeflectionPitchAbs) * (1 - flipPowerFactor);
+    const float stickDeflectionRollExpo = flipPowerFactor * stickDeflectionRollAbs + power3(stickDeflectionRollAbs) * (1 - flipPowerFactor);
+    const float stickDeflectionYawExpo = flipPowerFactor * stickDeflectionYawAbs + power3(stickDeflectionYawAbs) * (1 - flipPowerFactor);
+
+    float signPitch = stickDeflectionPitch < 0 ? -1 : 1;
+    float signRoll = stickDeflectionRoll < 0 ? 1 : -1;
+    //float signYaw = stickDeflectionYaw < 0 ? -1 : 1;
+
+    float stickDeflectionLength = sqrtf(sq(stickDeflectionPitchAbs) + sq(stickDeflectionRollAbs));
+    float stickDeflectionExpoLength = sqrtf(sq(stickDeflectionPitchExpo) + sq(stickDeflectionRollExpo));
+
+    if (stickDeflectionYawAbs > MAX(stickDeflectionPitchAbs, stickDeflectionRollAbs)) {
+        // If yaw is the dominant, disable pitch and roll
+        stickDeflectionLength = stickDeflectionYawAbs;
+        stickDeflectionExpoLength = stickDeflectionYawExpo;
+        signRoll = 0;
+        signPitch = 0;
+    } else {
+        // If pitch/roll dominant, disable yaw
+        //signYaw = 0;
+    }
+
+    const float cosPhi = (stickDeflectionLength > 0) ? (stickDeflectionPitchAbs + stickDeflectionRollAbs) / (sqrtf(2.0f) * stickDeflectionLength) : 0;
+    const float cosThreshold = sqrtf(3.0f)/2.0f; // cos(PI/6.0f)
+
+    if (cosPhi < cosThreshold) {
+        // Enforce either roll or pitch exclusively, if not on diagonal
+        if (stickDeflectionRollAbs > stickDeflectionPitchAbs) {
+            signPitch = 0;
+        } else {
+            signRoll = 0;
+        }
+    }
+
+    // Apply a reasonable amount of stick deadband
+    const float crashFlipStickMinExpo = flipPowerFactor * CRASH_FLIP_STICK_MINF + power3(CRASH_FLIP_STICK_MINF) * (1 - flipPowerFactor);
+    const float flipStickRange = 1.0f - crashFlipStickMinExpo;
+    const float flipPower = MAX(0.0f, stickDeflectionExpoLength - crashFlipStickMinExpo) / flipStickRange;
+
+    // at this point we have a power value in the range 0..1
+
+    // notmalise the roll and pitch input to match the motors
+    Vector2f input {signRoll, signPitch};
+    input = input.normalized() * 0.5;
+
+    // we bypass spin min and friends in the deadzone because we only want spin up when the sticks are moved
+    float motorOutput = !is_zero(flipPower) ? motors->thrust_to_actuator(flipPower) : 0.0f;
+
+    for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; ++i) {
+        if (!motors->is_motor_enabled(i)) {
+            continue;
+        }
+
+        const Vector2f output {motors->get_roll_factor(i), motors->get_pitch_factor(i)};
+        // if output aligns with input then use this motor
+        if ((input - output).length() > 0.5) {
+            motors->rc_write(i, motors->get_pwm_output_min());
+            continue;
+        }
+
+        int16_t pwm = motors->get_pwm_output_min()
+            + (motors->get_pwm_output_max() - motors->get_pwm_output_min()) * motorOutput;
+
+        motors->rc_write(i, pwm);
+    }
+}
+
+#endif

ArduCopter/motor_test.cpp

@@ -95,29 +95,29 @@ void Copter::motor_test_output()
 
 // mavlink_motor_test_check - perform checks before motor tests can begin
 //  return true if tests can continue, false if not
-bool Copter::mavlink_motor_test_check(const GCS_MAVLINK &gcs_chan, bool check_rc)
+bool Copter::mavlink_motor_control_check(const GCS_MAVLINK &gcs_chan, bool check_rc, const char* mode)
 {
     // check board has initialised
     if (!ap.initialised) {
-        gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"Motor Test: Board initialising");
+        gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: Board initialising", mode);
         return false;
     }
 
     // check rc has been calibrated
     if (check_rc && !arming.rc_calibration_checks(true)) {
-        gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"Motor Test: RC not calibrated");
+        gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: RC not calibrated", mode);
         return false;
     }
 
     // ensure we are landed
     if (!ap.land_complete) {
-        gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"Motor Test: vehicle not landed");
+        gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: vehicle not landed", mode);
         return false;
     }
 
     // check if safety switch has been pushed
     if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
-        gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"Motor Test: Safety switch");
+        gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: Safety switch", mode);
         return false;
     }
 
@@ -141,7 +141,7 @@ MAV_RESULT Copter::mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t
            The RC calibrated check can be skipped if direct pwm is
            supplied
         */
-        if (!mavlink_motor_test_check(gcs_chan, throttle_type != 1)) {
+        if (!mavlink_motor_control_check(gcs_chan, throttle_type != 1, "Motor Test")) {
             return MAV_RESULT_FAILED;
         } else {
             // start test