From a08443ff197612e8b30c3016fc2ac4c975d51808 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 27 Sep 2019 21:17:52 +1000 Subject: [PATCH] SITL: break out a SIM_SerialDevice base class, use it for SIM_Vicon --- libraries/SITL/SIM_SerialDevice.cpp | 54 +++++++++++++++++++++++++++++ libraries/SITL/SIM_SerialDevice.h | 46 ++++++++++++++++++++++++ libraries/SITL/SIM_Vicon.cpp | 29 ++-------------- libraries/SITL/SIM_Vicon.h | 15 ++------ 4 files changed, 105 insertions(+), 39 deletions(-) create mode 100644 libraries/SITL/SIM_SerialDevice.cpp create mode 100644 libraries/SITL/SIM_SerialDevice.h diff --git a/libraries/SITL/SIM_SerialDevice.cpp b/libraries/SITL/SIM_SerialDevice.cpp new file mode 100644 index 0000000000..e2345b84ce --- /dev/null +++ b/libraries/SITL/SIM_SerialDevice.cpp @@ -0,0 +1,54 @@ +/* + 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 . + */ +/* + base class for serially-attached simulated devices +*/ + +#include "SIM_SerialDevice.h" +#include +#include +#include + +using namespace SITL; + +SerialDevice::SerialDevice() +{ + int tmp[2]; + if (pipe(tmp) == -1) { + AP_HAL::panic("pipe() failed"); + } + fd_my_end = tmp[1]; + fd_their_end = tmp[0]; + + // close file descriptors on exec: + fcntl(fd_my_end, F_SETFD, FD_CLOEXEC); + fcntl(fd_their_end, F_SETFD, FD_CLOEXEC); + + // make sure we don't screw the simulation up by blocking: + fcntl(fd_my_end, F_SETFL, fcntl(fd_my_end, F_GETFL, 0) | O_NONBLOCK); + fcntl(fd_their_end, F_SETFL, fcntl(fd_their_end, F_GETFL, 0) | O_NONBLOCK); +} + +bool SerialDevice::init_sitl_pointer() +{ + if (_sitl == nullptr) { + _sitl = AP::sitl(); + if (_sitl == nullptr) { + return false; + } + } + return true; +} + diff --git a/libraries/SITL/SIM_SerialDevice.h b/libraries/SITL/SIM_SerialDevice.h new file mode 100644 index 0000000000..1be2933732 --- /dev/null +++ b/libraries/SITL/SIM_SerialDevice.h @@ -0,0 +1,46 @@ +/* + 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 . + */ +/* + base class for serially-attached simulated devices +*/ + +#pragma once + +#include + +#include + +namespace SITL { + +class SerialDevice { +public: + + SerialDevice(); + + // return fd on which data from the device can be read, and data + // to the device can be written + int fd() { return fd_their_end; } + +protected: + + SITL *_sitl; + + int fd_their_end; + int fd_my_end; + + bool init_sitl_pointer(); +}; + +} diff --git a/libraries/SITL/SIM_Vicon.cpp b/libraries/SITL/SIM_Vicon.cpp index bc832bd8da..a0f7b78830 100644 --- a/libraries/SITL/SIM_Vicon.cpp +++ b/libraries/SITL/SIM_Vicon.cpp @@ -29,23 +29,9 @@ using namespace SITL; #define USE_VISION_POSITION_ESTIMATE 1 -Vicon::Vicon() +Vicon::Vicon() : + SerialDevice::SerialDevice() { - int tmp[2]; - if (pipe(tmp) == -1) { - AP_HAL::panic("pipe() failed"); - } - fd_my_end = tmp[1]; - fd_their_end = tmp[0]; - - // close file descriptors on exec: - fcntl(fd_my_end, F_SETFD, FD_CLOEXEC); - fcntl(fd_their_end, F_SETFD, FD_CLOEXEC); - - // make sure we don't screw the simulation up by blocking: - fcntl(fd_my_end, F_SETFL, fcntl(fd_my_end, F_GETFL, 0) | O_NONBLOCK); - fcntl(fd_their_end, F_SETFL, fcntl(fd_their_end, F_GETFL, 0) | O_NONBLOCK); - if (!valid_channel(mavlink_ch)) { AP_HAL::panic("Invalid mavlink channel"); } @@ -142,17 +128,6 @@ void Vicon::update_vicon_position_estimate(const Location &loc, time_send_us = now_us + delay_ms * 1000UL; } -bool Vicon::init_sitl_pointer() -{ - if (_sitl == nullptr) { - _sitl = AP::sitl(); - if (_sitl == nullptr) { - return false; - } - } - return true; -} - /* update vicon sensor state */ diff --git a/libraries/SITL/SIM_Vicon.h b/libraries/SITL/SIM_Vicon.h index 2672dc84b1..e114df1e6f 100644 --- a/libraries/SITL/SIM_Vicon.h +++ b/libraries/SITL/SIM_Vicon.h @@ -24,9 +24,11 @@ #include +#include "SIM_SerialDevice.h" + namespace SITL { -class Vicon { +class Vicon : public SerialDevice { public: Vicon(); @@ -34,14 +36,8 @@ public: // update state void update(const Location &loc, const Vector3f &position, const Quaternion &attitude); - // return fd on which data from the device can be read, and data - // to the device can be written - int fd() { return fd_their_end; } - private: - SITL *_sitl; - // TODO: make these parameters: const uint8_t system_id = 17; const uint8_t component_id = 18; @@ -49,9 +45,6 @@ private: // we share channels with the ArduPilot binary! const mavlink_channel_t mavlink_ch = (mavlink_channel_t)(MAVLINK_COMM_0+5); - int fd_their_end; - int fd_my_end; - uint64_t last_observation_usec; uint64_t time_send_us; uint64_t time_offset_us; @@ -69,8 +62,6 @@ private: void maybe_send_heartbeat(); uint32_t last_heartbeat_ms; - - bool init_sitl_pointer(); }; }