Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

This commit is contained in:
rmackay9 2012-10-18 17:02:45 +09:00
parent 785d9ca6a7
commit a1b4531545
5 changed files with 21 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_OpticalFlow/AP_OpticalFlow.cpp
View File

@ -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();

3
libraries/AP_OpticalFlow/AP_OpticalFlow.h
View File

@ -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.

27
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp
View File

@ -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);
}
}

6
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h
View File

@ -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

2
libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde
View File

@ -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;