mirror of https://github.com/ArduPilot/ardupilot
Copter: Guided prevent takeoff without takeoff command.
This commit is contained in:
parent
52166c94fe
commit
9f1cf90160
|
@ -606,17 +606,6 @@ void ModeGuided::accel_control_run()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// landed with positive desired climb rate, initiate takeoff
|
|
||||||
if (motors->armed() && copter.ap.auto_armed && copter.ap.land_complete && is_positive(guided_vel_target_cms.z)) {
|
|
||||||
zero_throttle_and_relax_ac();
|
|
||||||
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
|
||||||
if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
|
|
||||||
set_land_complete(false);
|
|
||||||
set_throttle_takeoff();
|
|
||||||
}
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// if not armed set throttle to zero and exit immediately
|
// if not armed set throttle to zero and exit immediately
|
||||||
if (is_disarmed_or_landed()) {
|
if (is_disarmed_or_landed()) {
|
||||||
make_safe_spool_down();
|
make_safe_spool_down();
|
||||||
|
@ -677,17 +666,6 @@ void ModeGuided::velaccel_control_run()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// landed with positive desired climb rate, initiate takeoff
|
|
||||||
if (motors->armed() && copter.ap.auto_armed && copter.ap.land_complete && is_positive(guided_vel_target_cms.z)) {
|
|
||||||
zero_throttle_and_relax_ac();
|
|
||||||
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
|
||||||
if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
|
|
||||||
set_land_complete(false);
|
|
||||||
set_throttle_takeoff();
|
|
||||||
}
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// if not armed set throttle to zero and exit immediately
|
// if not armed set throttle to zero and exit immediately
|
||||||
if (is_disarmed_or_landed()) {
|
if (is_disarmed_or_landed()) {
|
||||||
make_safe_spool_down();
|
make_safe_spool_down();
|
||||||
|
|
Loading…
Reference in New Issue