You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
31 lines
621 B
31 lines
621 B
8 years ago
|
#include "mode.h"
|
||
|
|
#include "Rover.h"
|
||
|
|
|
||
|
|
bool ModeRTL::_enter()
|
||
|
|
{
|
||
|
|
rover.do_RTL();
|
||
|
|
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();
|
||
|
|
}
|
||
|
|
}
|