Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: boats always navigate when outside waypoint radius

This commit is contained in:
Randy Mackay 2018-02-05 15:39:48 +09:00
parent e6d4b2a087
commit 0da8ff6b2e
3 changed files with 26 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
APMrover2/mode_auto.cpp
View File

@ -46,14 +46,14 @@ void ModeAuto::update()
case Auto_WP: case Auto_WP:
{ {
_distance_to_destination = get_distance(rover.current_loc, _destination); _distance_to_destination = get_distance(rover.current_loc, _destination);
const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius;
// check if we've reached the destination // check if we've reached the destination
if (!_reached_destination) { if (!_reached_destination && (near_wp || location_passed_point(rover.current_loc, _origin, _destination))) {
if (_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;
}
} }
if (!_reached_destination || rover.is_boat()) { // determine if we should keep navigating
if (!_reached_destination || (rover.is_boat() && !near_wp)) {
// continue driving towards destination // continue driving towards destination
calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination, _reversed); calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination, _reversed);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true); calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true);

16
APMrover2/mode_guided.cpp
View File

@ -20,13 +20,15 @@ void ModeGuided::update()
switch (_guided_mode) { switch (_guided_mode) {
case Guided_WP: case Guided_WP:
{ {
if (!_reached_destination || rover.is_boat()) { _distance_to_destination = get_distance(rover.current_loc, _destination);
// check if we've reached the destination const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius;
_distance_to_destination = get_distance(rover.current_loc, _destination); // check if we've reached the destination
if (!_reached_destination && (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination))) { if (!_reached_destination && (near_wp || location_passed_point(rover.current_loc, _origin, _destination))) {
_reached_destination = true; _reached_destination = true;
rover.gcs().send_mission_item_reached_message(0); rover.gcs().send_mission_item_reached_message(0);
} }
// determine if we should keep navigating
if (!_reached_destination || (rover.is_boat() && !near_wp)) {
// drive towards destination // drive towards destination
calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _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);

20
APMrover2/mode_rtl.cpp
View File

@ -22,15 +22,17 @@ bool ModeRTL::_enter()
void ModeRTL::update() void ModeRTL::update()
{ {
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); const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius;
// check if we've reached the destination // check if we've reached the destination
if (!_reached_destination && (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination))) { if (!_reached_destination && (near_wp || 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");
} }
// determine if we should keep navigating
if (!_reached_destination || (rover.is_boat() && !near_wp)) {
// continue driving towards destination // continue driving towards destination
calc_steering_to_waypoint(_reached_destination ? rover.current_loc :_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);