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

Also fixed test sketch which seemed broken.
---
 libraries/DataFlash/DataFlash_APM2.cpp         | 18 +++++++++++++++---
 libraries/DataFlash/DataFlash_APM2.h           |  1 +
 .../examples/DataFlash_test/DataFlash_test.pde | 14 ++++++++++++++
 3 files changed, 30 insertions(+), 3 deletions(-)

diff --git a/libraries/DataFlash/DataFlash_APM2.cpp b/libraries/DataFlash/DataFlash_APM2.cpp
index 392eee6b55..3ef762ac66 100644
--- a/libraries/DataFlash/DataFlash_APM2.cpp
+++ b/libraries/DataFlash/DataFlash_APM2.cpp
@@ -37,8 +37,7 @@ extern "C" {
 #include <inttypes.h>
 #include <avr/interrupt.h>
 }
-//#include <FastSerial.h>
-//#include <AP_Common.h>
+#include <FastSerial.h>
 
 #if defined(ARDUINO) && ARDUINO >= 100
  #include "Arduino.h"
@@ -46,6 +45,7 @@ extern "C" {
  #include "WConstants.h"
 #endif
 
+#include <AP_Semaphore.h>               // for removing conflict with optical flow sensor on SPI3 bus
 #include "DataFlash_APM2.h"
 /*
  * #define ENABLE_FASTSERIAL_DEBUG
@@ -93,6 +93,13 @@ extern "C" {
 // *** INTERNAL FUNCTIONS ***
 unsigned char DataFlash_APM2::SPI_transfer(unsigned char data)
 {
+    unsigned char retval;
+
+    // get spi3 semaphore if required.  if failed to get semaphore then just quietly fail
+    if( !AP_Semaphore_spi3.get(this) ) {
+        return 0;
+    }
+
     /* Wait for empty transmit buffer */
     while ( !( UCSR3A & (1<<UDRE3)) ) ;
     /* Put data into buffer, sends the data */
@@ -100,7 +107,12 @@ unsigned char DataFlash_APM2::SPI_transfer(unsigned char data)
     /* Wait for data to be received */
     while ( !(UCSR3A & (1<<RXC3)) ) ;
     /* Get and return received data from buffer */
-    return UDR3;
+    retval = UDR3;
+
+    // release spi3 semaphore
+    AP_Semaphore_spi3.release(this);
+
+    return retval;
 }
 
 // disable device
diff --git a/libraries/DataFlash/DataFlash_APM2.h b/libraries/DataFlash/DataFlash_APM2.h
index c43199c5d6..6c04070ef3 100644
--- a/libraries/DataFlash/DataFlash_APM2.h
+++ b/libraries/DataFlash/DataFlash_APM2.h
@@ -4,6 +4,7 @@
 #ifndef __DATAFLASH_APM2_H__
 #define __DATAFLASH_APM2_H__
 
+#include <AP_Semaphore.h>       // for removing conflict with dataflash on SPI3 bus
 #include "DataFlash.h"
 
 class DataFlash_APM2 : public DataFlash_Class
diff --git a/libraries/DataFlash/examples/DataFlash_test/DataFlash_test.pde b/libraries/DataFlash/examples/DataFlash_test/DataFlash_test.pde
index 3b7e9b0ea1..0a249dfa68 100644
--- a/libraries/DataFlash/examples/DataFlash_test/DataFlash_test.pde
+++ b/libraries/DataFlash/examples/DataFlash_test/DataFlash_test.pde
@@ -4,9 +4,23 @@
  */
 
 // Libraries
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
 #include <SPI.h>                        // Arduino SPI lib
+#include <AP_Semaphore.h>               // for removing conflict with dataflash on SPI3 bus
 #include <DataFlash.h>
 
+////////////////////////////////////////////////////////////////////////////////
+// Serial ports
+////////////////////////////////////////////////////////////////////////////////
+//
+// Note that FastSerial port buffers are allocated at ::begin time,
+// so there is not much of a penalty to defining ports that we don't
+// use.
+//
+FastSerialPort0(Serial);        // FTDI/console
+
 #define HEAD_BYTE1 0xA3
 #define HEAD_BYTE2 0x95