From 70edf31ca674f28776193ceaa0f606de1134844c Mon Sep 17 00:00:00 2001
From: Lucas De Marchi <lucas.demarchi@intel.com>
Date: Mon, 2 Nov 2015 13:19:13 -0200
Subject: [PATCH] AP_OpticalFlow: make error path common

---
 libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp
index 630fb9f4f6..8ba244f39b 100644
--- a/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp
+++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp
@@ -103,18 +103,14 @@ bool AP_OpticalFlow_Linux::read(optical_flow_s* report)
     // Perform the writing and reading in a single command
     if (PX4FLOW_REG == 0x00) {
         if (hal.i2c->readRegisters(PX4FLOW_I2C_ADDRESS, 0, I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE, val) != 0) {
-            num_errors++;
-            i2c_sem->give();
-            return false;
+            goto fail_transfer;
         }
         memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE);
     }
 
     if (PX4FLOW_REG == 0x16) {
         if (hal.i2c->readRegisters(PX4FLOW_I2C_ADDRESS, 0, I2C_INTEGRAL_FRAME_SIZE, val) != 0) {
-            num_errors++;
-            i2c_sem->give();
-            return false;
+            goto fail_transfer;
         }
         memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE);
     }
@@ -140,6 +136,11 @@ bool AP_OpticalFlow_Linux::read(optical_flow_s* report)
     }
 
     return true;
+
+fail_transfer:
+    num_errors++;
+    i2c_sem->give();
+    return false;
 }
 
 // update - read latest values from sensor and fill in x,y and totals.