From 08189e075403f67baa3c48be0dde766e2343504b Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 26 Mar 2018 13:54:05 +1100
Subject: [PATCH] SITL: add parameter for Vicon observation delay

---
 libraries/SITL/SIM_Vicon.cpp | 35 +++++++++++++++++++++++++++++++----
 libraries/SITL/SIM_Vicon.h   |  8 ++++++--
 libraries/SITL/SITL.cpp      |  1 +
 libraries/SITL/SITL.h        |  1 +
 4 files changed, 39 insertions(+), 6 deletions(-)

diff --git a/libraries/SITL/SIM_Vicon.cpp b/libraries/SITL/SIM_Vicon.cpp
index 846b4ece0a..aa7449614d 100644
--- a/libraries/SITL/SIM_Vicon.cpp
+++ b/libraries/SITL/SIM_Vicon.cpp
@@ -77,17 +77,17 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
     vicon.last_observation_ms = now;
 
     obs_elements observation;
-    if (vicon.observation_history_length == 0) {
+    if (_sitl->vicon_observation_history_length == 0) {
         // no delay
         observation = new_obs;
     } else {
-        vicon.observation_history.push(new_obs);
+        vicon.observation_history->push(new_obs);
 
-        if (vicon.observation_history.space() != 0) {
+        if (vicon.observation_history->space() != 0) {
             ::fprintf(stderr, "Insufficient delay\n");
             return;
         }
-        if (!vicon.observation_history.pop(observation)) {
+        if (!vicon.observation_history->pop(observation)) {
             abort();
         }
     }
@@ -117,11 +117,38 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
     }
 }
 
+bool Vicon::init_sitl_pointer()
+{
+    if (_sitl == nullptr) {
+        _sitl = (SITL *)AP_Param::find_object("SIM_");
+        if (_sitl == nullptr) {
+            return false;
+        }
+    }
+    if (_sitl->vicon_observation_history_length > 0 &&
+        vicon.observation_history == nullptr) {
+        const uint8_t maxlen = 100;
+        if (_sitl->vicon_observation_history_length > maxlen) {
+            ::fprintf(stderr, "Clamping history length to %u", maxlen);
+            _sitl->vicon_observation_history_length = maxlen;
+        }
+        vicon.observation_history = new ObjectArray<obs_elements>(_sitl->vicon_observation_history_length);
+        if (vicon.observation_history == nullptr) {
+            AP_HAL::panic("Failed to allocate history");
+        }
+    }
+    return true;
+}
+
 /*
   update vicon sensor state
  */
 void Vicon::update(const Location &loc, const Vector3f &position, const Quaternion &attitude)
 {
+    if (!init_sitl_pointer()) {
+        return;
+    }
+
     maybe_send_heartbeat();
     update_vicon_position_estimate(loc, position, attitude);
 }
diff --git a/libraries/SITL/SIM_Vicon.h b/libraries/SITL/SIM_Vicon.h
index 5f55f859a7..a83047a2d2 100644
--- a/libraries/SITL/SIM_Vicon.h
+++ b/libraries/SITL/SIM_Vicon.h
@@ -20,6 +20,8 @@
 
 #include "SIM_Aircraft.h"
 
+#include <SITL/SITL.h>
+
 #include <AP_HAL/utility/RingBuffer.h>
 
 namespace SITL {
@@ -38,6 +40,8 @@ public:
 
 private:
 
+    SITL *_sitl;
+
     // TODO: make these parameters:
     const uint8_t system_id = 17;
     const uint8_t component_id = 18;
@@ -57,8 +61,7 @@ private:
     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};
+        ObjectArray<obs_elements> *observation_history;
         uint32_t last_observation_ms;
     } vicon;
 
@@ -69,6 +72,7 @@ private:
     void maybe_send_heartbeat();
     uint32_t last_heartbeat_ms;
 
+    bool init_sitl_pointer();
 };
 
 }
diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index 6dc3aae23e..bc4bf8f643 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -112,6 +112,7 @@ const AP_Param::GroupInfo SITL::var_info2[] = {
     AP_GROUPINFO("ARSPD2_FAIL", 11, SITL,  arspd2_fail, 0),
     AP_GROUPINFO("ARSPD2_FAILP",12, SITL,  arspd2_fail_pressure, 0),
     AP_GROUPINFO("ARSPD2_PITOT",13, SITL,  arspd2_fail_pitot_pressure, 0),
+    AP_GROUPINFO("VICON_HSTLEN",14, SITL,  vicon_observation_history_length, 0),
     AP_GROUPEND
 };
     
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index 372f722076..b3eb69aa1c 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -90,6 +90,7 @@ public:
     AP_Float gps_noise; // amplitude of the gps altitude error
     AP_Int16 gps_lock_time; // delay in seconds before GPS gets lock
     AP_Int16 gps_alt_offset; // gps alt error
+    AP_Int8  vicon_observation_history_length; // frame delay for vicon messages
 
     AP_Float mag_noise;   // in mag units (earth field is 818)
     AP_Float mag_error;   // in degrees