/* 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 "SIM_last_letter.h" #if HAL_SIM_LAST_LETTER_ENABLED #include #include #include #include #include extern const AP_HAL::HAL& hal; namespace SITL { last_letter::last_letter(const char *_frame_str) : Aircraft(_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) { // in child close(0); open("/dev/null", O_RDONLY|O_CLOEXEC); for (uint8_t i=3; i<100; i++) { close(i); } int ret = execlp("roslaunch", "roslaunch", "last_letter", "gazebo.launch", "ArduPlane:=true", nullptr); 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); airspeed = pkt.airspeed; airspeed_pitot = pkt.airspeed; // 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(); update_position(); time_advance(); // update magnetic field update_mag_field_bf(); } } // namespace SITL #endif // HAL_SIM_LAST_LETTER_ENABLED