diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 8b62917404..2a8a843c7e 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -274,6 +274,11 @@ private: uint32_t start_ms; } takeoff_state; + // throw mode state + struct { + ThrowModeStage stage; + } throw_state = {Throw_Disarmed}; + uint32_t precland_last_update_ms; // altitude below which we do no navigation in auto takeoff diff --git a/ArduCopter/control_throw.cpp b/ArduCopter/control_throw.cpp index 6b950f29cb..f3faa848b2 100644 --- a/ArduCopter/control_throw.cpp +++ b/ArduCopter/control_throw.cpp @@ -33,8 +33,6 @@ void Copter::throw_exit() // 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 @@ -46,7 +44,7 @@ void Copter::throw_run() // 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; + throw_state.stage = 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; @@ -62,9 +60,9 @@ void Copter::throw_run() // 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()) { + } else if (throw_state.stage == Throw_Disarmed && motors.armed()) { gcs_send_text(MAV_SEVERITY_INFO,"waiting for throw"); - throw_state = Throw_Detecting; + throw_state.stage = Throw_Detecting; // prevent motors from rotating before the throw is detected unless enabled by the user if (g.throw_motor_start == 1) { @@ -73,9 +71,9 @@ void Copter::throw_run() motors.set_interlock(false); } - } else if (throw_state == Throw_Detecting && throw_detected()){ + } else if (throw_state.stage == Throw_Detecting && throw_detected()){ gcs_send_text(MAV_SEVERITY_INFO,"throw detected - uprighting"); - throw_state = Throw_Uprighting; + throw_state.stage = Throw_Uprighting; // Cancel the waiting for throw tone sequence AP_Notify::flags.waiting_for_throw = false; @@ -86,9 +84,9 @@ void Copter::throw_run() // 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()) { + } else if (throw_state.stage == Throw_Uprighting && throw_attitude_good()) { gcs_send_text(MAV_SEVERITY_INFO,"uprighted - controlling height"); - throw_state = Throw_HgtStabilise; + throw_state.stage = Throw_HgtStabilise; // initialize vertical speed and acceleration limits // use brake mode values for rapid response @@ -106,9 +104,9 @@ void Copter::throw_run() // 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()) { + } else if (throw_state.stage == Throw_HgtStabilise && throw_height_good()) { gcs_send_text(MAV_SEVERITY_INFO,"height achieved - controlling position"); - throw_state = Throw_PosHold; + throw_state.stage = Throw_PosHold; // initialise the loiter target to the curent position and velocity wp_nav.init_loiter_target(); @@ -118,7 +116,7 @@ void Copter::throw_run() } // Throw State Processing - switch (throw_state) { + switch (throw_state.stage) { case Throw_Disarmed: diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 9d60b730e1..c310336b2d 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -249,8 +249,8 @@ enum FlipState { Flip_Abandon }; -// Throw states -enum ThrowModeState { +// Throw stages +enum ThrowModeStage { Throw_Disarmed, Throw_Detecting, Throw_Uprighting,