mirror of https://github.com/ArduPilot/ardupilot
Copter: remove un-needed PayloadPlace::run_should_run
make this look like the other handling, especially in terms of what we do if we shouldn't be running
This commit is contained in:
parent
5820a9bc2f
commit
e2157d8b9f
|
@ -52,7 +52,6 @@ public:
|
|||
float descent_max_cm;
|
||||
|
||||
private:
|
||||
bool run_should_run();
|
||||
void run_hover();
|
||||
void run_descent();
|
||||
void run_release();
|
||||
|
|
|
@ -1164,8 +1164,8 @@ void PayloadPlace::run()
|
|||
{
|
||||
const char* prefix_str = "PayloadPlace:";
|
||||
|
||||
if (!run_should_run()) {
|
||||
copter.flightmode->zero_throttle_and_relax_ac();
|
||||
if (copter.flightmode->is_disarmed_or_landed()) {
|
||||
copter.flightmode->make_safe_ground_handling();
|
||||
return;
|
||||
}
|
||||
|
||||
|
@ -1369,29 +1369,6 @@ void PayloadPlace::run()
|
|||
}
|
||||
}
|
||||
|
||||
bool PayloadPlace::run_should_run()
|
||||
{
|
||||
// must be armed
|
||||
if (!copter.motors->armed()) {
|
||||
return false;
|
||||
}
|
||||
// must be auto-armed
|
||||
if (!copter.ap.auto_armed) {
|
||||
return false;
|
||||
}
|
||||
// must not be landed
|
||||
if (copter.ap.land_complete &&
|
||||
(state == State::FlyToLocation || state == State::Descent_Start)) {
|
||||
return false;
|
||||
}
|
||||
// interlock must be enabled (i.e. unsafe)
|
||||
if (!copter.motors->get_interlock()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void PayloadPlace::run_hover()
|
||||
{
|
||||
const auto &pos_control = copter.pos_control;
|
||||
|
|
Loading…
Reference in New Issue