SITL: add vicon-over-mavlink support

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;

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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  simple vicon simulator class
+
+  XKFR
+
+*/
+
+#include "SIM_Vicon.h"
+#include <stdio.h>
+#include <unistd.h>
+#include <fcntl.h>
+
+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);
+}
+

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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  simple particle sensor simulation
+*/
+
+#pragma once
+
+#include "SIM_Aircraft.h"
+
+#include <AP_HAL/utility/RingBuffer.h>
+
+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<obs_elements> 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;
+
+};
+
+}