|
|
|
@ -109,6 +109,36 @@ static void init_optflow()
@@ -109,6 +109,36 @@ static void init_optflow()
|
|
|
|
|
#endif // OPTFLOW == ENABLED |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// called at 200hz |
|
|
|
|
#if OPTFLOW == ENABLED |
|
|
|
|
static void 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(); |
|
|
|
|
// Use range from a separate range finder if available, not the PX4Flows built in sensor which is ineffective |
|
|
|
|
float ground_distance_m = 0.01f*float(sonar_alt); |
|
|
|
|
ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update, sonar_alt_health, ground_distance_m); |
|
|
|
|
if (g.log_bitmask & MASK_LOG_OPTFLOW) { |
|
|
|
|
Log_Write_Optflow(); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
#endif // OPTFLOW == ENABLED |
|
|
|
|
|
|
|
|
|
// read_battery - check battery voltage and current and invoke failsafe if necessary |
|
|
|
|
// called at 10hz |
|
|
|
|
static void read_battery(void) |
|
|
|
|