2017-07-18 23:17:45 -03:00
|
|
|
#include "mode.h"
|
|
|
|
#include "Rover.h"
|
|
|
|
|
|
|
|
bool ModeRTL::_enter()
|
|
|
|
{
|
2017-08-03 05:14:52 -03:00
|
|
|
// refuse RTL if home has not been set
|
2018-03-15 22:49:06 -03:00
|
|
|
if (!AP::ahrs().home_is_set()) {
|
2017-08-03 05:14:52 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2022-01-05 20:01:49 -04:00
|
|
|
// initialise waypoint navigation library
|
|
|
|
g2.wp_nav.init(MAX(0, g2.rtl_speed));
|
|
|
|
|
2019-05-08 21:54:40 -03:00
|
|
|
// set target to the closest rally point or home
|
|
|
|
#if AP_RALLY == ENABLED
|
|
|
|
if (!g2.wp_nav.set_desired_location(g2.rally.calc_best_rally_or_home_location(rover.current_loc, ahrs.get_home().alt))) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
#else
|
|
|
|
// set destination
|
2019-05-09 20:42:12 -03:00
|
|
|
if (!g2.wp_nav.set_desired_location(ahrs.get_home())) {
|
2019-05-08 21:54:40 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2021-03-03 00:51:46 -04:00
|
|
|
send_notification = true;
|
2019-10-04 21:20:51 -03:00
|
|
|
_loitering = false;
|
2017-07-18 23:17:45 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
void ModeRTL::update()
|
|
|
|
{
|
2018-02-05 02:39:48 -04:00
|
|
|
// determine if we should keep navigating
|
2019-05-09 07:48:24 -03:00
|
|
|
if (!g2.wp_nav.reached_destination()) {
|
2019-04-29 03:31:45 -03:00
|
|
|
// update navigation controller
|
|
|
|
navigate_to_waypoint();
|
2017-08-03 05:14:52 -03:00
|
|
|
} else {
|
2019-04-29 03:31:45 -03:00
|
|
|
// send notification
|
2021-03-03 00:51:46 -04:00
|
|
|
if (send_notification) {
|
|
|
|
send_notification = false;
|
2019-04-29 03:31:45 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Reached destination");
|
|
|
|
}
|
|
|
|
|
2019-05-09 07:48:24 -03:00
|
|
|
// we have reached the destination
|
2019-10-04 21:20:51 -03:00
|
|
|
// boats loiter, rovers stop
|
2019-10-04 15:26:56 -03:00
|
|
|
if (!rover.is_boat()) {
|
2019-05-09 07:48:24 -03:00
|
|
|
stop_vehicle();
|
2019-10-04 15:26:56 -03:00
|
|
|
} else {
|
|
|
|
// if not loitering yet, start loitering
|
2019-10-04 21:20:51 -03:00
|
|
|
if (!_loitering) {
|
|
|
|
_loitering = rover.mode_loiter.enter();
|
|
|
|
}
|
|
|
|
// update stop or loiter
|
|
|
|
if (_loitering) {
|
|
|
|
rover.mode_loiter.update();
|
|
|
|
} else {
|
|
|
|
stop_vehicle();
|
2019-10-04 15:26:56 -03:00
|
|
|
}
|
|
|
|
}
|
2019-10-04 21:20:51 -03:00
|
|
|
|
2019-04-29 03:31:45 -03:00
|
|
|
// update distance to destination
|
|
|
|
_distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination());
|
2017-07-18 23:17:45 -03:00
|
|
|
}
|
|
|
|
}
|
2019-04-29 03:31:45 -03:00
|
|
|
|
2019-05-17 03:55:31 -03:00
|
|
|
// get desired location
|
|
|
|
bool ModeRTL::get_desired_location(Location& destination) const
|
|
|
|
{
|
|
|
|
if (g2.wp_nav.is_destination_valid()) {
|
2019-05-10 02:59:52 -03:00
|
|
|
destination = g2.wp_nav.get_oa_destination();
|
2019-05-17 03:55:31 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2019-04-29 03:31:45 -03:00
|
|
|
bool ModeRTL::reached_destination() const
|
|
|
|
{
|
|
|
|
return g2.wp_nav.reached_destination();
|
|
|
|
}
|
2019-11-04 04:31:13 -04:00
|
|
|
|
|
|
|
// set desired speed in m/s
|
|
|
|
bool ModeRTL::set_desired_speed(float speed)
|
|
|
|
{
|
2022-01-05 20:43:58 -04:00
|
|
|
return g2.wp_nav.set_speed_max(speed);
|
2019-11-04 04:31:13 -04:00
|
|
|
}
|