Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: added optional flow delay

Browse Source
This commit is contained in:
Andrew Tridgell 2015-01-06 14:18:54 +11:00
parent 228b04e21e
commit e9058df31f
1 changed files with 23 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_HAL_AVR_SITL/sitl_optical_flow.cpp
View File

@ -23,6 +23,11 @@ extern const AP_HAL::HAL& hal;
#include <stdint.h> #include <stdint.h>
#include <math.h> #include <math.h>
#define MAX_OPTFLOW_DELAY 20
static uint8_t next_optflow_index;
static uint8_t optflow_delay;
static OpticalFlow::OpticalFlow_state optflow_data[MAX_OPTFLOW_DELAY];
/* /*
update the optical flow with new data update the optical flow with new data
*/ */
@ -93,6 +98,24 @@ void SITL_State::_update_flow(void)
// poll to provide a delta angle across the interface. // poll to provide a delta angle across the interface.
state.bodyRate = Vector2f(gyro.x, gyro.y); state.bodyRate = Vector2f(gyro.x, gyro.y);
optflow_data[next_optflow_index++] = state;
if (next_optflow_index >= optflow_delay+1) {
next_optflow_index = 0;
}
state = optflow_data[next_optflow_index];
if (_sitl->flow_delay != optflow_delay) {
// cope with updates to the delay control
if (_sitl->flow_delay > MAX_OPTFLOW_DELAY) {
_sitl->flow_delay = MAX_OPTFLOW_DELAY;
}
optflow_delay = _sitl->flow_delay;
for (uint8_t i=0; i<optflow_delay; i++) {
optflow_data[i] = state;
}
}
_optical_flow->setHIL(state); _optical_flow->setHIL(state);
} }