Skip to content
Snippets Groups Projects
Commit a7d9e181 authored by rmackay9's avatar rmackay9
Browse files

AP_OpticalFlow: added ability to pass in semaphores for main spi bus to stop...

AP_OpticalFlow: added ability to pass in semaphores for main spi bus to stop conflicts with dataflash on APM1
parent 5f2c9007
No related branches found
No related tags found
No related merge requests found
......@@ -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();
......
......@@ -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.
......
......@@ -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);
}
}
......
......@@ -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
......@@ -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;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment