SITL: add rover with vectored thrust support

libraries/SITL/SIM_Rover.cpp

@@ -34,6 +34,12 @@ SimRover::SimRover(const char *frame_str) :
         // with a sabertooth controller
         max_accel = 14;
         max_speed = 4;
+        return;
+    }
+
+    vectored_thrust = strstr(frame_str, "vector") != nullptr;
+    if (vectored_thrust) {
+        printf("Vectored Thrust Rover Simulation Started\n");
     }
 }
 
@@ -58,6 +64,9 @@ float SimRover::calc_yaw_rate(float steering, float speed)
     if (skid_steering) {
         return steering * skid_turn_rate;
     }
+    if (vectored_thrust) {
+        return steering * vectored_turn_rate_max;
+    }
     if (fabsf(steering) < 1.0e-6 or fabsf(speed) < 1.0e-6) {
         return 0;
     }
@@ -94,6 +103,13 @@ void SimRover::update(const struct sitl_input &input)
     } else {
         steering = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
         throttle = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
+
+        // vectored thrust conversion
+        if (vectored_thrust) {
+            const float steering_angle_rad = radians(steering * vectored_angle_max);
+            steering = sinf(steering_angle_rad) * throttle;
+            throttle *= cosf(steering_angle_rad);
+        }
     }
 
     // how much time has passed?

libraries/SITL/SIM_Rover.h

@@ -45,6 +45,11 @@ private:
     float skid_turn_rate = 140.0f;      // skid-steering vehicle's maximum turn rate in deg/sec
     bool skid_steering;                 // true if this vehicle is a skid-steering vehicle
 
+    // vectored thrust related members
+    bool vectored_thrust;                   // true if vehicle uses vectored thrust (i.e. steering controls direction of thrust)
+    float vectored_angle_max = 90.0f;       // maximum angle (in degrees) to which thrust can be turned
+    float vectored_turn_rate_max = 90.0f;   // maximum turn rate (in deg/sec) with full throttle angled at 90deg
+
     float turn_circle(float steering) const;
     float calc_yaw_rate(float steering, float speed);
     float calc_lat_accel(float steering_angle, float speed);