2017-07-18 23:17:45 -03:00
|
|
|
#include "mode.h"
|
|
|
|
|
#include "Rover.h"
|
|
|
|
|
|
|
|
|
|
bool ModeRTL::_enter()
|
|
|
|
|
{
|
|
|
|
|
rover.do_RTL();
|
2017-07-12 06:45:58 -03:00
|
|
|
g2.motors.slew_limit_throttle(true);
|
2017-07-18 23:17:45 -03:00
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void ModeRTL::update()
|
|
|
|
|
{
|
|
|
|
|
if (!rover.in_auto_reverse) {
|
|
|
|
|
rover.set_reverse(false);
|
|
|
|
|
}
|
|
|
|
|
calc_lateral_acceleration();
|
|
|
|
|
calc_nav_steer();
|
|
|
|
|
calc_throttle(g.speed_cruise);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void ModeRTL::update_navigation()
|
|
|
|
|
{
|
|
|
|
|
// no loitering around the wp with the rover, goes direct to the wp position
|
|
|
|
|
if (rover.verify_RTL()) {
|
|
|
|
|
g2.motors.set_throttle(g.throttle_min.get());
|
|
|
|
|
rover.set_mode(rover.mode_hold);
|
|
|
|
|
} else {
|
|
|
|
|
calc_lateral_acceleration();
|
|
|
|
|
calc_nav_steer();
|
|
|
|
|
}
|
|
|
|
|
}
|
