From ff17c78c67c83638e176cf4e74dd961034165a1c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 16 Mar 2018 17:22:14 +1100 Subject: [PATCH] Copter: create Copter::Mode::_TakeOff subobject from takeoff_state --- ArduCopter/mode.cpp | 6 ++--- ArduCopter/mode.h | 22 ++++++++++------ ArduCopter/mode_althold.cpp | 8 +++--- ArduCopter/mode_flowhold.cpp | 8 +++--- ArduCopter/mode_loiter.cpp | 8 +++--- ArduCopter/mode_poshold.cpp | 10 +++---- ArduCopter/mode_sport.cpp | 8 +++--- ArduCopter/takeoff.cpp | 51 ++++++++++++++++++------------------ 8 files changed, 64 insertions(+), 57 deletions(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 6a2233187b..f812c6f7d1 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -352,9 +352,9 @@ void Copter::Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_o // roll_out and pitch_out are returned } -bool Copter::Mode::takeoff_triggered(const float target_climb_rate) const +bool Copter::Mode::_TakeOff::triggered(const float target_climb_rate) const { - if (!ap.land_complete) { + if (!copter.ap.land_complete) { // can't take off if we're already flying return false; } @@ -363,7 +363,7 @@ bool Copter::Mode::takeoff_triggered(const float target_climb_rate) const return false; } #if FRAME_CONFIG == HELI_FRAME - if (!motors->rotor_runup_complete()) { + if (!copter.motors->rotor_runup_complete()) { // hold heli on the ground until rotor speed runup has finished return false; } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 1ff27cad49..3b75570548 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -134,21 +134,27 @@ protected: ap_t ≈ // auto-takeoff support; takeoff state is shared across all mode instances - typedef struct { - bool running; + class _TakeOff { + public: + void start(float alt_cm); + void stop(); + void get_climb_rates(float& pilot_climb_rate, + float& takeoff_climb_rate); + bool triggered(float target_climb_rate) const; + + bool running() const { return _running; } + private: + bool _running; float max_speed; float alt_delta; uint32_t start_ms; - } takeoff_state_t; + }; - static takeoff_state_t takeoff_state; + static _TakeOff takeoff; - void takeoff_timer_start(float alt_cm); - void takeoff_stop(); - void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate); + static void takeoff_stop() { takeoff.stop(); } // takeoff support - bool takeoff_triggered(float target_climb_rate) const; virtual bool do_user_takeoff_start(float takeoff_alt_cm); // gnd speed limit required to observe optical flow sensor limits diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index 13f8da8f0c..88c3a41093 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -45,7 +45,7 @@ void Copter::ModeAltHold::run() // Alt Hold State Machine Determination if (!motors->armed() || !motors->get_interlock()) { althold_state = AltHold_MotorStopped; - } else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) { + } else if (takeoff.running() || takeoff.triggered(target_climb_rate)) { althold_state = AltHold_Takeoff; } else if (!ap.auto_armed || ap.land_complete) { althold_state = AltHold_Landed; @@ -85,8 +85,8 @@ void Copter::ModeAltHold::run() motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // initiate take-off - if (!takeoff_state.running) { - takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); + if (!takeoff.running()) { + takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); // indicate we are taking off set_land_complete(false); // clear i terms @@ -94,7 +94,7 @@ void Copter::ModeAltHold::run() } // get take-off adjusted pilot and takeoff climb rates - 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 = get_avoidance_adjusted_climbrate(target_climb_rate); diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index b68e71bcbe..e45c8d6d37 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 (takeoff_state.running || takeoff_triggered(target_climb_rate)) { + } else if (takeoff.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 (!takeoff_state.running) { - takeoff_timer_start(constrain_float(copter.g.pilot_takeoff_alt,0.0f,1000.0f)); + if (!takeoff.running()) { + takeoff.start(constrain_float(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 - 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/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 530596ff3d..16727a4e70 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -105,7 +105,7 @@ void Copter::ModeLoiter::run() // Loiter State Machine Determination if (!motors->armed() || !motors->get_interlock()) { loiter_state = Loiter_MotorStopped; - } else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) { + } else if (takeoff.running() || takeoff.triggered(target_climb_rate)) { loiter_state = Loiter_Takeoff; } else if (!ap.auto_armed || ap.land_complete) { loiter_state = Loiter_Landed; @@ -141,8 +141,8 @@ void Copter::ModeLoiter::run() motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // initiate take-off - if (!takeoff_state.running) { - takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); + if (!takeoff.running()) { + takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); // indicate we are taking off set_land_complete(false); // clear i term when we're taking off @@ -150,7 +150,7 @@ void Copter::ModeLoiter::run() } // get takeoff adjusted pilot and takeoff climb rates - 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 = get_avoidance_adjusted_climbrate(target_climb_rate); diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 45bd2b6727..3058930ed5 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -155,17 +155,17 @@ void Copter::ModePosHold::run() target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up); // get takeoff adjusted pilot and takeoff climb rates - takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); + takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate); // check for take-off #if FRAME_CONFIG == HELI_FRAME // helicopters are held on the ground until rotor speed runup has finished - if (ap.land_complete && (takeoff_state.running || (target_climb_rate > 0.0f && motors->rotor_runup_complete()))) { + if (ap.land_complete && (takeoff.running() || (target_climb_rate > 0.0f && motors->rotor_runup_complete()))) { #else - if (ap.land_complete && (takeoff_state.running || target_climb_rate > 0.0f)) { + if (ap.land_complete && (takeoff.running() || target_climb_rate > 0.0f)) { #endif - if (!takeoff_state.running) { - takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); + if (!takeoff.running()) { + takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); } // indicate we are taking off diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index fafaa548c0..4a3fb74aa6 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -74,7 +74,7 @@ void Copter::ModeSport::run() // State Machine Determination if (!motors->armed() || !motors->get_interlock()) { sport_state = Sport_MotorStopped; - } else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) { + } else if (takeoff.running() || takeoff.triggered(target_climb_rate)) { sport_state = Sport_Takeoff; } else if (!ap.auto_armed || ap.land_complete) { sport_state = Sport_Landed; @@ -106,8 +106,8 @@ void Copter::ModeSport::run() motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // initiate take-off - if (!takeoff_state.running) { - takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); + if (!takeoff.running()) { + takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); // indicate we are taking off set_land_complete(false); // clear i terms @@ -115,7 +115,7 @@ void Copter::ModeSport::run() } // get take-off adjusted pilot and takeoff climb rates - 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 = get_avoidance_adjusted_climbrate(target_climb_rate); diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index e3039287a2..0c70aa0fca 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -1,6 +1,6 @@ #include "Copter.h" -Copter::Mode::takeoff_state_t Copter::Mode::takeoff_state; +Copter::Mode::_TakeOff Copter::Mode::takeoff; // 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 @@ -9,7 +9,7 @@ Copter::Mode::takeoff_state_t Copter::Mode::takeoff_state; bool Copter::Mode::do_user_takeoff_start(float takeoff_alt_cm) { - takeoff_timer_start(takeoff_alt_cm); + copter.flightmode->takeoff.start(takeoff_alt_cm); return true; } @@ -48,60 +48,61 @@ bool Copter::Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate) } // start takeoff to specified altitude above home in centimeters -void Copter::Mode::takeoff_timer_start(float alt_cm) +void Copter::Mode::_TakeOff::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)); + const float speed = MIN(copter.wp_nav->get_speed_up(), MAX(copter.g.pilot_speed_up*2.0f/3.0f, copter.g.pilot_speed_up-50.0f)); // sanity check speed and target - if (takeoff_state.running || speed <= 0.0f || alt_cm <= 0.0f) { + if (running() || speed <= 0.0f || alt_cm <= 0.0f) { return; } // initialise takeoff state - takeoff_state.running = true; - takeoff_state.max_speed = speed; - takeoff_state.start_ms = millis(); - takeoff_state.alt_delta = alt_cm; + _running = true; + max_speed = speed; + start_ms = millis(); + alt_delta = alt_cm; } // stop takeoff -void Copter::Mode::takeoff_stop() +void Copter::Mode::_TakeOff::stop() { - takeoff_state.running = false; - takeoff_state.start_ms = 0; + _running = false; + start_ms = 0; } // returns pilot and takeoff climb rates // 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::Mode::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) { + if (!_running) { takeoff_climb_rate = 0.0f; return; } // acceleration of 50cm/s/s - static const float takeoff_accel = 50.0f; - float takeoff_minspeed = MIN(50.0f,takeoff_state.max_speed); - float time_elapsed = (millis()-takeoff_state.start_ms)*1.0e-3f; - float speed = MIN(time_elapsed*takeoff_accel+takeoff_minspeed, takeoff_state.max_speed); + static constexpr float TAKEOFF_ACCEL = 50.0f; + const float takeoff_minspeed = MIN(50.0f, max_speed); + const float time_elapsed = (millis() - start_ms) * 1.0e-3f; + const float speed = MIN(time_elapsed * TAKEOFF_ACCEL + takeoff_minspeed, max_speed); - float time_to_max_speed = (takeoff_state.max_speed-takeoff_minspeed)/takeoff_accel; + const float time_to_max_speed = (max_speed - takeoff_minspeed) / TAKEOFF_ACCEL; float height_gained; if (time_elapsed <= time_to_max_speed) { - height_gained = 0.5f*takeoff_accel*sq(time_elapsed) + takeoff_minspeed*time_elapsed; + height_gained = 0.5f * TAKEOFF_ACCEL * sq(time_elapsed) + takeoff_minspeed * time_elapsed; } else { - height_gained = 0.5f*takeoff_accel*sq(time_to_max_speed) + takeoff_minspeed*time_to_max_speed + - (time_elapsed-time_to_max_speed)*takeoff_state.max_speed; + height_gained = 0.5f * TAKEOFF_ACCEL * sq(time_to_max_speed) + takeoff_minspeed * time_to_max_speed + + (time_elapsed - time_to_max_speed) * max_speed; } // check if the takeoff is over - if (height_gained >= takeoff_state.alt_delta) { - takeoff_stop(); + if (height_gained >= alt_delta) { + stop(); } // if takeoff climb rate is zero return @@ -118,7 +119,7 @@ void Copter::Mode::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeo // if overall climb rate is still positive, move to take-off climb rate if (takeoff_climb_rate + pilot_climb_rate > 0.0f) { takeoff_climb_rate = takeoff_climb_rate + pilot_climb_rate; - pilot_climb_rate = 0; + pilot_climb_rate = 0.0f; } else { // if overall is negative, move to pilot climb rate pilot_climb_rate = pilot_climb_rate + takeoff_climb_rate;