SITL: Add scrimmage simulator support

libraries/SITL/SIM_Aircraft.h

@@ -103,6 +103,11 @@ public:
 
     float gross_mass() const { return mass + external_payload_mass; }
 
+    virtual void set_config(const char* config) {
+        config_ = config;
+    }
+
+
     const Location &get_location() const { return location; }
 
     const Vector3f &get_position() const { return position; }
@@ -178,6 +183,7 @@ protected:
     const char *frame;
     bool use_time_sync = true;
     float last_speedup = -1.0f;
+    const char *config_ = "";
 
     // allow for AHRS_ORIENTATION
     AP_Int8 *ahrs_orientation;

libraries/SITL/SIM_Scrimmage.cpp

@@ -0,0 +1,216 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  simulator connector for Scrimmage simulator
+*/
+
+#include "SIM_Scrimmage.h"
+
+#include <fcntl.h>
+#include <stdio.h>
+#include <inttypes.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <cstdlib>
+#include <fstream>
+#include <sstream>
+#include <iostream>
+#include <algorithm>
+
+#include <AP_HAL/AP_HAL.h>
+
+extern const AP_HAL::HAL& hal;
+
+namespace SITL {
+
+Scrimmage::Scrimmage(const char *home_str, const char *_frame_str) :
+    Aircraft(home_str, _frame_str),
+    prev_timestamp_us(0),
+    recv_sock(true),
+    send_sock(true),
+    frame_str(_frame_str)
+{
+    // Set defaults for scrimmage-copter
+    if (strcmp(frame_str, "scrimmage-copter")== 0) {
+        mission_name = "arducopter.xml";
+        motion_model = "Multirotor";
+        visual_model = "iris";
+    }
+}
+
+void Scrimmage::set_config(const char *config)
+{
+    // The configuration string is a comma-separated sequence of key:value
+    // pairs
+
+    // Iterate over comma-separated strings and store parameters
+    char *end_str;
+    char* copy_config = strdup(config);
+    char *token = strtok_r(copy_config, ",", &end_str);
+    while (token != NULL)
+    {
+        char *end_token;
+        char *token2 = strtok_r(token, "=", &end_token);
+        if (strcmp(token2, "mission")==0) {
+            mission_name = strtok_r(NULL, "=", &end_token);
+        } else if (strcmp(token2, "motion_model")==0) {
+            motion_model = strtok_r(NULL, "=", &end_token);
+        } else if (strcmp(token2, "visual_model")==0) {
+            visual_model = strtok_r(NULL, "=", &end_token);
+        } else if (strcmp(token2, "terrain")==0) {
+            terrain = strtok_r(NULL, "=", &end_token);
+        } else {
+            printf("Invalid scrimmage param: %s", token2);
+        }
+        token = strtok_r(NULL, ",", &end_str);
+    }
+    free(copy_config);
+
+    // start scrimmage after parsing simulation configuration
+    start_scrimmage();
+}
+
+void Scrimmage::set_interface_ports(const char* address, const int port_in, const int port_out)
+{
+    fdm_port_in = port_in;
+    fdm_port_out = port_out;
+    fdm_address = address;
+    printf("ArduPilot sending to scrimmage on %s:%d\n",fdm_address, fdm_port_out);
+    printf("ArduPilot listening to scrimmage on %s:%d\n",fdm_address, fdm_port_in);
+
+    recv_sock.bind(fdm_address, fdm_port_in);
+
+    recv_sock.reuseaddress();
+    recv_sock.set_blocking(false);
+
+    send_sock.reuseaddress();
+    send_sock.set_blocking(false);
+}
+
+// Start Scrimmage child
+void Scrimmage::start_scrimmage(void)
+{
+    pid_t child_pid = fork();
+    if (child_pid == 0) {
+        // In child
+
+        // Construct the scrimmage command string with overrides for initial
+        // position and heading.
+        char* full_exec_str;
+        int len;
+        // Get required string length
+        len = snprintf(NULL, 0, "xterm +hold -T SCRIMMAGE -e 'scrimmage %s -o \"latitude_origin=%f,"
+        "longitude_origin=%f,altitude_origin=%f,heading=%f,motion_model=%s,visual_model=%s,terrain=%s,"
+        "to_ardupilot_port=%d,from_ardupilot_port=%d,to_ardupilot_ip=%s\"'", mission_name, home.lat*1.0e-7f,home.lng*1.0e-7f,
+        home.alt*1.0e-2f, home_yaw, motion_model, visual_model, terrain, fdm_port_in, fdm_port_out, fdm_address);
+
+        full_exec_str = (char *)malloc(len+1);
+        snprintf(full_exec_str, len+1, "xterm +hold -T SCRIMMAGE -e 'scrimmage %s -o \"latitude_origin=%f,"
+        "longitude_origin=%f,altitude_origin=%f,heading=%f,motion_model=%s,visual_model=%s,terrain=%s,"
+        "to_ardupilot_port=%d,from_ardupilot_port=%d,to_ardupilot_ip=%s\"'", mission_name, home.lat*1.0e-7f,home.lng*1.0e-7f,
+        home.alt*1.0e-2f, home_yaw, motion_model, visual_model, terrain, fdm_port_in, fdm_port_out, fdm_address);
+
+        printf("%s\n", full_exec_str);
+
+        // system call worked
+        int ret = system(full_exec_str);
+
+        if (ret != 0) {
+            std::cerr << "scrimmage didn't open.\n";
+            perror("scrimmage");
+        }
+
+        free(full_exec_str);
+    }
+}
+
+void Scrimmage::send_servos(const struct sitl_input &input)
+{
+    servo_packet pkt;
+
+    for (int i = 0; i < MAX_NUM_SERVOS; i++) {
+        pkt.servos[i] = input.servos[i];
+    }
+    send_sock.sendto(&pkt, sizeof(servo_packet), fdm_address, fdm_port_out);
+}
+
+/*
+  receive an update from the FDM
+  This is a blocking function
+ */
+void Scrimmage::recv_fdm(const struct sitl_input &input)
+{
+    fdm_packet pkt;
+
+    // wait for packet from scrimmage
+    while (recv_sock.recv(&pkt, sizeof(pkt), 100) != sizeof(pkt));
+
+    // auto-adjust to simulation frame rate
+    uint64_t dt_us = 0;
+    if (pkt.timestamp_us > prev_timestamp_us)
+        dt_us = pkt.timestamp_us - prev_timestamp_us;
+    time_now_us += dt_us;
+
+    float dt_inv = 1.0e6 / ((float)dt_us);
+    if ( dt_inv > 100) {
+        adjust_frame_time(dt_inv);
+    }
+    prev_timestamp_us = pkt.timestamp_us;
+
+    // dcm_bl: dcm from body to local frame
+    dcm.from_euler(pkt.roll, pkt.pitch, pkt.yaw);
+    dcm.normalize();
+
+    // subtract gravity to get specific force measuremnt of the IMU
+    accel_body = Vector3f(pkt.xAccel, pkt.yAccel, pkt.zAccel) - dcm.transposed()*Vector3f(0.0f, 0.0f, GRAVITY_MSS);
+    gyro = Vector3f(pkt.rollRate, pkt.pitchRate, pkt.yawRate);
+
+    ang_accel = (gyro - gyro_prev) * std::min((float)1000000, dt_inv);
+    gyro_prev = gyro;
+
+    velocity_ef = Vector3f(pkt.speedN, pkt.speedE, pkt.speedD);
+
+    location.lat = pkt.latitude * 1.0e7;
+    location.lng = pkt.longitude * 1.0e7;
+    location.alt = pkt.altitude * 1.0e2;
+    position.z = (home.alt - location.alt) * 1.0e-2;
+
+
+    // velocity relative to air mass, in earth frame TODO
+    velocity_air_ef = velocity_ef;
+
+    // velocity relative to airmass in body frame TODO
+    velocity_air_bf = dcm.transposed() * velocity_air_ef;
+
+    battery_voltage = 0;
+    battery_current = 0;
+    rpm1 = 0;
+    rpm2 = 0;
+
+    airspeed = pkt.airspeed;
+    airspeed_pitot = pkt.airspeed;
+}
+
+/*
+  update the Scrimmage simulation by one time step
+ */
+void Scrimmage::update(const struct sitl_input &input)
+{
+    send_servos(input);
+    recv_fdm(input);
+    update_mag_field_bf();
+}
+
+} // namespace SITL

