diff --git a/libraries/AP_HAL_AVR_SITL/sitl_optical_flow.cpp b/libraries/AP_HAL_AVR_SITL/sitl_optical_flow.cpp index 35f93e6234..69a6733d12 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_optical_flow.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_optical_flow.cpp @@ -30,12 +30,20 @@ void SITL_State::_update_flow(void) { double p, q, r; Vector3f gyro; + static uint32_t last_flow_ms; if (!_optical_flow || !_sitl->flow_enable) { return; } + // update at the requested rate + uint32_t now = hal.scheduler->millis(); + if (now - last_flow_ms < 1000*(1.0f/_sitl->flow_rate)) { + return; + } + last_flow_ms = now; + // convert roll rates to body frame SITL::convert_body_frame(_sitl->state.rollDeg, _sitl->state.pitchDeg,