mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 17:38:32 -04:00
Plane: mode_qloiter: combine enter and init
This commit is contained in:
parent
70d9d20faf
commit
fa908b0a1d
@ -2,11 +2,6 @@
|
|||||||
#include "Plane.h"
|
#include "Plane.h"
|
||||||
|
|
||||||
bool ModeQLoiter::_enter()
|
bool ModeQLoiter::_enter()
|
||||||
{
|
|
||||||
return plane.mode_qstabilize._enter();
|
|
||||||
}
|
|
||||||
|
|
||||||
void ModeQLoiter::init()
|
|
||||||
{
|
{
|
||||||
// initialise loiter
|
// initialise loiter
|
||||||
loiter_nav->clear_pilot_desired_acceleration();
|
loiter_nav->clear_pilot_desired_acceleration();
|
||||||
@ -23,6 +18,7 @@ void ModeQLoiter::init()
|
|||||||
|
|
||||||
// prevent re-init of target position
|
// prevent re-init of target position
|
||||||
quadplane.last_loiter_ms = AP_HAL::millis();
|
quadplane.last_loiter_ms = AP_HAL::millis();
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ModeQLoiter::update()
|
void ModeQLoiter::update()
|
||||||
@ -43,7 +39,7 @@ void ModeQLoiter::run()
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
if (!quadplane.motors->armed()) {
|
if (!quadplane.motors->armed()) {
|
||||||
plane.mode_qloiter.init();
|
plane.mode_qloiter._enter();
|
||||||
}
|
}
|
||||||
|
|
||||||
quadplane.check_attitude_relax();
|
quadplane.check_attitude_relax();
|
||||||
|
Loading…
Reference in New Issue
Block a user