diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index 99b59cefc0..306ea24bac 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/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(); diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h index 7107d73c06..34eafc7fa4 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.h @@ -21,6 +21,7 @@ #include #include #include +#include #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. diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp index a9c2d2ff2d..0fc29d658d 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp +++ b/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); } } diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h index 4a180cb5f5..3a82c9f982 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h +++ b/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 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 5bd0c2087f..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 @@ -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;