Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: add parameter for Vicon observation delay

Browse Source
This commit is contained in:
Peter Barker 2018-03-26 13:54:05 +11:00 committed by Randy Mackay
parent dfcf4788d3
commit 08189e0754
4 changed files with 39 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/SITL/SIM_Vicon.cpp
View File

@ -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);
}

8
libraries/SITL/SIM_Vicon.h
View File

@ -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();
};
}

1
libraries/SITL/SITL.cpp
View File

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

1
libraries/SITL/SITL.h
View File

@ -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