Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: factor out takeoff_trigger from althold, sport and loiter

Browse Source
This commit is contained in:
Peter Barker 2017-12-12 21:53:56 +11:00 committed by Francisco Ferreira
parent 2c0df87585
commit bf5f51a616
5 changed files with 26 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduCopter/mode.cpp
View File

@ -242,3 +242,23 @@ void Copter::Mode::update_navigation()
// run autopilot to make high level decisions about control modes
run_autopilot();
}
bool Copter::Mode::takeoff_triggered(const float target_climb_rate) const
{
if (!ap.land_complete) {
// can't take off if we're already flying
return false;
}
if (target_climb_rate <= 0.0f) {
// can't takeoff unless we want to go up...
return false;
}
#if FRAME_CONFIG == HELI_FRAME
if (!motors->rotor_runup_complete()) {
// hold heli on the ground until rotor speed runup has finished
return false;
}
#endif
return true;
}

3
ArduCopter/mode.h
View File

@ -72,6 +72,9 @@ protected:
ap_t &ap;
takeoff_state_t &takeoff_state;
// takeoff support
bool takeoff_triggered(float target_climb_rate) const;
// gnd speed limit required to observe optical flow sensor limits
float &ekfGndSpdLimit;

9
ArduCopter/mode_althold.cpp
View File

@ -49,17 +49,10 @@ void Copter::ModeAltHold::run()
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
#if FRAME_CONFIG == HELI_FRAME
// helicopters are held on the ground until rotor speed runup has finished
bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors->rotor_runup_complete());
#else
bool takeoff_triggered = ap.land_complete && (target_climb_rate > 0.0f);
#endif
// Alt Hold State Machine Determination
if (!motors->armed() || !motors->get_interlock()) {
althold_state = AltHold_MotorStopped;
} else if (takeoff_state.running || takeoff_triggered) {
} else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) {
althold_state = AltHold_Takeoff;
} else if (!ap.auto_armed || ap.land_complete) {
althold_state = AltHold_Landed;

9
ArduCopter/mode_loiter.cpp
View File

@ -101,17 +101,10 @@ void Copter::ModeLoiter::run()
wp_nav->loiter_soften_for_landing();
}
#if FRAME_CONFIG == HELI_FRAME
// helicopters are held on the ground until rotor speed runup has finished
bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors->rotor_runup_complete());
#else
bool takeoff_triggered = ap.land_complete && (target_climb_rate > 0.0f);
#endif
// Loiter State Machine Determination
if (!motors->armed() || !motors->get_interlock()) {
loiter_state = Loiter_MotorStopped;
} else if (takeoff_state.running || takeoff_triggered) {
} else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) {
loiter_state = Loiter_Takeoff;
} else if (!ap.auto_armed || ap.land_complete) {
loiter_state = Loiter_Landed;

9
ArduCopter/mode_sport.cpp
View File

@ -71,17 +71,10 @@ void Copter::ModeSport::run()
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
#if FRAME_CONFIG == HELI_FRAME
// helicopters are held on the ground until rotor speed runup has finished
bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors->rotor_runup_complete());
#else
bool takeoff_triggered = ap.land_complete && (target_climb_rate > 0.0f);
#endif
// State Machine Determination
if (!motors->armed() || !motors->get_interlock()) {
sport_state = Sport_MotorStopped;
} else if (takeoff_state.running || takeoff_triggered) {
} else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) {
sport_state = Sport_Takeoff;
} else if (!ap.auto_armed || ap.land_complete) {
sport_state = Sport_Landed;