Rover: add support for lateral control input

APMrover2/AP_MotorsUGV.cpp

@@ -177,6 +177,12 @@ void AP_MotorsUGV::set_throttle(float throttle)
     _throttle = constrain_float(throttle, -_throttle_max, _throttle_max);
 }
 
+// set lateral input as a value from -100 to +100
+void AP_MotorsUGV::set_lateral(float lateral)
+{
+    _lateral = constrain_float(lateral, -100.0f, 100.0f);
+}
+
 // get slew limited throttle
 // used by manual mode to avoid bad steering behaviour during transitions from forward to reverse
 // same as private slew_limit_throttle method (see below) but does not update throttle state
@@ -202,8 +208,8 @@ bool AP_MotorsUGV::have_skid_steering() const
     return false;
 }
 
-// returns true if omni rover
+// returns true if vehicle is capable of lateral movement
-bool AP_MotorsUGV::is_omni_rover() const
+bool AP_MotorsUGV::has_lateral_control() const
 {
     if (SRV_Channels::function_assigned(SRV_Channel::k_motor1) &&
         SRV_Channels::function_assigned(SRV_Channel::k_motor2) &&
@@ -234,7 +240,7 @@ void AP_MotorsUGV::output(bool armed, float ground_speed, float dt)
     output_regular(armed, ground_speed, _steering, _throttle);
 
     // output for omni style frames
-    output_omni(armed, _steering, _throttle);
+    output_omni(armed, _steering, _throttle, _lateral);
 
     // output for skid steering style frames
     output_skid_steering(armed, _steering, _throttle);
@@ -464,36 +470,42 @@ void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering
 }
 
 // output for omni style frames
-void AP_MotorsUGV::output_omni(bool armed, float steering, float throttle)
+void AP_MotorsUGV::output_omni(bool armed, float steering, float throttle, float lateral)
 {
-    if (!is_omni_rover()) {
+    if (!has_lateral_control()) {
         return;
     }
     if (armed) {
-        // scale throttle and steering to a 1000 to 2000 range for motor throttle calculations
+        // scale throttle, steering and lateral to -1 ~ 1
-        const float scaled_throttle = (throttle - (100)) * (2000 - 1000) / (-100 - (100)) + 1000;
+        const float scaled_throttle = throttle / 100.0f;
-        const float scaled_steering = (steering - (-4500)) * (2000 - 1000) / (4500 - (-4500)) + 1000;
+        const float scaled_steering = steering / 4500.0f;
+        const float scaled_lateral = lateral / 100.0f;
 
         // calculate desired vehicle speed and direction
-        // 1500 is a place-holder value for lateral movement input
+        const float magnitude = safe_sqrt((scaled_throttle*scaled_throttle)+(scaled_lateral*scaled_lateral));
-        const float magnitude = safe_sqrt((scaled_throttle*scaled_throttle)+(1500*1500));
+        const float theta = atan2f(scaled_throttle,scaled_lateral);
-        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(theta) * magnitude and vy = sin(theta) * 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
+        // calculate output throttle for each motor. Output is multiplied by 0.5 to bring the range generally within -1 ~ 1
-        // 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
+        // First wheel (motor 1) moves only parallel to x-axis so only X component is taken. Normal range is -2 ~ 2 with the steering
+        // motor_2 and motor_3 utilizes both X and Y components.
         // 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);
+        float motor_1 = 0.5 * ((-Vx) + scaled_steering);
-        const int16_t motor_2 = ((((0.5*Vx)-((safe_sqrt(3)/2)*Vy)) + scaled_steering) - (1121)) * (100 - (-100)) / (2973 - (1121)) + (-100);
+        float motor_2 = 0.5 * (((0.5*Vx)-((safe_sqrt(3)/2)*Vy)) + scaled_steering);
-        const int16_t motor_3 = ((((0.5*Vx)+((safe_sqrt(3)/2)*Vy)) + scaled_steering) - (-1468)) * (100 - (-100)) / (383 - (-1468)) + (-100);
+        float motor_3 = 0.5 * (((0.5*Vx)+((safe_sqrt(3)/2)*Vy)) + scaled_steering);
 
-        // send pwm value to each motor
+        // apply constraints
-        output_throttle(SRV_Channel::k_motor1, motor_1);
+        motor_1 = constrain_float(motor_1, -1.0f, 1.0f);
-        output_throttle(SRV_Channel::k_motor2, motor_2);
+        motor_2 = constrain_float(motor_2, -1.0f, 1.0f);
-        output_throttle(SRV_Channel::k_motor3, motor_3);
+        motor_3 = constrain_float(motor_3, -1.0f, 1.0f);
+
+        // scale back and send pwm value to each motor
+        output_throttle(SRV_Channel::k_motor1, 100.0f * motor_1);
+        output_throttle(SRV_Channel::k_motor2, 100.0f * motor_2);
+        output_throttle(SRV_Channel::k_motor3, 100.0f * motor_3);
     } else {
         // handle disarmed case
         if (_disarm_disable_pwm) {

APMrover2/AP_MotorsUGV.h

@@ -44,6 +44,9 @@ public:
     float get_throttle() const { return _throttle; }
     void set_throttle(float throttle);
 
+    // set lateral input as a value from -100 to +100
+    void set_lateral(float lateral);
+
     // get slew limited throttle
     // used by manual mode to avoid bad steering behaviour during transitions from forward to reverse
     // same as private slew_limit_throttle method (see below) but does not update throttle state
@@ -52,8 +55,8 @@ public:
     // true if vehicle is capable of skid steering
     bool have_skid_steering() const;
 
-    //true if vehicle is an omni rover
+    //true if vehicle is capable of lateral movement
-    bool is_omni_rover() const;
+    bool has_lateral_control() const;
 
     // true if vehicle has vectored thrust (i.e. boat with motor on steering servo)
     bool have_vectored_thrust() const { return is_positive(_vector_throttle_base); }
@@ -96,7 +99,7 @@ protected:
     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);
+    void output_omni(bool armed, float steering, float throttle, float lateral);
 
     // output to skid steering channels
     void output_skid_steering(bool armed, float steering, float throttle);
@@ -131,4 +134,5 @@ protected:
     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
+    float   _lateral;  // requested lateral input as a value from -4500 to +4500
 };

APMrover2/Rover.cpp

@@ -27,6 +27,7 @@ Rover::Rover(void) :
     channel_steer(nullptr),
     channel_throttle(nullptr),
     channel_aux(nullptr),
+    channel_lateral(nullptr),
     DataFlash{fwver.fw_string, g.log_bitmask},
     modes(&g.mode1),
     nav_controller(&L1_controller),

APMrover2/Rover.h

@@ -155,6 +155,7 @@ private:
     RC_Channel *channel_steer;
     RC_Channel *channel_throttle;
     RC_Channel *channel_aux;
+    RC_Channel *channel_lateral;
 
     DataFlash_Class DataFlash;
 

APMrover2/mode.cpp

@@ -7,6 +7,7 @@ Mode::Mode() :
     g2(rover.g2),
     channel_steer(rover.channel_steer),
     channel_throttle(rover.channel_throttle),
+    channel_lateral(rover.channel_lateral),
     mission(rover.mission),
     attitude_control(rover.g2.attitude_control)
 { }
@@ -124,6 +125,19 @@ void Mode::get_pilot_desired_steering_and_speed(float &steering_out, float &spee
     speed_out = speed_out_limited;
 }
 
+// decode pilot lateral movement input and return in lateral_out argument
+void Mode::get_pilot_desired_lateral(float &lateral_out)
+{
+    // no RC input means no lateral input
+    if (rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
+        lateral_out = 0;
+        return;
+    }
+
+    // get pilot lateral input
+    lateral_out = rover.channel_lateral->get_control_in();
+}
+
 // set desired location
 void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
 {

APMrover2/mode.h

@@ -112,6 +112,9 @@ protected:
     // decode pilot input steering and return steering_out and speed_out (in m/s)
     void get_pilot_desired_steering_and_speed(float &steering_out, float &speed_out);
 
+    // decode pilot lateral movement input and return in lateral_out argument
+    void get_pilot_desired_lateral(float &lateral_out);
+
     // calculate steering output to drive along line from origin to destination waypoint
     void calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed = false);
 
@@ -159,6 +162,7 @@ protected:
     class ParametersG2 &g2;
     class RC_Channel *&channel_steer;
     class RC_Channel *&channel_throttle;
+    class RC_Channel *&channel_lateral;
     class AP_Mission &mission;
     class AR_AttitudeControl &attitude_control;
 
@@ -352,6 +356,10 @@ public:
     // manual mode does not require position or velocity estimate
     bool requires_position() const override { return false; }
     bool requires_velocity() const override { return false; }
+
+protected:
+
+    void _exit() override;
 };
 
 

APMrover2/mode_manual.cpp

@@ -1,12 +1,20 @@
 #include "mode.h"
 #include "Rover.h"
 
+void ModeManual::_exit()
+{
+    // clear lateral when exiting manual mode
+    g2.motors.set_lateral(0);
+}
+
 void ModeManual::update()
 {
-    float desired_steering, desired_throttle;
+    float desired_steering, desired_throttle, desired_lateral;
     get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
+    get_pilot_desired_lateral(desired_lateral);
 
     // copy RC scaled inputs to outputs
     g2.motors.set_throttle(desired_throttle);
     g2.motors.set_steering(desired_steering, false);
+    g2.motors.set_lateral(desired_lateral);
 }

APMrover2/radio.cpp

@@ -9,10 +9,12 @@ void Rover::set_control_channels(void)
     channel_steer    = RC_Channels::rc_channel(rcmap.roll()-1);
     channel_throttle = RC_Channels::rc_channel(rcmap.throttle()-1);
     channel_aux      = RC_Channels::rc_channel(g.aux_channel-1);
+    channel_lateral  = RC_Channels::rc_channel(rcmap.yaw()-1);
 
     // set rc channel ranges
     channel_steer->set_angle(SERVO_MAX);
     channel_throttle->set_angle(100);
+    channel_lateral->set_angle(100);
 
     // Allow to reconfigure ouput when not armed
     if (!arming.is_armed()) {
@@ -32,6 +34,7 @@ void Rover::init_rc_in()
     // set rc dead zones
     channel_steer->set_default_dead_zone(30);
     channel_throttle->set_default_dead_zone(30);
+    channel_lateral->set_default_dead_zone(30);
 }
 
 void Rover::init_rc_out()