Rover: initial implementation of skid steering for APMrover2

this adds SKID_STEER_IN and SKID_STEER_OUT parameters for controlling
skid skeering control and output

APMrover2/Parameters.h

@@ -88,6 +88,8 @@ public:
         k_param_throttle_cruise,
         k_param_throttle_slewrate,
         k_param_throttle_reduction,
+        k_param_skid_steer_in,
+        k_param_skid_steer_out,
 
         // failsafe control
         k_param_fs_action = 180,
@@ -192,6 +194,8 @@ public:
     AP_Int8     throttle_max;
     AP_Int8     throttle_cruise;
     AP_Int8     throttle_slewrate;
+    AP_Int8     skid_steer_in;
+    AP_Int8     skid_steer_out;
 
     // failsafe control
     AP_Int8     fs_action;

APMrover2/Parameters.pde

@@ -208,6 +208,20 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
 	GSCALAR(throttle_slewrate,      "THR_SLEWRATE",     0),
 
+    // @Param: SKID_STEER_OUT
+    // @DisplayName: Skid steering output
+    // @Description: Set this to 1 for skid steering controlled rovers (tank track style). When enabled, servo1 is used for the left track control, servo3 is used for right track control
+    // @Values: 0:Disabled, 1:SkidSteeringOutput
+    // @User: Standard
+	GSCALAR(skid_steer_out,          "SKID_STEER_OUT",     0),
+
+    // @Param: SKID_STEER_IN
+    // @DisplayName: Skid steering input
+    // @Description: Set this to 1 for skid steering input rovers (tank track style in RC controller). When enabled, servo1 is used for the left track control, servo3 is used for right track control
+    // @Values: 0:Disabled, 1:SkidSteeringOutput
+    // @User: Standard
+	GSCALAR(skid_steer_in,           "SKID_STEER_IN",     0),
+
     // @Param: FS_ACTION
     // @DisplayName: Failsafe Action
     // @Description: What to do on a failsafe event

APMrover2/Steering.pde

@@ -91,10 +91,11 @@ static void set_servos(void)
 {
     int16_t last_throttle = g.channel_throttle.radio_out;
 
-	if (control_mode == MANUAL || control_mode == LEARNING) {
-		// do a direct pass through of radio values
-		g.channel_steer.radio_out 		= g.channel_steer.radio_in;
-		g.channel_throttle.radio_out 	= g.channel_throttle.radio_in;
+	if ((control_mode == MANUAL || control_mode == LEARNING) &&
+        (g.skid_steer_out == g.skid_steer_in)) {
+        // do a direct pass through of radio values
+        g.channel_steer.radio_out       = hal.rcin->read(CH_STEER);
+        g.channel_throttle.radio_out    = hal.rcin->read(CH_THROTTLE);
 	} else {       
         g.channel_steer.calc_pwm();
 		g.channel_throttle.servo_out = constrain_int16(g.channel_throttle.servo_out, 
@@ -105,6 +106,25 @@ static void set_servos(void)
 
         // limit throttle movement speed
         throttle_slew_limit(last_throttle);
+
+        if (g.skid_steer_out) {
+            // convert the two radio_out values to skid steering values
+            /*
+              mixing rule:
+              steering = motor1 - motor2
+              throttle = 0.5*(motor1 + motor2)
+              motor1 = throttle + 0.5*steering
+              motor2 = throttle - 0.5*steering
+            */          
+            float steering_scaled = g.channel_steer.norm_output();
+            float throttle_scaled = g.channel_throttle.norm_output();
+            float motor1 = throttle_scaled + 0.5*steering_scaled;
+            float motor2 = throttle_scaled - 0.5*steering_scaled;
+            g.channel_steer.servo_out = 4500*motor1;
+            g.channel_throttle.servo_out = 100*motor2;
+            g.channel_steer.calc_pwm();
+            g.channel_throttle.calc_pwm();
+        }
     }
 
 

APMrover2/defines.h

@@ -52,13 +52,8 @@ enum ch7_option {
 #define GPS_PROTOCOL_MTK19	6
 #define GPS_PROTOCOL_AUTO	7
 
-#define CH_ROLL CH_1
-#define CH_PITCH CH_2
+#define CH_STEER    CH_1
 #define CH_THROTTLE CH_3
-#define CH_RUDDER CH_4
-#define CH_YAW CH_4
-#define CH_AIL1 CH_5
-#define CH_AIL2 CH_6
 
 // HIL enumerations
 #define HIL_MODE_DISABLED			0

APMrover2/radio.pde

@@ -54,7 +54,7 @@ static void init_rc_out()
 
 static void read_radio()
 {
-    g.channel_steer.set_pwm(hal.rcin->read(CH_ROLL));
+    g.channel_steer.set_pwm(hal.rcin->read(CH_STEER));
 
 	g.channel_throttle.set_pwm(hal.rcin->read(CH_3));
   	g.rc_5.set_pwm(hal.rcin->read(CH_5));
@@ -72,13 +72,35 @@ static void read_radio()
 		throttle_nudge = 0;
 	}
 
-	/*
-	cliSerial->printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"),
-				g.rc_1.control_in,
-				g.rc_2.control_in,
-				g.rc_3.control_in,
-				g.rc_4.control_in);
-	*/
+    if (g.skid_steer_in) {
+        // convert the two radio_in values from skid steering values
+        /*
+          mixing rule:
+          steering = motor1 - motor2
+          throttle = 0.5*(motor1 + motor2)
+          motor1 = throttle + 0.5*steering
+          motor2 = throttle - 0.5*steering
+        */          
+
+        float motor1 = g.channel_steer.norm_input();
+        float motor2 = g.channel_throttle.norm_input();
+        float steering_scaled = motor2 - motor1;
+        float throttle_scaled = 0.5f*(motor1 + motor2);
+        int16_t steer = g.channel_steer.radio_trim;
+        int16_t thr   = g.channel_throttle.radio_trim;
+        if (steering_scaled > 0.0f) {
+            steer += steering_scaled*(g.channel_steer.radio_max-g.channel_steer.radio_trim);
+        } else {
+            steer += steering_scaled*(g.channel_steer.radio_trim-g.channel_steer.radio_min);
+        }
+        if (throttle_scaled > 0.0f) {
+            thr += throttle_scaled*(g.channel_throttle.radio_max-g.channel_throttle.radio_trim);
+        } else {
+            thr += throttle_scaled*(g.channel_throttle.radio_trim-g.channel_throttle.radio_min);
+        }
+        g.channel_steer.set_pwm(steer);
+        g.channel_throttle.set_pwm(thr);
+    }
 }
 
 static void control_failsafe(uint16_t pwm)