libraries/SITL/SIM_Scrimmage.h

@@ -0,0 +1,95 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  simulator connection for Scrimmage Simulator
+*/
+
+#pragma once
+
+#include <string>
+#include <map>
+
+#include <AP_HAL/utility/Socket.h>
+
+#include "SIM_Aircraft.h"
+
+namespace SITL {
+
+/*
+  a last_letter simulator
+ */
+class Scrimmage : public Aircraft {
+public:
+    Scrimmage(const char *home_str, const char *frame_str);
+
+    /* update model by one time step */
+    void update(const struct sitl_input &input) override;
+
+    /* static object creator */
+    static Aircraft *create(const char *home_str, const char *frame_str) {
+        return new Scrimmage(home_str, frame_str);
+    }
+
+    void set_config(const char *config) override;
+
+    /*  Create and set in/out socket for extenal simulator */
+    void set_interface_ports(const char* address, const int port_in, const int port_out) override;
+
+private:
+    uint16_t fdm_port_in;
+    uint16_t fdm_port_out;
+    const char* fdm_address;
+
+    /*
+      packet sent to Scrimmage from ArduPilot
+     */
+    static const int MAX_NUM_SERVOS = 16;
+    struct servo_packet {
+        uint16_t servos[MAX_NUM_SERVOS];
+    };
+
+    /*
+      state packet sent from Scrimmage to ArduPilot
+     */
+    struct fdm_packet {
+        uint64_t timestamp_us; // simulation time in microseconds
+        double latitude, longitude;
+        double altitude;
+        double heading;
+        double speedN, speedE, speedD;
+        double xAccel, yAccel, zAccel;
+        double rollRate, pitchRate, yawRate;
+        double roll, pitch, yaw;
+        double airspeed;
+    };
+
+    void recv_fdm(const struct sitl_input &input);
+    void send_servos(const struct sitl_input &input);
+    void start_scrimmage(void);
+
+    uint64_t prev_timestamp_us;
+    SocketAPM recv_sock;
+    SocketAPM send_sock;
+
+    const char *frame_str;
+
+    // Use ArduPlane by default
+    const char *mission_name = "arduplane.xml";
+    const char *motion_model = "JSBSimControl";
+    const char *visual_model = "zephyr-blue";
+    const char *terrain = "mcmillan";
+};
+
+} // namespace SITL