Browse Source

SITL: produce flow data at the requested rate

mission-4.1.18
Andrew Tridgell 10 years ago
parent
commit
0076413c0a
  1. 8
      libraries/AP_HAL_AVR_SITL/sitl_optical_flow.cpp

8
libraries/AP_HAL_AVR_SITL/sitl_optical_flow.cpp

@ -30,12 +30,20 @@ void SITL_State::_update_flow(void)
{ {
double p, q, r; double p, q, r;
Vector3f gyro; Vector3f gyro;
static uint32_t last_flow_ms;
if (!_optical_flow || if (!_optical_flow ||
!_sitl->flow_enable) { !_sitl->flow_enable) {
return; 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 // convert roll rates to body frame
SITL::convert_body_frame(_sitl->state.rollDeg, SITL::convert_body_frame(_sitl->state.rollDeg,
_sitl->state.pitchDeg, _sitl->state.pitchDeg,

Loading…
Cancel
Save