SITL: added CRRCSim simulation connection

libraries/SITL/SIM_Aircraft.cpp

@@ -43,7 +43,8 @@ Aircraft::Aircraft(const char *home_str, const char *frame_str) :
     time_now_us(0),
     gyro_noise(radians(0.1f)),
     accel_noise(0.3),
-    rate_hz(400)
+    rate_hz(400),
+    last_time_us(0)
 {
     char *saveptr=NULL;
     char *s = strdup(home_str);
@@ -86,7 +87,11 @@ void Aircraft::update_position(void)
 
     location.alt  = home.alt - position.z*100.0f;
 
-    time_now_us += frame_time_us;
+    // we only advance time if it hasn't been advanced already by the
+    // backend
+    if (last_time_us == time_now_us) {
+        time_now_us += frame_time_us;
+    }
     sync_frame_time();
 }
 

libraries/SITL/SIM_Aircraft.h

@@ -106,6 +106,9 @@ protected:
 
     /* return normal distribution random numbers */
     double rand_normal(double mean, double stddev);
+
+private:
+    uint64_t last_time_us;
 };
 
 #endif // _SIM_AIRCRAFT_H

libraries/SITL/SIM_CRRCSim.cpp

@@ -0,0 +1,157 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   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 ardupilot version of CRRCSim
+*/
+
+#include <AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include "SIM_CRRCSim.h"
+#include <stdio.h>
+
+extern const AP_HAL::HAL& hal;
+
+/*
+  constructor
+ */
+CRRCSim::CRRCSim(const char *home_str, const char *frame_str) :
+    Aircraft(home_str, frame_str),
+    last_timestamp(0),
+    sock(true)
+{
+    // try to bind to a specific port so that if we restart ArduPilot
+    // CRRCSim keeps sending us packets. Not strictly necessary but
+    // useful for debugging
+    sock.bind("127.0.0.1", 9003);
+
+    sock.reuseaddress();
+
+    if (!sock.connect("127.0.0.1", 9002)) {
+        hal.scheduler->panic(PSTR("Unable to connect to CRRCSim on port 9002"));
+    }
+    sock.set_blocking(false);
+    heli_servos = (strstr(frame_str,"heli") != NULL);
+}
+
+/*
+  decode and send servos for heli
+*/
+void CRRCSim::send_servos_heli(const struct sitl_input &input)
+{
+    float swash1 = (input.servos[0]-1000) / 1000.0f;
+    float swash2 = (input.servos[1]-1000) / 1000.0f;
+    float swash3 = (input.servos[2]-1000) / 1000.0f;
+    float tail_rotor = (input.servos[3]-1000) / 1000.0f;
+    float rsc = (input.servos[7]-1000) / 1000.0f;
+
+    float col_pitch = (swash1+swash2+swash3)/3.0 - 0.5f;
+    float roll_rate = (swash1 - swash2)/2;
+    float pitch_rate = -((swash1 + swash2)/2.0 - swash3)/2;
+    float yaw_rate = -(tail_rotor - 0.5);
+    
+    servo_packet pkt;
+    pkt.roll_rate  = constrain_float(roll_rate, -0.5, 0.5);
+    pkt.pitch_rate = constrain_float(pitch_rate, -0.5, 0.5);
+    pkt.throttle   = constrain_float(rsc, 0, 1);
+    pkt.yaw_rate   = constrain_float(yaw_rate, -0.5, 0.5);
+    pkt.col_pitch  = constrain_float(col_pitch, -0.5, 0.5);
+
+    sock.send(&pkt, sizeof(pkt));
+}
+
+/*
+  decode and send servos for fixed wing
+*/
+void CRRCSim::send_servos_fixed_wing(const struct sitl_input &input)
+{
+    float roll_rate  = ((input.servos[0]-1000)/1000.0) - 0.5;
+    float pitch_rate = ((input.servos[1]-1000)/1000.0) - 0.5;
+    float yaw_rate   = ((input.servos[3]-1000)/1000.0) - 0.5;
+    float throttle   = ((input.servos[2]-1000)/1000.0);
+
+    servo_packet pkt;
+    pkt.roll_rate  = constrain_float(roll_rate, -0.5, 0.5);
+    pkt.pitch_rate = constrain_float(pitch_rate, -0.5, 0.5);
+    pkt.throttle   = constrain_float(throttle, 0, 1);
+    pkt.yaw_rate   = constrain_float(yaw_rate, -0.5, 0.5);
+    pkt.col_pitch  = 0;
+
+    sock.send(&pkt, sizeof(pkt));
+}
+
+/*
+  decode and send servos
+*/
+void CRRCSim::send_servos(const struct sitl_input &input)
+{
+    if (heli_servos) {
+        send_servos_heli(input);
+    } else {
+        send_servos_fixed_wing(input);
+    }
+}
+
+/*
+  receive an update from the FDM
+  This is a blocking function
+ */
+void CRRCSim::recv_fdm(const struct sitl_input &input)
+{
+    fdm_packet pkt;
+
+    /*
+      we re-send the servo packet every 0.1 seconds until we get a
+      reply. This allows us to cope with some packet loss to the FDM
+     */
+    while (!sock.recv(&pkt, sizeof(pkt), 100)) {
+        send_servos(input);
+    }
+
+    accel_body = Vector3f(pkt.xAccel, pkt.yAccel, pkt.zAccel);
+    gyro = Vector3f(pkt.rollRate, pkt.pitchRate, pkt.yawRate);
+    velocity_ef = Vector3f(pkt.speedN, pkt.speedE, pkt.speedD);
+
+    Location loc1, loc2;
+    loc2.lat = pkt.latitude * 1.0e7;
+    loc2.lng = pkt.longitude * 1.0e7;
+    memset(&loc1, 0, sizeof(loc1));
+    Vector2f posdelta = location_diff(loc1, loc2);
+    position.x = posdelta.x;
+    position.y = posdelta.y;
+    position.z = -pkt.altitude;
+
+    dcm.from_euler(pkt.roll, pkt.pitch, pkt.yaw);
+
+    // auto-adjust to crrcsim frame rate
+    double deltat = pkt.timestamp - last_timestamp;
+    time_now_us += deltat * 1.0e6;
+
+    if (deltat < 0.01 && deltat > 0) {
+        adjust_frame_time(1.0/deltat);
+    }
+    last_timestamp = pkt.timestamp;
+}
+
+/*
+  update the CRRCSim simulation by one time step
+ */
+void CRRCSim::update(const struct sitl_input &input)
+{
+    send_servos(input);
+    recv_fdm(input);
+    update_position();
+}
+#endif // CONFIG_HAL_BOARD

