ardupilot/APMrover2/mode_smart_rtl.cpp

92 lines
3.2 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;
}
// initialise waypoint speed
set_desired_speed_to_default(true);
// init location target
set_desired_location(rover.current_loc);
// init state
smart_rtl_state = SmartRTL_WaitForPathCleanup;
_reached_destination = 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 next_point;
if (!g2.smart_rtl.pop_point(next_point)) {
// if not more points, we have reached home
gcs().send_text(MAV_SEVERITY_INFO, "Reached destination");
smart_rtl_state = SmartRTL_StopAtHome;
break;
}
_load_point = false;
// set target destination to new point
if (!set_desired_location_NED(next_point)) {
// this failure should never happen but we add it just in case
gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination");
smart_rtl_state = SmartRTL_Failure;
}
}
// check if we've reached the next point
_distance_to_destination = get_distance(rover.current_loc, _destination);
if (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination)) {
_load_point = true;
}
// continue driving towards destination
calc_steering_to_waypoint(_origin, _destination);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, false);
break;
case SmartRTL_StopAtHome:
case SmartRTL_Failure:
_reached_destination = true;
if (rover.is_boat()) {
// boats attempt to hold position at home
calc_steering_to_waypoint(rover.current_loc, _destination);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, false);
} else {
// rovers stop
stop_vehicle();
}
break;
}
}
// 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);
}