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);
// RTL never reverses
rover.set_reverse(false);
// init state
smart_rtl_state = SmartRTL_WaitForPathCleanup;
_reached_destination = false;
return true;
}
// set desired location to a reasonable stopping point, return true on success
bool AR_WPNav::set_desired_location_to_stopping_location()
{
Location stopping_loc;
if (!get_stopping_location(stopping_loc)) {
return false;
}
return set_desired_location(stopping_loc);
}
bool ModeRTL::_enter()
{
// refuse RTL if home has not been set
if (!AP::ahrs().home_is_set()) {
return false;
}
// initialise waypoint speed
set_desired_speed_to_default(true);
// set target to the closest rally point or home
#if AP_RALLY == ENABLED
set_desired_location(rover.g2.rally.calc_best_rally_or_home_location(rover.current_loc, ahrs.get_home().alt));
#else
// set destination
set_desired_location(rover.home);
#endif
return true;
}
bool ModeGuided::_enter()
{
// initialise waypoint speed
set_desired_speed_to_default();
// set desired location to reasonable stopping point
calc_stopping_location(_destination);
set_desired_location(_destination);
return true;
}
// set desired location as offset from the EKF origin, return true on success
bool AR_WPNav::set_desired_location_NED(const Vector3f& destination, float next_leg_bearing_cd)
{
// initialise destination to ekf origin
Location destination_ned;
if (!AP::ahrs().get_origin(destination_ned)) {
return false;
}
// apply offset
destination_ned.offset(destination.x, destination.y);
return set_desired_location(destination_ned, next_leg_bearing_cd);
}
// set desired location as an offset from the EKF origin in NED frame
bool Mode::set_desired_location_NED(const Vector3f& destination, float next_leg_bearing_cd)
{
Location destination_ned;
// initialise destination to ekf origin
if (!ahrs.get_origin(destination_ned)) {
return false;
}
// apply offset
location_offset(destination_ned, destination.x, destination.y);
set_desired_location(destination_ned, next_leg_bearing_cd);
return true;
}
bool ModeGuided::_enter()
{
// initialise waypoint speed
set_desired_speed_to_default();
// when entering guided mode we set the target as the current location.
_desired_lat_accel = 0.0f;
set_desired_location(rover.current_loc);
// guided mode never travels in reverse
rover.set_reverse(false);
return true;
}
bool ModeRTL::_enter()
{
// refuse RTL if home has not been set
if (rover.home_is_set == HOME_UNSET) {
return false;
}
// initialise waypoint speed
set_desired_speed_to_default(true);
// set destination
set_desired_location(rover.home);
// RTL never reverses
rover.set_reverse(false);
return true;
}
void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination)
{
// just starting so we haven't previously reached the waypoint
previously_reached_wp = false;
// this will be used to remember the time in millis after we reach or pass the WP.
loiter_start_time = 0;
// this is the delay, stored in seconds
loiter_duration = cmd.p1;
// get heading to following waypoint (auto mode reduces speed to allow corning without large overshoot)
// in case of non-zero loiter duration, we provide heading-unknown to signal we should stop at the point
float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN;
if (!always_stop_at_destination && loiter_duration == 0) {
next_leg_bearing_cd = mission.get_next_ground_course_cd(MODE_NEXT_HEADING_UNKNOWN);
}
// retrieve and sanitize target location
Location cmdloc = cmd.content.location;
location_sanitize(rover.current_loc, cmdloc);
set_desired_location(cmdloc, next_leg_bearing_cd);
}
