From e9058df31f4a1b07ff3408fc5280e972fefbe4a7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 6 Jan 2015 14:18:54 +1100 Subject: [PATCH] SITL: added optional flow delay --- .../AP_HAL_AVR_SITL/sitl_optical_flow.cpp | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/libraries/AP_HAL_AVR_SITL/sitl_optical_flow.cpp b/libraries/AP_HAL_AVR_SITL/sitl_optical_flow.cpp index 69a6733d12..a0b1c5ce38 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_optical_flow.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_optical_flow.cpp @@ -23,6 +23,11 @@ extern const AP_HAL::HAL& hal; #include #include +#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 */ @@ -93,6 +98,24 @@ void SITL_State::_update_flow(void) // poll to provide a delta angle across the interface. 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; isetHIL(state); }