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

Plane : Add logging and EKF read for updated flow sensor interface

Browse Source
This commit is contained in:
priseborough 2014-11-02 16:47:41 +11:00 committed by Andrew Tridgell
parent 6d5fb33d1a
commit 2db9247117
1 changed files with 18 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
ArduPlane/ArduPlane.pde
View File

@ -808,7 +808,9 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ barometer_accumulate, 1, 900 },
{ update_notify, 1, 300 },
{ read_rangefinder, 1, 500 },
{ update_optical_flow, 1, 500 },
#if OPTFLOW == ENABLED
{ update_optical_flow, 5, 500 },
#endif
{ one_second_loop, 50, 1000 },
{ check_long_failsafe, 15, 1000 },
{ read_receiver_rssi, 5, 1000 },
@ -1559,26 +1561,31 @@ static void update_flight_stage(void)
airspeed.set_EAS2TAS(barometer.get_EAS2TAS());
}
// called at 100hz but data from sensor only arrives at 10 Hz
#if OPTFLOW == ENABLED
// called at 10hz
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 rawFlowRates = optflow.velocity();
Vector2i temp = optflow.raw();
Vector2f rawGyroRates;
rawGyroRates.x = 0.001f * float(temp.x);
rawGyroRates.y = 0.001f * float(temp.y);
// Use range from a separate range finder, not the PX4Flows built in sensor which is ineffective
float ground_distance_m = 0.01f*float(rangefinder.distance_cm());
ahrs.writeOptFlowMeas(flowQuality, rawFlowRates, rawGyroRates, last_of_update, rangefinder_state.in_range_count, ground_distance_m);
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*rangefinder.distance_cm();
ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update, rangefinder_state.in_range_count, ground_distance_m);
Log_Write_Optflow();
}
}
#endif
AP_HAL_MAIN();