diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index d13c4d9592..9c93f56ccf 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -415,16 +415,6 @@ private: int32_t nav_delay_time_max; // used for delaying the navigation commands (eg land,takeoff etc.) uint32_t nav_delay_time_start; - // throw mode state - struct { - ThrowModeStage stage; - ThrowModeStage prev_stage; - uint32_t last_log_ms; - bool nextmode_attempted; - uint32_t free_fall_start_ms; // system time free fall was detected - float free_fall_start_velz; // vertical velocity when free fall was detected - } throw_state = {Throw_Disarmed, Throw_Disarmed, 0, false, 0, 0.0f}; - // Battery Sensors AP_BattMonitor battery = AP_BattMonitor::create(); @@ -782,14 +772,6 @@ private: void set_mode_land_with_pause(mode_reason_t reason); bool landing_with_GPS(); - // 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 throw_position_good(); - bool smart_rtl_init(bool ignore_checks); void smart_rtl_exit(); void smart_rtl_run(); @@ -1036,6 +1018,8 @@ private: Copter::FlightMode_AVOID_ADSB flightmode_avoid_adsb{*this}; + Copter::FlightMode_THROW flightmode_throw{*this}; + public: void mavlink_delay_cb(); void failsafe_check(); diff --git a/ArduCopter/FlightMode.h b/ArduCopter/FlightMode.h index db5d364de8..8a4f614df7 100644 --- a/ArduCopter/FlightMode.h +++ b/ArduCopter/FlightMode.h @@ -824,3 +824,43 @@ protected: private: }; + + + +class FlightMode_THROW : public FlightMode { + +public: + + FlightMode_THROW(Copter &copter) : + Copter::FlightMode(copter) + { } + + bool init(bool ignore_checks) override; + void run() override; // should be called at 100hz or more + + bool requires_GPS() const override { return true; } + bool has_manual_throttle() const override { return false; } + bool allows_arming(bool from_gcs) const override { return true; }; + bool is_autopilot() const override { return false; } + + +protected: + + const char *name() const override { return "THROW"; } + const char *name4() const override { return "THRW"; } + +private: + + bool throw_detected(); + bool throw_position_good(); + bool throw_height_good(); + bool throw_attitude_good(); + + ThrowModeStage stage = Throw_Disarmed; + ThrowModeStage prev_stage = Throw_Disarmed; + uint32_t last_log_ms; + bool nextmode_attempted; + uint32_t free_fall_start_ms; // system time free fall was detected + float free_fall_start_velz; // vertical velocity when free fall was detected + +}; diff --git a/ArduCopter/control_throw.cpp b/ArduCopter/control_throw.cpp index 10f0929733..c68269d5ac 100644 --- a/ArduCopter/control_throw.cpp +++ b/ArduCopter/control_throw.cpp @@ -2,7 +2,7 @@ // throw_init - initialise throw controller -bool Copter::throw_init(bool ignore_checks) +bool Copter::FlightMode_THROW::init(bool ignore_checks) { #if FRAME_CONFIG == HELI_FRAME // do not allow helis to use throw to start @@ -15,15 +15,15 @@ bool Copter::throw_init(bool ignore_checks) } // init state - throw_state.stage = Throw_Disarmed; - throw_state.nextmode_attempted = false; + stage = Throw_Disarmed; + nextmode_attempted = false; return true; } // runs the throw to start controller // should be called at 100hz or more -void Copter::throw_run() +void Copter::FlightMode_THROW::run() { /* Throw State Machine Throw_Disarmed - motors are off @@ -36,22 +36,22 @@ 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.stage = Throw_Disarmed; + stage = Throw_Disarmed; - } else if (throw_state.stage == Throw_Disarmed && motors->armed()) { + } else if (stage == Throw_Disarmed && motors->armed()) { gcs().send_text(MAV_SEVERITY_INFO,"waiting for throw"); - throw_state.stage = Throw_Detecting; + stage = Throw_Detecting; - } else if (throw_state.stage == Throw_Detecting && throw_detected()){ + } else if (stage == Throw_Detecting && throw_detected()){ gcs().send_text(MAV_SEVERITY_INFO,"throw detected - uprighting"); - throw_state.stage = Throw_Uprighting; + stage = Throw_Uprighting; // Cancel the waiting for throw tone sequence AP_Notify::flags.waiting_for_throw = false; - } else if (throw_state.stage == Throw_Uprighting && throw_attitude_good()) { + } else if (stage == Throw_Uprighting && throw_attitude_good()) { gcs().send_text(MAV_SEVERITY_INFO,"uprighted - controlling height"); - throw_state.stage = Throw_HgtStabilise; + stage = Throw_HgtStabilise; // initialize vertical speed and acceleration limits // use brake mode values for rapid response @@ -71,19 +71,19 @@ void Copter::throw_run() 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); + _copter.set_auto_armed(true); - } else if (throw_state.stage == Throw_HgtStabilise && throw_height_good()) { + } else if (stage == Throw_HgtStabilise && throw_height_good()) { gcs().send_text(MAV_SEVERITY_INFO,"height achieved - controlling position"); - throw_state.stage = Throw_PosHold; + stage = 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); - } else if (throw_state.stage == Throw_PosHold && throw_position_good()) { - if (!throw_state.nextmode_attempted) { + _copter.set_auto_armed(true); + } else if (stage == Throw_PosHold && throw_position_good()) { + if (!nextmode_attempted) { switch (g2.throw_nextmode) { case AUTO: case GUIDED: @@ -97,12 +97,12 @@ void Copter::throw_run() // do nothing break; } - throw_state.nextmode_attempted = true; + nextmode_attempted = true; } } // Throw State Processing - switch (throw_state.stage) { + switch (stage) { case Throw_Disarmed: @@ -181,30 +181,30 @@ void Copter::throw_run() // log at 10hz or if stage changes uint32_t now = AP_HAL::millis(); - if ((throw_state.stage != throw_state.prev_stage) || (now - throw_state.last_log_ms) > 100) { - throw_state.prev_stage = throw_state.stage; - throw_state.last_log_ms = now; + if ((stage != prev_stage) || (now - last_log_ms) > 100) { + prev_stage = stage; + last_log_ms = now; float velocity = inertial_nav.get_velocity().length(); float velocity_z = inertial_nav.get_velocity().z; - float accel = ins.get_accel().length(); + float accel = _copter.ins.get_accel().length(); float ef_accel_z = ahrs.get_accel_ef().z; - bool throw_detect = (throw_state.stage > Throw_Detecting) || throw_detected(); - bool attitude_ok = (throw_state.stage > Throw_Uprighting) || throw_attitude_good(); - bool height_ok = (throw_state.stage > Throw_HgtStabilise) || throw_height_good(); - bool pos_ok = (throw_state.stage > Throw_PosHold) || throw_position_good(); - Log_Write_Throw(throw_state.stage, - velocity, - velocity_z, - accel, - ef_accel_z, - throw_detect, - attitude_ok, - height_ok, - pos_ok); + bool throw_detect = (stage > Throw_Detecting) || throw_detected(); + bool attitude_ok = (stage > Throw_Uprighting) || throw_attitude_good(); + bool height_ok = (stage > Throw_HgtStabilise) || throw_height_good(); + bool pos_ok = (stage > Throw_PosHold) || throw_position_good(); + _copter.Log_Write_Throw(stage, + velocity, + velocity_z, + accel, + ef_accel_z, + throw_detect, + attitude_ok, + height_ok, + pos_ok); } } -bool Copter::throw_detected() +bool Copter::FlightMode_THROW::throw_detected() { // Check that we have a valid navigation solution nav_filter_status filt_status = inertial_nav.get_filter_status(); @@ -227,19 +227,19 @@ bool Copter::throw_detected() 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; + bool no_throw_action = _copter.ins.get_accel().length() < 1.0f * GRAVITY_MSS; // High velocity or free-fall combined with increasing height indicate a possible air-drop or throw release bool possible_throw_detected = (free_falling || high_speed) && changing_height && no_throw_action; // Record time and vertical velocity when we detect the possible throw - if (possible_throw_detected && ((AP_HAL::millis() - throw_state.free_fall_start_ms) > 500)) { - throw_state.free_fall_start_ms = AP_HAL::millis(); - throw_state.free_fall_start_velz = inertial_nav.get_velocity().z; + if (possible_throw_detected && ((AP_HAL::millis() - free_fall_start_ms) > 500)) { + free_fall_start_ms = 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() - throw_state.free_fall_start_ms < 500) && ((inertial_nav.get_velocity().z - throw_state.free_fall_start_velz) < -250.0f)); + bool throw_condition_confirmed = ((AP_HAL::millis() - free_fall_start_ms < 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) { @@ -249,20 +249,20 @@ bool Copter::throw_detected() } } -bool Copter::throw_attitude_good() +bool Copter::FlightMode_THROW::throw_attitude_good() { // Check that we have uprighted the copter const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned(); return (rotMat.c.z > 0.866f); // is_upright } -bool Copter::throw_height_good() +bool Copter::FlightMode_THROW::throw_height_good() { // Check that we are no more than 0.5m below the demanded height return (pos_control->get_alt_error() < 50.0f); } -bool Copter::throw_position_good() +bool Copter::FlightMode_THROW::throw_position_good() { // check that our horizontal position error is within 50cm return (pos_control->get_horizontal_error() < 50.0f); diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index 686c3c5c25..b7e58e102c 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -149,7 +149,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) break; case THROW: - success = throw_init(ignore_checks); + success = flightmode_throw.init(ignore_checks); + if (success) { + flightmode = &flightmode_throw; + } break; case AVOID_ADSB: @@ -233,10 +236,6 @@ void Copter::update_flight_mode() switch (control_mode) { - case THROW: - throw_run(); - break; - case GUIDED_NOGPS: guided_nogps_run(); break; @@ -312,7 +311,6 @@ bool Copter::mode_requires_GPS() } switch (control_mode) { case SMART_RTL: - case THROW: return true; default: return false; @@ -369,9 +367,6 @@ void Copter::notify_flight_mode() // set flight mode string switch (control_mode) { - case THROW: - notify.set_flight_mode_str("THRW"); - break; case GUIDED_NOGPS: notify.set_flight_mode_str("GNGP"); break;