aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter : Modify EKF optical flow data interface

Browse Source
This commit is contained in:
priseborough 2014-11-01 09:49:02 +11:00 committed by Andrew Tridgell
parent 311206017c
commit a72b6b179b
2 changed files with 4 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/ArduCopter.pde
View File

@ -775,7 +775,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ throttle_loop, 8, 45 },
{ update_GPS, 8, 90 },
#if OPTFLOW == ENABLED
{ update_optflow, 40, 20 },
{ update_optical_flow, 40, 20 },
#endif
{ update_batt_compass, 40, 72 },
{ read_aux_switches, 40, 5 },
@ -1206,11 +1206,11 @@ static void update_optical_flow(void)
if (optflow.last_update() != last_of_update) {
last_of_update = optflow.last_update();
uint8_t flowQuality = optflow.quality();
Vector2f rawFlowRates = optflow.flowRate();
Vector2f rawGyroRates = optflow.bodyRate();
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, rawFlowRates, rawGyroRates, last_of_update, sonar_alt_health, ground_distance_m);
ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update, sonar_alt_health, ground_distance_m);
if (g.log_bitmask & MASK_LOG_OPTFLOW) {
Log_Write_Optflow();
}

24
ArduCopter/sensors.pde
View File

@ -109,30 +109,6 @@ static void init_optflow()
#endif // OPTFLOW == ENABLED
}
// called at 100hz but data from sensor only arrives at 20 Hz
#if OPTFLOW == ENABLED
static void update_optflow(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 if new data has arrived
if (optflow.last_update() != last_of_update) {
last_of_update = optflow.last_update();
if (should_log(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)