From 17af4c89334751ba1b957823d74f5124b429e26c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 10 Dec 2018 11:43:43 +0900 Subject: [PATCH] Rover: enable avoidance in auto, RTL, smartRTL --- APMrover2/mode_auto.cpp | 4 ++-- APMrover2/mode_rtl.cpp | 2 +- APMrover2/mode_smart_rtl.cpp | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/APMrover2/mode_auto.cpp b/APMrover2/mode_auto.cpp index e7ef862287..7c0f256b5b 100644 --- a/APMrover2/mode_auto.cpp +++ b/APMrover2/mode_auto.cpp @@ -53,7 +53,7 @@ void ModeAuto::update() if (!_reached_destination || (rover.is_boat() && !near_wp)) { // continue driving towards destination 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, false); + calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true); } else { // we have reached the destination so stop stop_vehicle(); @@ -66,7 +66,7 @@ void ModeAuto::update() if (!_reached_heading) { // run steering and throttle controllers calc_steering_to_heading(_desired_yaw_cd); - calc_throttle(_desired_speed, true, false); + calc_throttle(_desired_speed, true, true); // check if we have reached within 5 degrees of target _reached_heading = (fabsf(_desired_yaw_cd - ahrs.yaw_sensor) < 500); } else { diff --git a/APMrover2/mode_rtl.cpp b/APMrover2/mode_rtl.cpp index afeec521ae..6ba23ef4b7 100644 --- a/APMrover2/mode_rtl.cpp +++ b/APMrover2/mode_rtl.cpp @@ -37,7 +37,7 @@ void ModeRTL::update() if (!_reached_destination || (rover.is_boat() && !near_wp)) { // continue driving towards destination 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, false); + calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true); } else { // we've reached destination so stop stop_vehicle(); diff --git a/APMrover2/mode_smart_rtl.cpp b/APMrover2/mode_smart_rtl.cpp index 295385a143..56a6597810 100644 --- a/APMrover2/mode_smart_rtl.cpp +++ b/APMrover2/mode_smart_rtl.cpp @@ -65,7 +65,7 @@ void ModeSmartRTL::update() } // continue driving towards destination calc_steering_to_waypoint(_origin, _destination, _reversed); - calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, false); + calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true); break; case SmartRTL_StopAtHome: @@ -74,7 +74,7 @@ void ModeSmartRTL::update() if (rover.is_boat()) { // boats attempt to hold position at home calc_steering_to_waypoint(rover.current_loc, _destination, _reversed); - calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, false); + calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true); } else { // rovers stop stop_vehicle();