Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: break out a SIM_SerialDevice base class, use it for SIM_Vicon

Browse Source
This commit is contained in:
Peter Barker 2019-09-27 21:17:52 +10:00 committed by Peter Barker
parent f0084f743d
commit a08443ff19
4 changed files with 105 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
libraries/SITL/SIM_SerialDevice.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
base class for serially-attached simulated devices
*/
#include "SIM_SerialDevice.h"
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
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;
}

46
libraries/SITL/SIM_SerialDevice.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
base class for serially-attached simulated devices
*/
#pragma once
#include <SITL/SITL.h>
#include <AP_HAL/utility/RingBuffer.h>
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();
};
}

29
libraries/SITL/SIM_Vicon.cpp
View File

@ -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
*/

15
libraries/SITL/SIM_Vicon.h
View File

@ -24,9 +24,11 @@
#include <AP_HAL/utility/RingBuffer.h>
#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();
};
}