SITL: added direct support for last_letter simulator

libraries/SITL/SIM_last_letter.cpp

@@ -0,0 +1,129 @@
+/// -*- 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 last_letter
+*/
+
+#include <AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include "SIM_last_letter.h"
+#include <stdio.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+
+extern const AP_HAL::HAL& hal;
+
+/*
+  constructor
+ */
+last_letter::last_letter(const char *home_str, const char *frame_str) :
+    Aircraft(home_str, frame_str),
+    last_timestamp_us(0),
+    sock(true)
+{
+    // try to bind to a specific port so that if we restart ArduPilot
+    // last_letter keeps sending us packets. Not strictly necessary but
+    // useful for debugging
+    sock.bind("127.0.0.1", fdm_port+1);
+
+    sock.reuseaddress();
+    sock.set_blocking(false);
+
+    start_last_letter();
+}
+
+/*
+  start last_letter child
+ */
+void last_letter::start_last_letter(void)
+{
+    pid_t child_pid = fork();
+    if (child_pid == 0) {
+        close(0);
+        open("/dev/null", O_RDONLY);
+        // in child
+        for (uint8_t i=3; i<100; i++) {
+            close(i);
+        }
+
+        int ret = execlp("roslaunch", 
+                         "roslaunch", 
+                         "last_letter",
+                         "launcher.launch",
+                         "ArduPlane:=true",
+                         NULL);
+        if (ret != 0) {
+            perror("roslaunch");
+        }
+        exit(1);
+    }
+}
+
+/*
+  send servos
+*/
+void last_letter::send_servos(const struct sitl_input &input)
+{
+    servo_packet pkt;
+    memcpy(pkt.servos, input.servos, sizeof(pkt.servos));
+    sock.sendto(&pkt, sizeof(pkt), "127.0.0.1", fdm_port);
+}
+
+/*
+  receive an update from the FDM
+  This is a blocking function
+ */
+void last_letter::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) != sizeof(pkt)) {
+        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.lat = pkt.latitude * 1.0e7;
+    location.lng = pkt.longitude * 1.0e7;
+    location.alt = pkt.altitude*1.0e2;
+    dcm.from_euler(pkt.roll, pkt.pitch, pkt.yaw);
+
+    // auto-adjust to last_letter frame rate
+    uint64_t deltat_us = pkt.timestamp_us - last_timestamp_us;
+    time_now_us += deltat_us;
+
+    if (deltat_us < 1.0e4 && deltat_us > 0) {
+        adjust_frame_time(1.0e6/deltat_us);
+    }
+    last_timestamp_us = pkt.timestamp_us;
+}
+
+/*
+  update the last_letter simulation by one time step
+ */
+void last_letter::update(const struct sitl_input &input)
+{
+    send_servos(input);
+    recv_fdm(input);
+    sync_frame_time();
+}
+#endif // CONFIG_HAL_BOARD

libraries/SITL/SIM_last_letter.h

@@ -0,0 +1,76 @@
+/// -*- 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 last_letter
+*/
+
+#ifndef _SIM_LAST_LETTER_H
+#define _SIM_LAST_LETTER_H
+
+#include "SIM_Aircraft.h"
+#include <utility/Socket.h>
+
+/*
+  a last_letter simulator
+ */
+class last_letter : public Aircraft
+{
+public:
+    last_letter(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 last_letter(home_str, frame_str);
+    }
+
+private:
+    static const uint16_t fdm_port = 9002;
+
+    /*
+      packet sent to last_letter
+     */
+    struct servo_packet {
+        uint16_t servos[16];
+    };
+    
+    /*
+      reply packet sent from last_letter 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_last_letter(void);
+
+    uint64_t last_timestamp_us;
+    SocketAPM sock;
+};
+
+
+#endif // _SIM_LAST_LETTER_H