diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 7293c74599..32d703e408 100644 --- a/ArduCopter/Copter.h +++ b/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(); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 14fa7fb9e3..0d954abce1 100644 --- a/ArduCopter/Parameters.cpp +++ b/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 }; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index cf299cff23..7e13ce2f95 100644 --- a/ArduCopter/Parameters.h +++ b/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 diff --git a/ArduCopter/control_throw.cpp b/ArduCopter/control_throw.cpp new file mode 100644 index 0000000000..609109c44e --- /dev/null +++ b/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); +} + diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 84b0819151..ae35932269 100644 --- a/ArduCopter/defines.h +++ b/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 diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index ad91d1c053..f4cb61cf92 100644 --- a/ArduCopter/flight_mode.cpp +++ b/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; diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 4f1cfb3a11..d997ca3e0b 100644 --- a/ArduCopter/motors.cpp +++ b/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; } diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index 6d7a3e00ea..4c25f2c5f1 100644 --- a/ArduCopter/switches.cpp +++ b/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); diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index d5e617fea2..851ab28bba 100644 --- a/ArduCopter/system.cpp +++ b/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