AP_MotorsUGV: positive steering always rotates vehicle right

also scale steering down with increased speed for regular rovers
add support for disabling scaling of steering

APMrover2/AP_MotorsUGV.cpp

@@ -12,6 +12,7 @@
  */
 
 #include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_Math.h>
 #include "SRV_Channel/SRV_Channel.h"
 #include "AP_MotorsUGV.h"
 #include "Rover.h"
@@ -156,9 +157,12 @@ void AP_MotorsUGV::setup_servo_output()
 }
 
 // set steering as a value from -4500 to +4500
-void AP_MotorsUGV::set_steering(float steering)
+//   apply_scaling should be set to false for manual modes where
+//   no scaling by speed or angle should be performed
+void AP_MotorsUGV::set_steering(float steering, bool apply_scaling)
 {
     _steering = constrain_float(steering, -4500.0f, 4500.0f);
+    _scale_steering = apply_scaling;
 }
 
 // set throttle as a value from -100 to 100
@@ -196,7 +200,7 @@ bool AP_MotorsUGV::is_omni_rover() const
     return false;
 }
 
-void AP_MotorsUGV::output(bool armed, float dt)
+void AP_MotorsUGV::output(bool armed, float ground_speed, float dt)
 {
     // soft-armed overrides passed in armed status
     if (!hal.util->get_soft_armed()) {
@@ -214,7 +218,7 @@ void AP_MotorsUGV::output(bool armed, float dt)
     slew_limit_throttle(dt);
 
     // output for regular steering/throttle style frames
-    output_regular(armed, _steering, _throttle);
+    output_regular(armed, ground_speed, _steering, _throttle);
 
     // output for omni style frames
     output_omni(armed, _steering, _throttle);
@@ -399,15 +403,38 @@ void AP_MotorsUGV::setup_pwm_type()
 }
 
 // output to regular steering and throttle channels
-void AP_MotorsUGV::output_regular(bool armed, float steering, float throttle)
+void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering, float throttle)
 {
     // output to throttle channels
     if (armed) {
-        // vectored thrust handling
+        if (_scale_steering) {
-        if (have_vectored_thrust() && (fabsf(throttle) > _vector_throttle_base)) {
+            // vectored thrust handling
-            // scale steering down linearly as throttle increases above _vector_throttle_base
+            if (have_vectored_thrust()) {
-            const float steering_scalar = constrain_float(_vector_throttle_base / fabsf(throttle), 0.0f, 1.0f);
+                if (fabsf(throttle) > _vector_throttle_base) {
-            steering *= steering_scalar;
+                    // scale steering down linearly as throttle increases above _vector_throttle_base
+                    steering *= constrain_float(_vector_throttle_base / fabsf(throttle), 0.0f, 1.0f);
+                }
+            } else {
+                // scale steering down as speed increase above 1m/s
+                if (fabsf(ground_speed) > 1.0f) {
+                    steering *= (1.0f / fabsf(ground_speed));
+                } else {
+                    // regular steering rover at low speed so set limits to stop I-term build-up in controllers
+                    if (!have_skid_steering()) {
+                        limit.steer_left = true;
+                        limit.steer_right = true;
+                    }
+                }
+                // reverse steering output if backing up
+                if (is_negative(ground_speed)) {
+                    steering *= -1.0f;
+                }
+            }
+        } else {
+            // reverse steering direction when backing up
+            if (is_negative(throttle)) {
+                steering *= -1.0f;
+            }
         }
         output_throttle(SRV_Channel::k_throttle, throttle);
     } else {
@@ -439,12 +466,12 @@ void AP_MotorsUGV::output_omni(bool armed, float steering, float throttle)
         const float magnitude = safe_sqrt((scaled_throttle*scaled_throttle)+(1500*1500));
         const float theta = atan2f(scaled_throttle,1500);
 
-        // calculate X and Y vectors using the following the equations: vx = cos(θ) * magnitude and vy = sin(θ) * magnitude
+        // calculate X and Y vectors using the following the equations: vx = cos(?) * magnitude and vy = sin(?) * magnitude
         const float Vx = -(cosf(theta)*magnitude);
         const float Vy = -(sinf(theta)*magnitude);
 
         // calculate output throttle for each motor and scale it back to a -100 to 100 range
-        // calculations are done using the following equations: MOTOR1 = –vx, MOTOR2 = 0.5 * v – √(3/2) * vy, MOTOR 3 = 0.5 * vx + √(3/2) * vy
+        // calculations are done using the following equations: MOTOR1 = –vx, MOTOR2 = 0.5 * v – v(3/2) * vy, MOTOR 3 = 0.5 * vx + v(3/2) * vy
         // safe_sqrt((3)/2) used because the motors are 120 degrees apart in the frame, this setup is mandatory
         const int16_t motor_1 = (((-Vx) + scaled_steering) - (2500)) * (100 - (-100)) / (3500 - (2500)) + (-100);
         const int16_t motor_2 = ((((0.5*Vx)-((safe_sqrt(3)/2)*Vy)) + scaled_steering) - (1121)) * (100 - (-100)) / (2973 - (1121)) + (-100);
@@ -502,12 +529,9 @@ void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float thrott
         throttle_scaled = throttle_scaled / saturation_value;
     }
 
-    // reverse steering direction if throttle is negative to mimic regular rovers
-    const float steering_dir = is_negative(throttle_scaled) ? -1.0f : 1.0f;
-
     // add in throttle and steering
-    const float motor_left = throttle_scaled + (steering_dir * steering_scaled);
+    const float motor_left = throttle_scaled + steering_scaled;
-    const float motor_right = throttle_scaled - (steering_dir * steering_scaled);
+    const float motor_right = throttle_scaled - steering_scaled;
 
     // send pwm value to each motor
     output_throttle(SRV_Channel::k_throttleLeft, 100.0f * motor_left);

APMrover2/AP_MotorsUGV.h

@@ -35,8 +35,10 @@ public:
     void setup_servo_output();
 
     // get or set steering as a value from -4500 to +4500
+    //   apply_scaling should be set to false for manual modes where
+    //   no scaling by speed or angle should e performed
     float get_steering() const { return _steering; }
-    void set_steering(float steering);
+    void set_steering(float steering, bool apply_scaling = true);
 
     // get or set throttle as a value from -100 to 100
     float get_throttle() const { return _throttle; }
@@ -52,7 +54,9 @@ public:
     bool have_vectored_thrust() const { return is_positive(_vector_throttle_base); }
 
     // output to motors and steering servos
-    void output(bool armed, float dt);
+    // ground_speed should be the vehicle's speed over the surface in m/s
+    // dt should be expected time between calls to this function
+    void output(bool armed, float ground_speed, float dt);
 
     // test steering or throttle output as a percentage of the total (range -100 to +100)
     // used in response to DO_MOTOR_TEST mavlink command
@@ -84,7 +88,7 @@ protected:
     void setup_pwm_type();
 
     // output to regular steering and throttle channels
-    void output_regular(bool armed, float steering, float throttle);
+    void output_regular(bool armed, float ground_speed, float steering, float throttle);
 
     // output for omni style frames
     void output_omni(bool armed, float steering, float throttle);
@@ -121,4 +125,5 @@ protected:
     float   _steering;  // requested steering as a value from -4500 to +4500
     float   _throttle;  // requested throttle as a value from -100 to 100
     float   _throttle_prev; // throttle input from previous iteration
+    bool    _scale_steering = true; // true if we should scale steering by speed or angle
 };