Copter: Add flight mode for throw launch

The is commit adds a new flight mode called 'Throw' to Copter that enables the copter to be thrown into the air to start motors. This mode can only be netered when the copters EKF has a valid position estimate and goes through the following states

Throw_Disarmed - The copter is disarmed and motors are off.
Throw_Detecting - The copter is armed, but motors will not spin unless THROW_MOT_START has been set to 1. The copter is waiting to detect the throw. A throw with an upwards velocity of at least 50cm/s is required to trigger the detector.
Throw_Uprighting - The throw has been detected and the copter is being uprighted with 50% throttle to maximise control authority. This state transitions when the copter is within 30 degrees of level.
Throw_HgtStabilise - The copter is kept level and height is stabilised about the target height which is 3m above the height at which the throw release was detected. This state transitions when the height is no more than 0.5m below the demanded height.
Throw_PosHold - The horizontal motion is arrested and the copter is kept at a constant position and height.

ArduCopter/Copter.h

@@ -375,6 +375,10 @@ private:
     // Flip
     Vector3f flip_orig_attitude;         // original copter attitude before flip
 
+    // Throw
+    bool throw_early_exit_interlock = true; // value of the throttle interlock that must be restored when exiting throw mode early
+    bool throw_flight_commenced = false;    // true when the throw has been detected and the motors and control loops are running
+
     // Battery Sensors
     AP_BattMonitor battery;
 
@@ -802,6 +806,13 @@ private:
     void poshold_roll_controller_to_pilot_override();
     void poshold_pitch_controller_to_pilot_override();
 
+    // Throw to launch functionality
+    bool throw_init(bool ignore_checks);
+    void throw_run();
+    bool throw_detected();
+    bool throw_attitude_good();
+    bool throw_height_good();
+
     bool rtl_init(bool ignore_checks);
     void rtl_run();
     void rtl_climb_start();

ArduCopter/Parameters.cpp

@@ -1105,6 +1105,13 @@ const AP_Param::Info Copter::var_info[] = {
     // @Path: ../libraries/AP_Notify/AP_Notify.cpp
     GOBJECT(notify, "NTF_",  AP_Notify),
 
+    // @Param: THROW_MOT_START
+    // @DisplayName: Start motors before throwing is detected
+    // @Description: Used by THROW mode. Controls whether motors will run at the speed set by THR_MIN or will be stopped when armed and waiting for the throw.
+    // @Values: 0:Stopped,1:Running
+    // @User: Standard
+    GSCALAR(throw_motor_start, "THROW_MOT_START", 0),
+
     AP_VAREND
 };
 

ArduCopter/Parameters.h

@@ -180,6 +180,7 @@ public:
         k_param_motors = 90,
         k_param_disarm_delay,
         k_param_fs_crash_check,
+        k_param_throw_motor_start,
 
         // 97: RSSI
         k_param_rssi = 97,
@@ -450,6 +451,8 @@ public:
     AP_Float        fs_ekf_thresh;
     AP_Int16        gcs_pid_mask;
 
+    AP_Int8         throw_motor_start;
+
 #if FRAME_CONFIG ==     HELI_FRAME
     // Heli
     RC_Channel      heli_servo_1, heli_servo_2, heli_servo_3, heli_servo_4;     // servos for swash plate and tail

ArduCopter/control_throw.cpp

