Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: boats stay active at destination in guided, rtl, smartrtl

Browse Source
This commit is contained in:
Randy Mackay 2017-12-07 11:43:13 +09:00
parent 8eb991b610
commit 34c2fba9a7
3 changed files with 18 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
APMrover2/mode_guided.cpp
View File

@ -21,16 +21,15 @@ void ModeGuided::update()
switch (_guided_mode) { switch (_guided_mode) {
case Guided_WP: case Guided_WP:
{ {
if (!_reached_destination) { if (!_reached_destination || rover.is_boat()) {
// check if we've reached the destination // check if we've reached the destination
_distance_to_destination = get_distance(rover.current_loc, _destination); _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)) { if (!_reached_destination && (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination))) {
// trigger reached
_reached_destination = true; _reached_destination = true;
rover.gcs().send_mission_item_reached_message(0); rover.gcs().send_mission_item_reached_message(0);
} }
// drive towards destination // drive towards destination
calc_steering_to_waypoint(_origin, _destination); calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true); calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
} else { } else {
stop_vehicle(); stop_vehicle();

6
APMrover2/mode_rtl.cpp
View File

@ -22,17 +22,17 @@ bool ModeRTL::_enter()
void ModeRTL::update() void ModeRTL::update()
{ {
if (!_reached_destination) { if (!_reached_destination || rover.is_boat()) {
// calculate distance to home // calculate distance to home
_distance_to_destination = get_distance(rover.current_loc, _destination); _distance_to_destination = get_distance(rover.current_loc, _destination);
// check if we've reached the destination // check if we've reached the destination
if (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination)) { if (!_reached_destination && (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination))) {
// trigger reached // trigger reached
_reached_destination = true; _reached_destination = true;
gcs().send_text(MAV_SEVERITY_INFO, "Reached destination"); gcs().send_text(MAV_SEVERITY_INFO, "Reached destination");
} }
// continue driving towards destination // continue driving towards destination
calc_steering_to_waypoint(_origin, _destination); calc_steering_to_waypoint(_reached_destination ? rover.current_loc :_origin, _destination);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true); calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
} else { } else {
// we've reached destination so stop // we've reached destination so stop

13
APMrover2/mode_smart_rtl.cpp
View File

@ -17,6 +17,9 @@ bool ModeSmartRTL::_enter()
// initialise waypoint speed // initialise waypoint speed
set_desired_speed_to_default(true); set_desired_speed_to_default(true);
// init location target
set_desired_location(rover.current_loc);
// RTL never reverses // RTL never reverses
rover.set_reverse(false); rover.set_reverse(false);
@ -53,6 +56,7 @@ void ModeSmartRTL::update()
_load_point = false; _load_point = false;
// set target destination to new point // set target destination to new point
if (!set_desired_location_NED(next_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"); gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination");
smart_rtl_state = SmartRTL_Failure; smart_rtl_state = SmartRTL_Failure;
} }
@ -70,7 +74,14 @@ void ModeSmartRTL::update()
case SmartRTL_StopAtHome: case SmartRTL_StopAtHome:
case SmartRTL_Failure: case SmartRTL_Failure:
_reached_destination = true; _reached_destination = true;
stop_vehicle(); 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);
} else {
// rovers stop
stop_vehicle();
}
break; break;
} }
} }