From 7883582c6541ff432f39144689d40293ab8e6a81 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 12 Dec 2017 21:34:49 +1100 Subject: [PATCH] Copter: move takeoff state into Mode class --- ArduCopter/Copter.h | 12 ------------ ArduCopter/mode.cpp | 18 +----------------- ArduCopter/mode.h | 18 ++++++++++++++---- ArduCopter/mode_flowhold.cpp | 8 ++++---- ArduCopter/takeoff.cpp | 8 +++++--- 5 files changed, 24 insertions(+), 40 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 27a5b543bd..b8f653de4b 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -362,15 +362,6 @@ private: uint8_t count; uint8_t ch_flag; } aux_debounce[(CH_12 - CH_7)+1]; - - typedef struct { - bool running; - float max_speed; - float alt_delta; - uint32_t start_ms; - } takeoff_state_t; - takeoff_state_t takeoff_state; - // altitude below which we do no navigation in auto takeoff float auto_takeoff_no_nav_alt_cm; @@ -923,9 +914,6 @@ private: const char* get_frame_string(); void allocate_motors(void); - void takeoff_timer_start(float alt_cm); - void takeoff_stop(); - void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate); void auto_takeoff_set_start_alt(void); void auto_takeoff_attitude_run(float target_yaw_rate); diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index b7a3010f3a..6a2233187b 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -24,7 +24,6 @@ Copter::Mode::Mode(void) : channel_yaw(copter.channel_yaw), G_Dt(copter.G_Dt), ap(copter.ap), - takeoff_state(copter.takeoff_state), ekfGndSpdLimit(copter.ekfGndSpdLimit), #if FRAME_CONFIG == HELI_FRAME heli_flags(copter.heli_flags), @@ -281,7 +280,7 @@ void Copter::exit_mode(Copter::Mode *&old_flightmode, } // cancel any takeoffs in progress - takeoff_stop(); + old_flightmode->takeoff_stop(); #if MODE_SMARTRTL_ENABLED == ENABLED // call smart_rtl cleanup @@ -601,21 +600,6 @@ void Copter::Mode::set_throttle_takeoff() return copter.set_throttle_takeoff(); } -void Copter::Mode::takeoff_timer_start(float alt_cm) -{ - return copter.takeoff_timer_start(alt_cm); -} - -void Copter::Mode::takeoff_stop() -{ - return copter.takeoff_stop(); -} - -void Copter::Mode::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate) -{ - return copter.takeoff_get_climb_rates(pilot_climb_rate, takeoff_climb_rate); -} - float Copter::Mode::get_avoidance_adjusted_climbrate(float target_rate) { return copter.get_avoidance_adjusted_climbrate(target_rate); diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 64d9986a7d..1ff27cad49 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -132,7 +132,20 @@ protected: RC_Channel *&channel_yaw; float &G_Dt; ap_t ≈ - takeoff_state_t &takeoff_state; + + // auto-takeoff support; takeoff state is shared across all mode instances + typedef struct { + bool running; + float max_speed; + float alt_delta; + uint32_t start_ms; + } takeoff_state_t; + + static takeoff_state_t takeoff_state; + + void takeoff_timer_start(float alt_cm); + void takeoff_stop(); + void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate); // takeoff support bool takeoff_triggered(float target_climb_rate) const; @@ -162,9 +175,6 @@ protected: GCS_Copter &gcs(); void Log_Write_Event(uint8_t id); void set_throttle_takeoff(void); - void takeoff_timer_start(float alt_cm); - void takeoff_stop(void); - void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate); float get_avoidance_adjusted_climbrate(float target_rate); uint16_t get_pilot_speed_dn(void); diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 76e5615a5c..b68e71bcbe 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -244,7 +244,7 @@ void Copter::ModeFlowHold::run() if (!copter.motors->armed() || !copter.motors->get_interlock()) { flowhold_state = FlowHold_MotorStopped; - } else if (copter.takeoff_state.running || takeoff_triggered(target_climb_rate)) { + } else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) { flowhold_state = FlowHold_Takeoff; } else if (!copter.ap.auto_armed || copter.ap.land_complete) { flowhold_state = FlowHold_Landed; @@ -304,8 +304,8 @@ void Copter::ModeFlowHold::run() copter.motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // initiate take-off - if (!copter.takeoff_state.running) { - copter.takeoff_timer_start(constrain_float(copter.g.pilot_takeoff_alt,0.0f,1000.0f)); + if (!takeoff_state.running) { + takeoff_timer_start(constrain_float(copter.g.pilot_takeoff_alt,0.0f,1000.0f)); // indicate we are taking off copter.set_land_complete(false); // clear i terms @@ -313,7 +313,7 @@ void Copter::ModeFlowHold::run() } // get take-off adjusted pilot and takeoff climb rates - copter.takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); + takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); // get avoidance adjusted climb rate target_climb_rate = copter.get_avoidance_adjusted_climbrate(target_climb_rate); diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 64ee1e8e6d..e3039287a2 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -1,5 +1,7 @@ #include "Copter.h" +Copter::Mode::takeoff_state_t Copter::Mode::takeoff_state; + // This file contains the high-level takeoff logic for Loiter, PosHold, AltHold, Sport modes. // The take-off can be initiated from a GCS NAV_TAKEOFF command which includes a takeoff altitude // A safe takeoff speed is calculated and used to calculate a time_ms @@ -46,7 +48,7 @@ bool Copter::Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate) } // start takeoff to specified altitude above home in centimeters -void Copter::takeoff_timer_start(float alt_cm) +void Copter::Mode::takeoff_timer_start(float alt_cm) { // calculate climb rate float speed = MIN(wp_nav->get_speed_up(), MAX(g.pilot_speed_up*2.0f/3.0f, g.pilot_speed_up-50.0f)); @@ -64,7 +66,7 @@ void Copter::takeoff_timer_start(float alt_cm) } // stop takeoff -void Copter::takeoff_stop() +void Copter::Mode::takeoff_stop() { takeoff_state.running = false; takeoff_state.start_ms = 0; @@ -74,7 +76,7 @@ void Copter::takeoff_stop() // pilot_climb_rate is both an input and an output // takeoff_climb_rate is only an output // has side-effect of turning takeoff off when timeout as expired -void Copter::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate) +void Copter::Mode::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate) { // return pilot_climb_rate if take-off inactive if (!takeoff_state.running) {