SITL: support for steering/throttle rover

libraries/SITL/SIM_Morse.cpp

@@ -105,14 +105,16 @@ Morse::Morse(const char *frame_str) :
     }
 
     if (strstr(frame_option, "-rover")) {
-        output_type = OUTPUT_ROVER;
+        output_type = OUTPUT_ROVER_REGULAR;
+    } else if (strstr(frame_option, "-skid")) {
+        output_type = OUTPUT_ROVER_SKID;
     } else if (strstr(frame_option, "-quad")) {
         output_type = OUTPUT_QUAD;
     } else if (strstr(frame_option, "-pwm")) {
         output_type = OUTPUT_PWM;
     } else {
         // default to rover
-        output_type = OUTPUT_ROVER;
+        output_type = OUTPUT_ROVER_REGULAR;
     }
 
     for (uint8_t i=0; i<ARRAY_SIZE(sim_defaults); i++) {
@@ -339,10 +341,46 @@ bool Morse::sensors_receive(void)
     return parse_ok;
 }
 
+/*
+  output control command assuming steering/throttle rover
+ */
+void Morse::output_rover_regular(const struct sitl_input &input)
+{
+    float throttle = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
+    float ground_steer = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
+    float max_steer = radians(60);
+    float max_speed = 20;
+    float max_accel = 20;
+
+     // speed in m/s in body frame
+    Vector3f velocity_body = dcm.transposed() * velocity_ef;
+
+    // speed along x axis, +ve is forward
+    float speed = velocity_body.x;
+
+    // target speed with current throttle
+    float target_speed = throttle * max_speed;
+
+    // linear acceleration in m/s/s - very crude model
+    float accel = max_accel * (target_speed - speed) / max_speed;
+
+    //force directly proportion to acceleration 
+    float force = accel;
+
+    float steer = ground_steer * max_steer;
+
+    // construct a JSON packet for steer/force
+    char buf[60];
+    snprintf(buf, sizeof(buf)-1, "{\"steer\": %.3f, \"force\": %.2f, \"brake\": %.2f}\n",
+             steer, -force, 0.0);
+    buf[sizeof(buf)-1] = 0;
+
+    control_sock->send(buf, strlen(buf));
+}
 /*
   output control command assuming skid-steering rover
  */
-void Morse::output_rover(const struct sitl_input &input)
+void Morse::output_rover_skid(const struct sitl_input &input)
 {
     float motor1 = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
     float motor2 = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
@@ -510,8 +548,11 @@ void Morse::update(const struct sitl_input &input)
     update_mag_field_bf();
 
     switch (output_type) {
-    case OUTPUT_ROVER:
-        output_rover(input);
+    case OUTPUT_ROVER_REGULAR:
+        output_rover_regular(input);
+        break;
+    case OUTPUT_ROVER_SKID:
+        output_rover_skid(input);
         break;
     case OUTPUT_QUAD:
         output_quad(input);
@@ -626,4 +667,4 @@ void Morse::send_report(void)
         }
     }
 
-}
+}

libraries/SITL/SIM_Morse.h

@@ -64,15 +64,17 @@ private:
     uint16_t morse_control_port = 60001;
 
     enum {
-        OUTPUT_ROVER=1,
-        OUTPUT_QUAD=2,
-        OUTPUT_PWM=3
+        OUTPUT_ROVER_REGULAR=1,
+        OUTPUT_ROVER_SKID=2,
+        OUTPUT_QUAD=3,
+        OUTPUT_PWM=4
     } output_type;
 
     bool connect_sockets(void);
     bool parse_sensors(const char *json);
     bool sensors_receive(void);
-    void output_rover(const struct sitl_input &input);
+    void output_rover_regular(const struct sitl_input &input);
+    void output_rover_skid(const struct sitl_input &input);
     void output_quad(const struct sitl_input &input);
     void output_pwm(const struct sitl_input &input);
     void report_FPS();