AP_OpticalFlow: added ability to pass in semaphores for main spi bus to stop conflicts with dataflash on APM1

libraries/AP_OpticalFlow/AP_OpticalFlow.cpp

@@ -17,7 +17,7 @@ uint8_t AP_OpticalFlow::_num_calls; // number of times we have been called by 1k
 
 // init - initCommAPI parameter controls whether I2C/SPI interface is initialised (set to false if other devices are on the I2C/SPI bus and have already initialised the interface)
 bool
-AP_OpticalFlow::init(bool initCommAPI, AP_PeriodicProcess *scheduler)
+AP_OpticalFlow::init(bool initCommAPI, AP_PeriodicProcess *scheduler, AP_Semaphore* spi_semaphore, AP_Semaphore* spi3_semaphore)
 {
     _orientation = ROTATION_NONE;
     update_conversion_factors();

libraries/AP_OpticalFlow/AP_OpticalFlow.h

@@ -21,6 +21,7 @@
 #include <AP_Math.h>
 #include <AP_Common.h>
 #include <AP_PeriodicProcess.h>
+#include <AP_Semaphore.h>
 
 #define AP_OPTICALFLOW_NUM_CALLS_FOR_10HZ     100         // timer process runs at 1khz.  100 iterations = 10hz
 #define AP_OPTICALFLOW_NUM_CALLS_FOR_20HZ     50          // timer process runs at 1khz.  50 iterations = 20hz
@@ -49,7 +50,7 @@ public:
     ~AP_OpticalFlow() {
         _sensor = NULL;
     };
-    virtual bool                    init(bool initCommAPI, AP_PeriodicProcess *scheduler); // parameter controls whether I2C/SPI interface is initialised (set to false if other devices are on the I2C/SPI bus and have already initialised the interface)
+    virtual bool                    init(bool initCommAPI, AP_PeriodicProcess *scheduler, AP_Semaphore* spi_semaphore = NULL, AP_Semaphore* spi3_semaphore = NULL); // parameter controls whether I2C/SPI interface is initialised (set to false if other devices are on the I2C/SPI bus and have already initialised the interface)
     virtual byte                    read_register(byte address);
     virtual void                    write_register(byte address, byte value);
     virtual void                    set_orientation(enum Rotation rotation); // Rotation vector to transform sensor readings to the body frame.

libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp

@@ -41,11 +41,11 @@ union NumericIntType
 };
 
 // Constructors ////////////////////////////////////////////////////////////////
-AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080(AP_Semaphore* semaphore, int16_t cs_pin, int16_t reset_pin) :
-    _semaphore(semaphore),
+AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080(int16_t cs_pin, int16_t reset_pin) :
     _cs_pin(cs_pin),
     _reset_pin(reset_pin),
