From 0d95a9c41d3ecc51b41f818bf9b402de9a71ac59 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Mon, 2 Nov 2015 13:19:39 -0200 Subject: [PATCH] AP_OpticalFlow: reduce lock region Release the lock as soon as we can. --- libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp index 8ba244f39b..65585eec3a 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp @@ -115,6 +115,13 @@ bool AP_OpticalFlow_Linux::read(optical_flow_s* report) memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE); } + i2c_sem->give(); + + // reduce error count + if (num_errors > 0) { + num_errors--; + } + report->pixel_flow_x_integral = static_cast(f_integral.pixel_flow_x_integral) / 10000.0f; //convert to radians report->pixel_flow_y_integral = static_cast(f_integral.pixel_flow_y_integral) / 10000.0f; //convert to radians report->frame_count_since_last_readout = f_integral.frame_count_since_last_readout; @@ -128,13 +135,6 @@ bool AP_OpticalFlow_Linux::read(optical_flow_s* report) report->gyro_temperature = f_integral.gyro_temperature; // Temperature * 100 in centi-degrees Celsius report->sensor_id = 0; - i2c_sem->give(); - - // reduce error count - if (num_errors > 0) { - num_errors--; - } - return true; fail_transfer: