void Sub::update_optical_flow(void) { static uint32_t last_of_update = 0; // exit immediately if not enabled if (!optflow.enabled()) { return; } // read from sensor optflow.update(); // write to log and send to EKF if new data has arrived if (optflow.last_update() != last_of_update) { last_of_update = optflow.last_update(); uint8_t flowQuality = optflow.quality(); Vector2f flowRate = optflow.flowRate(); Vector2f bodyRate = optflow.bodyRate(); const Vector3f &posOffset = optflow.get_pos_offset(); ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update, posOffset); if (g.log_bitmask & MASK_LOG_OPTFLOW) { Log_Write_Optflow(); } } }
void OpticalFlow::update_state(const OpticalFlow_state &state) { _state = state; _last_update_ms = AP_HAL::millis(); // write to log and send to EKF if new data has arrived AP::ahrs_navekf().writeOptFlowMeas(quality(), _state.flowRate, _state.bodyRate, _last_update_ms, get_pos_offset()); Log_Write_Optflow(); }
// called at 50hz void Plane::update_optical_flow(void) { static uint32_t last_of_update = 0; // exit immediately if not enabled if (!optflow.enabled()) { return; } // read from sensor optflow.update(); // write to log and send to EKF if new data has arrived if (optflow.last_update() != last_of_update) { last_of_update = optflow.last_update(); uint8_t flowQuality = optflow.quality(); Vector2f flowRate = optflow.flowRate(); Vector2f bodyRate = optflow.bodyRate(); ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update); Log_Write_Optflow(); } }