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;
+
+};
+
+}