From f9b16a9a501b70d7c87b0fa3692a4e0e364ba843 Mon Sep 17 00:00:00 2001
From: rmackay9 <rmackay9@yahoo.com>
Date: Thu, 4 Oct 2012 22:46:09 +0900
Subject: [PATCH] AP_OpticalFlow: modified to use AP_Semaphore for SPI3 bus
 when required

---
 .../AP_OpticalFlow_ADNS3080.cpp               | 29 +++++++++++++++++++
 .../AP_OpticalFlow_test.pde                   |  1 +
 2 files changed, 30 insertions(+)

diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp
index 25d73202b3..6fc439ebbd 100644
--- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp
+++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp
@@ -12,6 +12,8 @@
 #include "AP_OpticalFlow_ADNS3080.h"
 #include "SPI.h"
 #include "SPI3.h"
+#include "AP_Semaphore.h"
+
 #if defined(ARDUINO) && ARDUINO >= 100
  #include "Arduino.h"
 #else
@@ -179,6 +181,14 @@ AP_OpticalFlow_ADNS3080::read_register(byte address)
     uint8_t result = 0;
     uint8_t junk = 0;
 
+    // get spi3 semaphore if required
+    if( _spi_bus == ADNS3080_SPIBUS_3 ) {
+        // if failed to get semaphore then just quietly fail
+        if( !AP_Semaphore_spi3.get(this) ) {
+            return 0;
+        }
+    }
+
     backup_spi_settings();
 
     // take the chip select low to select the device
@@ -199,6 +209,11 @@ AP_OpticalFlow_ADNS3080::read_register(byte address)
 
     restore_spi_settings();
 
+    // get spi3 semaphore if required
+    if( _spi_bus == ADNS3080_SPIBUS_3 ) {
+        AP_Semaphore_spi3.release(this);
+    }
+
     return result;
 }
 
@@ -208,6 +223,15 @@ AP_OpticalFlow_ADNS3080::write_register(byte address, byte value)
 {
     byte junk = 0;
 
+    // get spi3 semaphore if required
+    if( _spi_bus == ADNS3080_SPIBUS_3 ) {
+        // if failed to get semaphore then just quietly fail
+        if( !AP_Semaphore_spi3.get(this) ) {
+            Serial.println("Optflow: failed to get spi3 semaphore!");
+            return;
+        }
+    }
+
     backup_spi_settings();
 
     // take the chip select low to select the device
@@ -227,6 +251,11 @@ AP_OpticalFlow_ADNS3080::write_register(byte address, byte value)
     digitalWrite(_cs_pin, HIGH);
 
     restore_spi_settings();
+
+    // get spi3 semaphore if required
+    if( _spi_bus == ADNS3080_SPIBUS_3 ) {
+        AP_Semaphore_spi3.release(this);
+    }
 }
 
 // reset sensor by holding a pin high (or is it low?) for 10us.
diff --git a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde
index c44e4b8de3..12807d4dec 100644
--- a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde
+++ b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde
@@ -10,6 +10,7 @@
 #include <SPI3.h>               // SPI3 library
 #include <Arduino_Mega_ISR_Registry.h>
 #include <AP_PeriodicProcess.h>
+#include <AP_Semaphore.h>       // for removing conflict with dataflash on SPI3 bus
 #include "AP_OpticalFlow.h" // ArduCopter OpticalFlow Library
 
 ////////////////////////////////////////////////////////////////////////////////