Browse Source

Copter: move update_optflow to sensors.pde

No functional change
master
Randy Mackay 10 years ago
parent
commit
c93c7af20c
  1. 30
      ArduCopter/ArduCopter.pde
  2. 30
      ArduCopter/sensors.pde

30
ArduCopter/ArduCopter.pde

@ -1180,36 +1180,6 @@ static void one_hz_loop() @@ -1180,36 +1180,6 @@ static void one_hz_loop()
#endif
}
// 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
// called at 50hz
static void update_GPS(void)
{

30
ArduCopter/sensors.pde

@ -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)

Loading…
Cancel
Save