4fe937b985
This reverses the caller so the vehicle code calls into the mode instead of the mode calling up into the vehicle code
33 lines
713 B
C++
33 lines
713 B
C++
#include "mode.h"
|
|
#include "Rover.h"
|
|
|
|
bool ModeRTL::_enter()
|
|
{
|
|
rover.prev_WP = rover.current_loc;
|
|
rover.next_WP = rover.home;
|
|
g2.motors.slew_limit_throttle(true);
|
|
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();
|
|
}
|
|
}
|