@@ -0,0 +1,222 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include "Copter.h"
+
+
+// throw_init - initialise throw controller
+bool Copter::throw_init(bool ignore_checks)
+{
+#if FRAME_CONFIG == HELI_FRAME
+    // do not allow helis to use throw to start
+    return false;
+#endif
+    // this mode needs a position reference
+    if (position_ok()) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+// runs the throw to start controller
+// should be called at 100hz or more
+void Copter::throw_run()
+{
+    static ThrowModeState throw_state = Throw_Disarmed;
+
+    /* Throw State Machine
+    Throw_Disarmed - motors are off
+    Throw_Detecting -  motors are on and we are waiting for the throw
+    Throw_Uprighting - the throw has been detected and the copter is being uprighted
+    Throw_HgtStabilise - the copter is kept level and  height is stabilised about the target height
+    Throw_PosHold - the copter is kept at a constant position and height
+    */
+
+    // Don't enter THROW mode if interlock will prevent motors running
+    if (!motors.armed() && motors.get_interlock()) {
+        // state machine entry is always from a disarmed state
+        throw_state = Throw_Disarmed;
+
+        // remember the current value of the motor interlock so that this condition can be restored if we exit the throw mode before starting motors
+        throw_early_exit_interlock = true;
+
+        // prevent motors from rotating before the throw is detected unless enabled by the user
+        if (g.throw_motor_start == 1) {
+            motors.set_interlock(true);
+        } else {
+            motors.set_interlock(false);
+        }
+
+        // status to let system know flight control has not started which means the interlock setting needs to restored if we exit to another flight mode
+        // this is necessary because throw mode uses the interlock to achieve a post arm motor start.
+        throw_flight_commenced = false;
+
+    } else if (throw_state == Throw_Disarmed && motors.armed()) {
+        gcs_send_text(MAV_SEVERITY_INFO,"waiting for throw");
+        throw_state = Throw_Detecting;
+
+        // prevent motors from rotating before the throw is detected unless enabled by the user
+        if (g.throw_motor_start == 1) {
+            motors.set_interlock(true);
+        } else {
+            motors.set_interlock(false);
+        }
+
+    } else if (throw_state == Throw_Detecting && throw_detected()){
+        gcs_send_text(MAV_SEVERITY_INFO,"throw detected - uprighting");
+        throw_state = Throw_Uprighting;
+
+        // Cancel the waiting for throw tone sequence
+        AP_Notify::flags.waiting_for_throw = false;
+
+        // reset the interlock
+        motors.set_interlock(true);
+
+        // status to let system know flight control has started which means the entry interlock setting will not restored if we exit to another flight mode
+        throw_flight_commenced = true;
+
+    } else if (throw_state == Throw_Uprighting && throw_attitude_good()) {
+        gcs_send_text(MAV_SEVERITY_INFO,"uprighted - controlling height");
+        throw_state = Throw_HgtStabilise;
+
+        // initialize vertical speed and acceleration limits
+        // use brake mode values for rapid response
+        pos_control.set_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
+        pos_control.set_accel_z(BRAKE_MODE_DECEL_RATE);
+
+        // initialise the demanded height to 3m above the throw height
+        // we want to rapidly clear surrounding obstacles
+        pos_control.set_alt_target(inertial_nav.get_altitude() + 300);
+
+        // set the initial velocity of the height controller demand to the measured velocity if it is going up
+        // if it is going down, set it to zero to enforce a very hard stop
+        pos_control.set_desired_velocity_z(fmaxf(inertial_nav.get_velocity_z(),0.0f));
+
+        // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
+        set_auto_armed(true);
+
+    } else if (throw_state == Throw_HgtStabilise && throw_height_good()) {
+        gcs_send_text(MAV_SEVERITY_INFO,"height achieved - controlling position");
+        throw_state = Throw_PosHold;
+
+        // initialise the loiter target to the curent position and velocity
+        wp_nav.init_loiter_target();
+
+        // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
+        set_auto_armed(true);
+    }
+
+    // Throw State Processing
+    switch (throw_state) {
+
+    case Throw_Disarmed:
+
+        // demand zero throttle (motors will be stopped anyway) and continually reset the attitude controller
+        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
+        motors.slow_start(true);
+
+        break;
+
+    case Throw_Detecting:
+
+        // Hold throttle at zero during the throw and continually reset the attitude controller
+        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
+
+        // Play the waiting for throw tone sequence to alert the user
+        AP_Notify::flags.waiting_for_throw = true;
+
+        break;
+
+    case Throw_Uprighting:
+
+        // demand a level roll/pitch attitude with zero yaw rate
+        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
+
+        // output 50% throttle and turn off angle boost to maximise righting moment
+        attitude_control.set_throttle_out(500, false, g.throttle_filt);
+
+        break;
+
+    case Throw_HgtStabilise:
+
+        // call attitude controller
+        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
+
+        // call height controller
+        pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
+        pos_control.update_z_controller();
+
+        break;
+
+    case Throw_PosHold:
+
+        // run loiter controller
+        wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
+
+        // call attitude controller
+        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f);
+
+        // call height controller
+        pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
+        pos_control.update_z_controller();
+
+        break;
+    }
+}
+
+bool Copter::throw_detected()
+{
+    // Check that we have a valid navigation solution
+    nav_filter_status filt_status = inertial_nav.get_filter_status();
+    if (!filt_status.flags.attitude || !filt_status.flags.horiz_pos_abs || !filt_status.flags.vert_pos) {
+        return false;
+    }
+
+    // Check for high speed (note get_inertial_nav methods use a cm length scale)
+    bool high_speed = inertial_nav.get_velocity().length() > 500.0f;
+
+    // check for upwards trajectory
+    bool gaining_height = inertial_nav.get_velocity().z > 50.0f;
+
+    // Check the vertical acceleraton is greater than 0.25g
+    bool free_falling = ahrs.get_accel_ef().z > -0.25 * GRAVITY_MSS;
+
+    // Check if the accel length is < 1.0g indicating that any throw action is complete and the copter has been released
+    bool no_throw_action = ins.get_accel().length() < 1.0f * GRAVITY_MSS;
+
+    // High velocity or free-fall combined with incresing height indicate a possible throw release
+    bool possible_throw_detected = (free_falling || high_speed) && gaining_height && no_throw_action;
+
+    // Record time and vertical velocity when we detect the possible throw
+    static uint32_t free_fall_start_msec = 0;
+    static float free_fall_start_velz;
+    if (possible_throw_detected && ((AP_HAL::millis() - free_fall_start_msec) > 500)) {
+        free_fall_start_msec = AP_HAL::millis();
+        free_fall_start_velz = inertial_nav.get_velocity().z;
+    }
+
+    // Once a possible throw condition has been detected, we check for 2.5 m/s of downwards velocity change in less than 0.5 seconds to confirm
+    bool throw_condition_confirmed = ((AP_HAL::millis() - free_fall_start_msec < 500) && ((inertial_nav.get_velocity().z - free_fall_start_velz) < -250.0f));
+
+    // start motors and enter the control mode if we are in continuous freefall
+    if (throw_condition_confirmed) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+bool Copter::throw_attitude_good()
+{
+    // Check that we have uprighted the copter
+    const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned();
+    bool is_upright = (rotMat.c.z > 0.866f);
+    return is_upright;
+}
+
+bool Copter::throw_height_good()
+{
+    // Check that we are no more than 0.5m below the demanded height
+    return (pos_control.get_alt_error() < 50.0f);
+}
+

ArduCopter/defines.h

@@ -103,7 +103,8 @@ enum autopilot_modes {
     FLIP =         14,  // automatically flip the vehicle on the roll axis
     AUTOTUNE =     15,  // automatically tune the vehicle's roll and pitch gains
     POSHOLD =      16,  // automatic position hold with manual override, with automatic throttle
-    BRAKE =        17   // full-brake using inertial/GPS system, no pilot input
+    BRAKE =        17,  // full-brake using inertial/GPS system, no pilot input
+    THROW =        18   // throw to launch mode using inertial/GPS system, no pilot input
 };
 
 // Tuning enumeration
@@ -227,6 +228,15 @@ enum FlipState {
     Flip_Abandon
 };
 
+// Throw states
+enum ThrowModeState {
+    Throw_Disarmed,
+    Throw_Detecting,
+    Throw_Uprighting,
+    Throw_HgtStabilise,
+    Throw_PosHold
+};
+
 // LAND state
 #define LAND_STATE_FLY_TO_LOCATION  0
 #define LAND_STATE_DESCENDING       1

ArduCopter/flight_mode.cpp

@@ -22,6 +22,11 @@ bool Copter::set_mode(uint8_t mode)
         return true;
     }
 
