From dfcf4788d310c2ecfc2dcbfea28e4807edda67eb Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 8 Mar 2018 12:25:44 +1100 Subject: [PATCH] SITL: add vicon-over-mavlink support --- libraries/SITL/SIM_Aircraft.h | 6 ++ libraries/SITL/SIM_Vicon.cpp | 128 ++++++++++++++++++++++++++++++++++ libraries/SITL/SIM_Vicon.h | 74 ++++++++++++++++++++ 3 files changed, 208 insertions(+) create mode 100644 libraries/SITL/SIM_Vicon.cpp create mode 100644 libraries/SITL/SIM_Vicon.h diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 9acbd84febf..b5f4f8f99fe 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -115,6 +115,12 @@ public: const Location &get_location() const { return location; } + const Vector3f &get_position() const { return position; } + + void get_attitude(Quaternion &attitude) const { + attitude.from_rotation_matrix(dcm); + } + protected: SITL *sitl; Location home; diff --git a/libraries/SITL/SIM_Vicon.cpp b/libraries/SITL/SIM_Vicon.cpp new file mode 100644 index 00000000000..846b4ece0a5 --- /dev/null +++ b/libraries/SITL/SIM_Vicon.cpp @@ -0,0 +1,128 @@ +/* + 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 . + */ +/* + simple vicon simulator class + + XKFR + +*/ + +#include "SIM_Vicon.h" +#include +#include +#include + +using namespace SITL; + +Vicon::Vicon() +{ + int tmp[2]; + pipe(tmp); + fd_my_end = tmp[1]; + fd_their_end = tmp[0]; + + // 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); +} + +void Vicon::maybe_send_heartbeat() +{ + const uint32_t now = AP_HAL::millis(); + + if (now - last_heartbeat_ms < 100) { + // we only provide a heartbeat every so often + return; + } + last_heartbeat_ms = now; + + mavlink_message_t msg; + mavlink_msg_heartbeat_pack(system_id, + component_id, + &msg, + MAV_TYPE_GCS, + MAV_AUTOPILOT_INVALID, + 0, + 0, + 0); +} + +void Vicon::update_vicon_position_estimate(const Location &loc, + const Vector3f &position, + const Quaternion &attitude) +{ + const uint32_t now = AP_HAL::millis(); + + if (now - vicon.last_observation_ms < vicon.observation_interval_ms) { + return; + } + + obs_elements new_obs = { + now, + position, + attitude + }; + vicon.last_observation_ms = now; + + obs_elements observation; + if (vicon.observation_history_length == 0) { + // no delay + observation = new_obs; + } else { + vicon.observation_history.push(new_obs); + + if (vicon.observation_history.space() != 0) { + ::fprintf(stderr, "Insufficient delay\n"); + return; + } + if (!vicon.observation_history.pop(observation)) { + abort(); + } + } + + float roll; + float pitch; + float yaw; + observation.attitude.to_euler(roll, pitch, yaw); + + mavlink_message_t msg; + mavlink_msg_vicon_position_estimate_pack_chan(system_id, + component_id, + mavlink_ch, + &msg, + observation.time_ms*1000, + observation.position.x, + observation.position.y, + observation.position.z, + roll, + pitch, + yaw); + + // ::fprintf(stderr, "Vicon: writing pos=(%03.03f %03.03f %03.03f) att=(%01.03f %01.03f %01.03f)\n", observation.position.x, observation.position.y, observation.position.z, roll, pitch, yaw); + if (::write(fd_my_end, (void*)&msg, sizeof(msg)) != sizeof(msg)) { + ::fprintf(stderr, "Vicon: write failure\n"); + // abort(); + } +} + +/* + update vicon sensor state + */ +void Vicon::update(const Location &loc, const Vector3f &position, const Quaternion &attitude) +{ + maybe_send_heartbeat(); + update_vicon_position_estimate(loc, position, attitude); +} + diff --git a/libraries/SITL/SIM_Vicon.h b/libraries/SITL/SIM_Vicon.h new file mode 100644 index 00000000000..5f55f859a74 --- /dev/null +++ b/libraries/SITL/SIM_Vicon.h @@ -0,0 +1,74 @@ +/* + 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 . + */ +/* + simple particle sensor simulation +*/ + +#pragma once + +#include "SIM_Aircraft.h" + +#include + +namespace SITL { + +class Vicon { +public: + + Vicon(); + + // 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: + + // TODO: make these parameters: + const uint8_t system_id = 17; + const uint8_t component_id = 18; + + // we share channels with the ArduPilot binary! + const uint8_t mavlink_ch = 17; + + int fd_their_end; + int fd_my_end; + + struct obs_elements { + uint32_t time_ms; // measurement timestamp (msec) + Vector3f position; + Quaternion attitude; + }; + + struct { + const uint16_t observation_interval_ms = 20; + // delay results by some multiplier of the observation_interval: + const uint8_t observation_history_length = 0; + ObjectArray observation_history{observation_history_length}; + uint32_t last_observation_ms; + } vicon; + + void update_vicon_position_estimate(const Location &loc, + const Vector3f &position, + const Quaternion &attitude); + + void maybe_send_heartbeat(); + uint32_t last_heartbeat_ms; + +}; + +}