From e487d95c1b5deabf845de05106a157748f02a4a4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 22 May 2015 15:52:45 +1000 Subject: [PATCH] SITL: added direct support for last_letter simulator --- libraries/SITL/SIM_last_letter.cpp | 129 +++++++++++++++++++++++++++++ libraries/SITL/SIM_last_letter.h | 76 +++++++++++++++++ 2 files changed, 205 insertions(+) create mode 100644 libraries/SITL/SIM_last_letter.cpp create mode 100644 libraries/SITL/SIM_last_letter.h diff --git a/libraries/SITL/SIM_last_letter.cpp b/libraries/SITL/SIM_last_letter.cpp new file mode 100644 index 0000000000..abcaf56a84 --- /dev/null +++ b/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 . + */ +/* + simulator connector for ardupilot version of last_letter +*/ + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include "SIM_last_letter.h" +#include +#include +#include +#include + +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 diff --git a/libraries/SITL/SIM_last_letter.h b/libraries/SITL/SIM_last_letter.h new file mode 100644 index 0000000000..da3c7efd8c --- /dev/null +++ b/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 . + */ +/* + simulator connection for ardupilot version of last_letter +*/ + +#ifndef _SIM_LAST_LETTER_H +#define _SIM_LAST_LETTER_H + +#include "SIM_Aircraft.h" +#include + +/* + 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