ardupilot/Rover/mode_smart_rtl.cpp

147 lines
4.8 KiB
C++

#include "mode.h"
#include "Rover.h"
bool ModeSmartRTL::_enter()
{
// SmartRTL requires EKF (not DCM)
Location ekf_origin;
if (!ahrs.get_origin(ekf_origin)) {
return false;
}
// refuse to enter SmartRTL if smart RTL's home has not been set
if (!g2.smart_rtl.is_active()) {
return false;
}
// set desired location to reasonable stopping point
if (!g2.wp_nav.set_desired_location_to_stopping_location()) {
return false;
}
// initialise waypoint speed
if (is_positive(g2.rtl_speed)) {
g2.wp_nav.set_desired_speed(g2.rtl_speed);
} else {
g2.wp_nav.set_desired_speed_to_default();
}
// init state
smart_rtl_state = SmartRTL_WaitForPathCleanup;
_loitering = false;
return true;
}
void ModeSmartRTL::update()
{
switch (smart_rtl_state) {
case SmartRTL_WaitForPathCleanup:
// check if return path is computed and if yes, begin journey home
if (g2.smart_rtl.request_thorough_cleanup()) {
smart_rtl_state = SmartRTL_PathFollow;
_load_point = true;
}
// Note: this may lead to an unnecessary 20ms slow down of the vehicle (but it is unlikely)
stop_vehicle();
break;
case SmartRTL_PathFollow:
// load point if required
if (_load_point) {
Vector3f dest_NED;
if (!g2.smart_rtl.pop_point(dest_NED)) {
// if not more points, we have reached home
gcs().send_text(MAV_SEVERITY_INFO, "Reached destination");
smart_rtl_state = SmartRTL_StopAtHome;
break;
} else {
// peek at the next point. this can fail if the IO task currently has the path semaphore
Vector3f next_dest_NED;
if (g2.smart_rtl.peek_point(next_dest_NED)) {
if (!g2.wp_nav.set_desired_location_NED(dest_NED, next_dest_NED)) {
// this should never happen because the EKF origin should already be set
gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination");
smart_rtl_state = SmartRTL_Failure;
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
} else {
// no next point so add only immediate point
if (!g2.wp_nav.set_desired_location_NED(dest_NED)) {
// this should never happen because the EKF origin should already be set
gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination");
smart_rtl_state = SmartRTL_Failure;
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
}
}
_load_point = false;
}
// update navigation controller
navigate_to_waypoint();
// check if we've reached the next point
if (g2.wp_nav.reached_destination()) {
_load_point = true;
}
break;
case SmartRTL_StopAtHome:
case SmartRTL_Failure:
_reached_destination = true;
// we have reached the destination
// boats loiters, rovers stop
if (!rover.is_boat()) {
stop_vehicle();
} else {
// if not loitering yet, start loitering
if (!_loitering) {
_loitering = rover.mode_loiter.enter();
}
if (_loitering) {
rover.mode_loiter.update();
} else {
stop_vehicle();
}
}
break;
}
}
// get desired location
bool ModeSmartRTL::get_desired_location(Location& destination) const
{
switch (smart_rtl_state) {
case SmartRTL_WaitForPathCleanup:
return false;
case SmartRTL_PathFollow:
if (g2.wp_nav.is_destination_valid()) {
destination = g2.wp_nav.get_destination();
return true;
}
return false;
case SmartRTL_StopAtHome:
case SmartRTL_Failure:
return false;
}
// should never reach here but just in case
return false;
}
// set desired speed in m/s
bool ModeSmartRTL::set_desired_speed(float speed)
{
if (is_negative(speed)) {
return false;
}
g2.wp_nav.set_desired_speed(speed);
return true;
}
// save current position for use by the smart_rtl flight mode
void ModeSmartRTL::save_position()
{
const bool save_pos = (rover.control_mode != &rover.mode_smartrtl);
g2.smart_rtl.update(true, save_pos);
}