SITL: Add rover support for Airsim

libraries/SITL/SIM_AirSim.cpp

@@ -34,7 +34,16 @@ AirSim::AirSim(const char *frame_str) :
 	Aircraft(frame_str),
 	sock(true)
 {
-	printf("Starting SITL Airsim\n");
+    if (strstr(frame_str, "-copter")) {
+        output_type = OutputType::Copter;
+    } else if (strstr(frame_str, "-rover")) {
+        output_type = OutputType::Rover;
+    } else {
+        // default to copter
+        output_type = OutputType::Copter;
+    }
+
+	printf("Starting SITL Airsim type %u\n", (unsigned)output_type);
 }
 
 /*
@@ -61,9 +70,9 @@ void AirSim::set_interface_ports(const char* address, const int port_in, const i
 /*
 	Decode and send servos
 */
-void AirSim::send_servos(const struct sitl_input &input)
+void AirSim::output_copter(const struct sitl_input &input)
 {
-	servo_packet pkt{0};
+    servo_packet pkt;
 
 	for (uint8_t i=0; i<kArduCopterRotorControlCount; i++) {
 		pkt.pwm[i] = input.servos[i];
@@ -80,6 +89,24 @@ void AirSim::send_servos(const struct sitl_input &input)
 	}
 }
 
+void AirSim::output_rover(const struct sitl_input &input)
+{
+    rover_packet pkt;
+
+    pkt.steering = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
+    pkt.throttle = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
+
+    ssize_t send_ret = sock.sendto(&pkt, sizeof(pkt), airsim_ip, airsim_control_port);
+    if (send_ret != sizeof(pkt)) {
+        if (send_ret <= 0) {
+            printf("Unable to send control output to %s:%u - Error: %s, Return value: %ld\n",
+                     airsim_ip, airsim_control_port, strerror(errno), (long)send_ret);
+        } else {
+            printf("Sent %ld bytes instead of %lu bytes\n", (long)send_ret, (unsigned long)sizeof(pkt));
+        }
+    }
+}
+
 /*
   very simple JSON parser for sensor data
   called with pointer to one row of sensor data, nul terminated
@@ -320,7 +347,16 @@ void AirSim::recv_fdm()
 */
 void AirSim::update(const struct sitl_input &input)
 {
-	send_servos(input);
+    switch (output_type) {
+        case OutputType::Copter:
+            output_copter(input);
+            break;
+
+        case OutputType::Rover:
+            output_rover(input);
+            break;
+    }
+
     recv_fdm();
 
     // update magnetic field

libraries/SITL/SIM_AirSim.h

@@ -43,12 +43,21 @@ public:
     void set_interface_ports(const char* address, const int port_in, const int port_out) override;
 
 private:
-	/*
-		rotor control packet sent by Ardupilot
-	*/
-	static const int kArduCopterRotorControlCount = 11;
+    enum class OutputType {
+        Copter = 1,
+        Rover = 2
+    } output_type;
+
+    // Control packet for Rover
+    struct rover_packet {
+        float throttle;     // -1 to 1
+        float steering;     // -1 to 1
+    };
+
+    // rotor control packet sent by Ardupilot
+    static const int kArduCopterRotorControlCount = 11;
 
-	struct servo_packet {
+    struct servo_packet {
 		uint16_t pwm[kArduCopterRotorControlCount];
 	};
 
@@ -68,8 +77,9 @@ private:
     uint64_t last_frame_count;
     uint64_t last_timestamp;
 
-	void send_servos(const struct sitl_input &input);
-	void recv_fdm();
+    void output_copter(const struct sitl_input &input);
+    void output_rover(const struct sitl_input &input);
+    void recv_fdm();
     void report_FPS(void);
 
 	bool parse_sensors(const char *json);