|
|
|
@ -48,11 +48,16 @@ bool Copter::Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
@@ -48,11 +48,16 @@ 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::start(float alt_cm) |
|
|
|
|
{ |
|
|
|
|
// indicate we are taking off
|
|
|
|
|
copter.set_land_complete(false); |
|
|
|
|
// tell position controller to reset alt target and reset I terms
|
|
|
|
|
copter.set_throttle_takeoff(); |
|
|
|
|
|
|
|
|
|
// calculate climb rate
|
|
|
|
|
const float speed = MIN(copter.wp_nav->get_default_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 (running() || speed <= 0.0f || alt_cm <= 0.0f) { |
|
|
|
|
if (speed <= 0.0f || alt_cm <= 0.0f) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|