From 08189e075403f67baa3c48be0dde766e2343504b Mon Sep 17 00:00:00 2001 From: Peter Barker 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(_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 + #include 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 observation_history{observation_history_length}; + ObjectArray *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