Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter : Add range finder measurements to EKF optical flow data

This commit is contained in:
priseborough 2014-09-10 19:26:47 +10:00 committed by Andrew Tridgell
parent 988de2a898
commit 27f9289391
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduCopter/ArduCopter.pde
View File

@ -1188,7 +1188,7 @@ static void one_hz_loop()
#endif
}
// called at 100hz but data from sensor only arrives at 20 Hz
// called at 100hz but data from sensor only arrives at 10 Hz
#if OPTFLOW == ENABLED
static void update_optical_flow(void)
{
@ -1205,14 +1205,15 @@ static void update_optical_flow(void)
// 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();
uint8_t flowQuality = optflow.quality();
Vector2f rawFlowRates = optflow.velocity();
Vector2i temp = optflow.raw();
Vector2f rawGyroRates;
rawGyroRates.x = 0.001f * float(temp.x);
rawGyroRates.y = 0.001f * float(temp.y);
float ground_distance_m = optflow.ground_distance_m();
ahrs.writeOptFlowMeas(quality, rawFlowRates, rawGyroRates, ground_distance_m, last_of_update);
// 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);
if (g.log_bitmask & MASK_LOG_OPTFLOW) {
Log_Write_Optflow();
}