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

Copter : EKF optical flow fusion support preliminary changes

Browse Source 
Assumes optflow sensor has been modified to push data at a low rate, eg 10Hz
and velocity data is angular velocity, not linear velocity
This commit is contained in:
priseborough 2014-08-07 10:30:42 +10:00 committed by Andrew Tridgell
parent 8b80e58861
commit 34249ffa42
1 changed files with 29 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
ArduCopter/ArduCopter.pde
View File

@ -849,7 +849,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ throttle_loop, 2, 450 },
{ update_GPS, 2, 900 },
#if OPTFLOW == ENABLED
{ update_optflow, 2, 100 },
{ update_optical_flow, 1, 100 },
#endif
{ update_batt_compass, 10, 720 },
{ read_aux_switches, 10, 50 },
@ -1188,6 +1188,34 @@ static void one_hz_loop()
#endif
}
// called at 100hz but data from sensor only arrives at 20 Hz
#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 quality = optflow.quality();
Vector2f raw = optflow.velocity();
float ground_distance_m = optflow.ground_distance_m();
ahrs.writeOptFlowMeas(quality, raw, ground_distance_m, last_of_update);
if (g.log_bitmask & MASK_LOG_OPTFLOW) {
Log_Write_Optflow();
}
}
}
#endif // OPTFLOW == ENABLED
// called at 50hz
static void update_GPS(void)
{