-    _spi_bus(ADNS3080_SPI_UNKNOWN)
+    _spi_bus(ADNS3080_SPI_UNKNOWN),
+    _spi_semaphore(NULL)
 {
     num_pixels = ADNS3080_PIXELS_X;
     field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV;
@@ -56,7 +56,7 @@ AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080(AP_Semaphore* semaphore, int16_
 // init - initialise sensor
 // assumes SPI bus has been initialised but will attempt to initialise nonstandard SPI3 bus if required
 bool
-AP_OpticalFlow_ADNS3080::init(bool initCommAPI, AP_PeriodicProcess *scheduler)
+AP_OpticalFlow_ADNS3080::init(bool initCommAPI, AP_PeriodicProcess *scheduler, AP_Semaphore* spi_semaphore, AP_Semaphore* spi3_semaphore)
 {
     int8_t retry = 0;
     bool retvalue = false;
@@ -84,6 +84,7 @@ AP_OpticalFlow_ADNS3080::init(bool initCommAPI, AP_PeriodicProcess *scheduler)
 
     // check 3 times for the sensor on standard SPI bus
     _spi_bus = ADNS3080_SPIBUS_1;
+    _spi_semaphore = spi_semaphore;
     while( retvalue == false && retry < 3 ) {
         if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) {
             retvalue = true;
@@ -102,6 +103,7 @@ AP_OpticalFlow_ADNS3080::init(bool initCommAPI, AP_PeriodicProcess *scheduler)
         }
 
         _spi_bus = ADNS3080_SPIBUS_3;
+        _spi_semaphore = spi3_semaphore;
         retry = 0;
         while( retvalue == false && retry < 3 ) {
             if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) {
@@ -183,9 +185,9 @@ AP_OpticalFlow_ADNS3080::read_register(byte address)
     uint8_t junk = 0;
 
     // get spi semaphore if required
-    if( _semaphore != NULL) {
+    if( _spi_semaphore != NULL ) {
         // if failed to get semaphore then just quietly fail
-        if( !_semaphore->get(this) ) {
+        if( !_spi_semaphore->get(this) ) {
             return 0;
         }
     }
@@ -211,8 +213,8 @@ AP_OpticalFlow_ADNS3080::read_register(byte address)
     restore_spi_settings();
 
     // get spi semaphore if required
-    if( _semaphore != NULL) {
-        _semaphore->release(this);
+    if( _spi_semaphore != NULL ) {
+        _spi_semaphore->release(this);
     }
 
     return result;
@@ -225,10 +227,9 @@ AP_OpticalFlow_ADNS3080::write_register(byte address, byte value)
     byte junk = 0;
 
     // get spi semaphore if required
-    if( _semaphore != NULL) {
+    if( _spi_semaphore != NULL ) {
         // if failed to get semaphore then just quietly fail
-        if( !_semaphore->get(this) ) {
-            Serial.println("Optflow: failed to get spi3 semaphore!");
+        if( !_spi_semaphore->get(this) ) {
             return;
         }
     }
@@ -254,8 +255,8 @@ AP_OpticalFlow_ADNS3080::write_register(byte address, byte value)
     restore_spi_settings();
 
     // get spi3 semaphore if required
-    if( _semaphore != NULL) {
-        _semaphore->release(this);
+    if( _spi_semaphore != NULL ) {
+        _spi_semaphore->release(this);
     }
 }
 

libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h

@@ -83,8 +83,8 @@
 class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow
 {
 public:
-    AP_OpticalFlow_ADNS3080(AP_Semaphore* semaphore, int16_t cs_pin = ADNS3080_CHIP_SELECT, int16_t reset_pin = ADNS3080_RESET);
-    bool        init(bool initCommAPI, AP_PeriodicProcess *scheduler); // parameter controls whether I2C/SPI interface is initialised (set to false if other devices are on the I2C/SPI bus and have already initialised the interface)
+    AP_OpticalFlow_ADNS3080(int16_t cs_pin = ADNS3080_CHIP_SELECT, int16_t reset_pin = ADNS3080_RESET);
+    bool        init(bool initCommAPI, AP_PeriodicProcess *scheduler, AP_Semaphore* spi_semaphore = NULL, AP_Semaphore* spi3_semaphore = NULL); // parameter controls whether SPI interface is initialised (set to false if other devices are on the I2C/SPI bus and have already initialised the interface)
     uint8_t     read_register(uint8_t address);
     void        write_register(uint8_t address, uint8_t value);
     void        reset();      // reset sensor by holding a pin high (or is it low?) for 10us.
@@ -139,7 +139,7 @@ private:
     bool        _motion; // true if there has been motion
     bool        _overflow; // true if the x or y data buffers overflowed
     uint8_t     _spi_bus;   // 0 = unknown, 1 = using SPI, 3 = using SPI3
-    AP_Semaphore* _semaphore;
+    AP_Semaphore* _spi_semaphore;
 };
 
 #endif

libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde

@@ -27,7 +27,7 @@ FastSerialPort0(Serial);        // FTDI/console
 #define CONFIG_MPU6000_CHIP_SELECT_PIN 53   // MPU600 chip select pin
 
 //AP_OpticalFlow_ADNS3080 flowSensor;             // for APM1
-AP_OpticalFlow_ADNS3080 flowSensor(NULL,A3);  // override chip select pin to use A3 if using APM2 or APM2.5
+AP_OpticalFlow_ADNS3080 flowSensor(A3);  // override chip select pin to use A3 if using APM2 or APM2.5
 
 Arduino_Mega_ISR_Registry isr_registry;
 AP_TimerProcess scheduler;