Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: create Copter::Mode::_TakeOff subobject from takeoff_state

This commit is contained in:
Peter Barker 2018-03-16 17:22:14 +11:00 committed by Randy Mackay
parent 7883582c65
commit ff17c78c67
8 changed files with 64 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/mode.cpp
View File

@ -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;
}

22
ArduCopter/mode.h
View File

@ -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

8
ArduCopter/mode_althold.cpp
View File

@ -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);

8
ArduCopter/mode_flowhold.cpp
View File

@ -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);

8
ArduCopter/mode_loiter.cpp
View File

@ -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);

10
ArduCopter/mode_poshold.cpp
View File

@ -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

8
ArduCopter/mode_sport.cpp
View File

@ -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);

51
ArduCopter/takeoff.cpp
View File

@ -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;