+    // If exiting throw mode before commencing flight, restore the throttle interlock to the value last set by the switch
+    if ((mode == THROW) && (control_mode != THROW) && !throw_flight_commenced) {
+        motors.set_interlock(throw_early_exit_interlock);
+    }
+
     switch(mode) {
         case ACRO:
             #if FRAME_CONFIG == HELI_FRAME
@@ -95,6 +100,10 @@ bool Copter::set_mode(uint8_t mode)
             success = brake_init(ignore_checks);
             break;
 
+        case THROW:
+            success = throw_init(ignore_checks);
+            break;
+
         default:
             success = false;
             break;
@@ -206,6 +215,10 @@ void Copter::update_flight_mode()
         case BRAKE:
             brake_run();
             break;
+
+        case THROW:
+            throw_run();
+            break;
     }
 }
 
@@ -271,6 +284,7 @@ bool Copter::mode_requires_GPS(uint8_t mode) {
         case DRIFT:
         case POSHOLD:
         case BRAKE:
+        case THROW:
             return true;
         default:
             return false;
@@ -295,7 +309,7 @@ bool Copter::mode_has_manual_throttle(uint8_t mode) {
 // mode_allows_arming - returns true if vehicle can be armed in the specified mode
 //  arming_from_gcs should be set to true if the arming request comes from the ground station
 bool Copter::mode_allows_arming(uint8_t mode, bool arming_from_gcs) {
-    if (mode_has_manual_throttle(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || mode == DRIFT || mode == SPORT || (arming_from_gcs && mode == GUIDED)) {
+    if (mode_has_manual_throttle(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || mode == DRIFT || mode == SPORT || mode == THROW || (arming_from_gcs && mode == GUIDED)) {
         return true;
     }
     return false;
@@ -370,6 +384,9 @@ void Copter::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
     case BRAKE:
         port->print("BRAKE");
         break;
+    case THROW:
+        port->print("THROW");
+        break;
     default:
         port->printf("Mode(%u)", (unsigned)mode);
         break;

ArduCopter/motors.cpp

@@ -77,7 +77,7 @@ void Copter::auto_disarm_check()
 
     // exit immediately if we are already disarmed, or if auto
     // disarming is disabled
-    if (!motors.armed() || disarm_delay_ms == 0) {
+    if (!motors.armed() || disarm_delay_ms == 0 || control_mode == THROW) {
         auto_disarm_begin = tnow_ms;
         return;
     }

ArduCopter/switches.cpp

@@ -567,6 +567,9 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
             // control signal in tradheli
             motors.set_interlock(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE);
 
+            // remember the current value of the motor interlock so that this condition can be restored if we exit the throw mode early
+            throw_early_exit_interlock = motors.get_interlock();
+
             // Log new status
             if (motors.get_interlock()){
                 Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED);

ArduCopter/system.cpp

@@ -407,7 +407,8 @@ void Copter::update_auto_armed()
         }
 #else
         // if motors are armed and throttle is above zero auto_armed should be true
-        if(motors.armed() && !ap.throttle_zero) {
+        // if motors are armed and we are in throw mode, then auto_ermed should be true
+        if(motors.armed() && (!ap.throttle_zero || control_mode == THROW)) {
             set_auto_armed(true);
         }
 #endif // HELI_FRAME