libraries/SITL/SIM_CRRCSim.h

@@ -0,0 +1,80 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   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 ardupilot version of CRRCSim
+*/
+
+#ifndef _SIM_CRRCSIM_H
+#define _SIM_CRRCSIM_H
+
+#include "SIM_Aircraft.h"
+#include <utility/Socket.h>
+
+/*
+  a CRRCSim simulator
+ */
+class CRRCSim : public Aircraft
+{
+public:
+    CRRCSim(const char *home_str, const char *frame_str);
+
+    /* update model by one time step */
+    void update(const struct sitl_input &input);
+
+    /* static object creator */
+    static Aircraft *create(const char *home_str, const char *frame_str) {
+        return new CRRCSim(home_str, frame_str);
+    }
+
+private:
+    /*
+      packet sent to CRRCSim
+     */
+    struct servo_packet {
+        float roll_rate;
+        float pitch_rate;
+        float throttle;
+        float yaw_rate;
+        float col_pitch;
+    };
+    
+    /*
+      reply packet sent from CRRCSim to ArduPilot
+     */
+    struct fdm_packet {
+        double timestamp;
+        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 send_servos_heli(const struct sitl_input &input);
+    void send_servos_fixed_wing(const struct sitl_input &input);
+    void recv_fdm(const struct sitl_input &input);
+    void send_servos(const struct sitl_input &input);
+
+    bool heli_servos;
+    double last_timestamp;
+    SocketAPM sock;
+};
+
+
+#endif // _SIM_CRRCSIM_H