diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index 55793f73c4..59bf4fc6df 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/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();
}
diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h
index 42d6bf4806..69b76a31c3 100644
--- a/libraries/SITL/SIM_Aircraft.h
+++ b/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
diff --git a/libraries/SITL/SIM_CRRCSim.cpp b/libraries/SITL/SIM_CRRCSim.cpp
new file mode 100644
index 0000000000..679d1d2f0b
--- /dev/null
+++ b/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 .
+ */
+/*
+ simulator connector for ardupilot version of CRRCSim
+*/
+
+#include
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include "SIM_CRRCSim.h"
+#include
+
+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
diff --git a/libraries/SITL/SIM_CRRCSim.h b/libraries/SITL/SIM_CRRCSim.h
new file mode 100644
index 0000000000..41dda74d4b
--- /dev/null
+++ b/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 .
+ */
+/*
+ simulator connection for ardupilot version of CRRCSim
+*/
+
+#ifndef _SIM_CRRCSIM_H
+#define _SIM_CRRCSIM_H
+
+#include "SIM_Aircraft.h"
+#include
+
+/*
+ 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