Rover: move avoidance into calc_throttle

also enable avoidance for steering and guided modes

APMrover2/mode.cpp

@@ -169,13 +169,21 @@ void Mode::set_desired_speed_to_default(bool rtl)
     _desired_speed = get_speed_default(rtl);
 }
 
-void Mode::calc_throttle(float target_speed, bool nudge_allowed)
+void Mode::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_enabled)
 {
     // add in speed nudging
     if (nudge_allowed) {
         target_speed = calc_speed_nudge(target_speed, g.speed_cruise, g.throttle_cruise * 0.01f);
     }
 
+    // get acceleration limited target speed
+    target_speed = attitude_control.get_desired_speed_accel_limited(target_speed);
+
+    // apply object avoidance to desired speed using half vehicle's maximum acceleration/deceleration
+    if (avoidance_enabled) {
+        g2.avoid.adjust_speed(0.0f, 0.5f * attitude_control.get_accel_max(), ahrs.yaw, target_speed, rover.G_Dt);
+    }
+
     // call throttle controller and convert output to -100 to +100 range
     float throttle_out;
 

APMrover2/mode.h

@@ -116,7 +116,7 @@ protected:
 
     // calculates the amount of throttle that should be output based
     // on things like proximity to corners and current speed
-    virtual void calc_throttle(float target_speed, bool nudge_allowed = true);
+    virtual void calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_enabled);
 
     // performs a controlled stop. returns true once vehicle has stopped
     bool stop_vehicle();
@@ -194,7 +194,7 @@ public:
 
     // methods that affect movement of the vehicle in this mode
     void update() override;
-    void calc_throttle(float target_speed, bool nudge_allowed = true);
+    void calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_enabled);
 
     // attributes of the mode
     bool is_autopilot_mode() const override { return true; }

APMrover2/mode_acro.cpp

@@ -35,10 +35,7 @@ void ModeAcro::update()
         // convert pilot throttle input to desired speed
         float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
 
-        // apply object avoidance to desired speed using half vehicle's maximum acceleration/deceleration
+        calc_throttle(target_speed, false, true);
-        rover.g2.avoid.adjust_speed(0.0f, 0.5f * attitude_control.get_accel_max(), ahrs.yaw, target_speed, rover.G_Dt);
-
-        calc_throttle(target_speed, false);
     }
 }
 

APMrover2/mode_auto.cpp

@@ -56,7 +56,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);
+                calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, false);
             } else {
                 // we have reached the destination so stop
                 stop_vehicle();
@@ -69,7 +69,7 @@ void ModeAuto::update()
             if (!_reached_heading) {
                 // run steering and throttle controllers
                 calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
-                calc_throttle(_desired_speed, true);
+                calc_throttle(_desired_speed, true, false);
                 // check if we have reached within 5 degrees of target
                 _reached_heading = (fabsf(_desired_yaw_cd - ahrs.yaw_sensor) < 500);
             } else {
@@ -197,12 +197,12 @@ bool ModeAuto::check_trigger(void)
     return false;
 }
 
-void ModeAuto::calc_throttle(float target_speed, bool nudge_allowed)
+void ModeAuto::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_enabled)
 {
     // If not autostarting set the throttle to minimum
     if (!check_trigger()) {
         stop_vehicle();
         return;
     }
-    Mode::calc_throttle(target_speed, nudge_allowed);
+    Mode::calc_throttle(target_speed, nudge_allowed, avoidance_enabled);
 }

APMrover2/mode_guided.cpp

@@ -31,7 +31,7 @@ void ModeGuided::update()
             if (!_reached_destination || (rover.is_boat() && !near_wp)) {
                 // drive towards 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, true);
             } else {
                 stop_vehicle();
             }
@@ -48,7 +48,7 @@ void ModeGuided::update()
             if (have_attitude_target) {
                 // run steering and throttle controllers
                 calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
-                calc_throttle(_desired_speed, true);
+                calc_throttle(_desired_speed, true, true);
             } else {
                 stop_vehicle();
                 g2.motors.set_steering(0.0f);
@@ -67,7 +67,7 @@ void ModeGuided::update()
                 // run steering and throttle controllers
                 float steering_out = attitude_control.get_steering_out_rate(radians(_desired_yaw_rate_cds / 100.0f), g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, _desired_speed < 0);
                 g2.motors.set_steering(steering_out * 4500.0f);
-                calc_throttle(_desired_speed, true);
+                calc_throttle(_desired_speed, true, true);
             } else {
                 stop_vehicle();
                 g2.motors.set_steering(0.0f);

APMrover2/mode_rtl.cpp

@@ -35,7 +35,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);
-        calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
+        calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, false);
     } else {
         // we've reached destination so stop
         stop_vehicle();

APMrover2/mode_smart_rtl.cpp

@@ -68,7 +68,7 @@ void ModeSmartRTL::update()
             }
             // continue driving towards destination
             calc_steering_to_waypoint(_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, false);
             break;
 
         case SmartRTL_StopAtHome:
@@ -77,7 +77,7 @@ void ModeSmartRTL::update()
             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);
+                calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, false);
             } else {
                 // rovers stop
                 stop_vehicle();

APMrover2/mode_steering.cpp

@@ -54,5 +54,5 @@ void ModeSteering::update()
     calc_steering_from_lateral_acceleration(desired_lat_accel, reversed);
 
     // run speed to throttle controller
-    calc_throttle(target_speed, false);
+    calc_throttle(target_speed, false, true);
 }