mirror of https://github.com/ArduPilot/ardupilot
AP_OpticalFlow_ADNS3080: detect if optical flow sensor is on standard or secondary SPI bus
This commit is contained in:
parent
17ccc7e62e
commit
99eaa37a38
|
@ -7,24 +7,11 @@
|
||||||
* License as published by the Free Software Foundation; either
|
* License as published by the Free Software Foundation; either
|
||||||
* version 2.1 of the License, or (at your option) any later version.
|
* version 2.1 of the License, or (at your option) any later version.
|
||||||
*
|
*
|
||||||
* External ADNS3080 OpticalFlow is connected via Serial port 2 (in SPI mode)
|
|
||||||
* TXD2 = MOSI = pin PH1
|
|
||||||
* RXD2 = MISO = pin PH0
|
|
||||||
* XCK2 = SCK = pin PH2
|
|
||||||
* Chip Select pin is PC4 (33) [PH6 (9)]
|
|
||||||
* We are using the 16 clocks per conversion timming to increase efficiency (fast)
|
|
||||||
* The sampling frequency is 400Hz (Timer2 overflow interrupt)
|
|
||||||
* So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so
|
|
||||||
* we have an 4x oversampling and averaging.
|
|
||||||
*
|
|
||||||
* Methods:
|
|
||||||
* Init() : Initialization of interrupts an Timers (Timer2 overflow interrupt)
|
|
||||||
* Read() : Read latest values from OpticalFlow and store to x,y, surface_quality parameters
|
|
||||||
*
|
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "AP_OpticalFlow_ADNS3080.h"
|
#include "AP_OpticalFlow_ADNS3080.h"
|
||||||
#include "SPI.h"
|
#include "SPI.h"
|
||||||
|
#include "SPI3.h"
|
||||||
#if defined(ARDUINO) && ARDUINO >= 100
|
#if defined(ARDUINO) && ARDUINO >= 100
|
||||||
#include "Arduino.h"
|
#include "Arduino.h"
|
||||||
#else
|
#else
|
||||||
|
@ -35,42 +22,45 @@
|
||||||
|
|
||||||
// We use Serial Port 2 in SPI Mode
|
// We use Serial Port 2 in SPI Mode
|
||||||
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
|
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
|
||||||
#define AP_SPI_DATAIN 50 // MISO // PB3
|
#define ADNS3080_SPI_MISO 50 // PB3
|
||||||
#define AP_SPI_DATAOUT 51 // MOSI // PB2
|
#define ADNS3080_SPI_MOSI 51 // PB2
|
||||||
#define AP_SPI_CLOCK 52 // SCK // PB1
|
#define ADNS3080_SPI_SCK 52 // PB1
|
||||||
#else // normal arduino SPI pins...these need to be checked
|
#else // normal arduino SPI pins...these need to be checked
|
||||||
#define AP_SPI_DATAIN 12 //MISO
|
#define ADNS3080_SPI_MISO 12 // MISO
|
||||||
#define AP_SPI_DATAOUT 11 //MOSI
|
#define ADNS3080_SPI_MOSI 11 // MOSI
|
||||||
#define AP_SPI_CLOCK 13 //SCK
|
#define ADNS3080_SPI_SCK 13 // SCK
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
union NumericIntType
|
union NumericIntType
|
||||||
{
|
{
|
||||||
int intValue;
|
int16_t intValue;
|
||||||
unsigned int uintValue;
|
uint16_t uintValue;
|
||||||
byte byteValue[2];
|
uint8_t byteValue[2];
|
||||||
};
|
};
|
||||||
|
|
||||||
// Constructors ////////////////////////////////////////////////////////////////
|
// Constructors ////////////////////////////////////////////////////////////////
|
||||||
AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080(int cs_pin, int reset_pin) : _cs_pin(cs_pin), _reset_pin(reset_pin)
|
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)
|
||||||
{
|
{
|
||||||
num_pixels = ADNS3080_PIXELS_X;
|
num_pixels = ADNS3080_PIXELS_X;
|
||||||
field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV;
|
field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV;
|
||||||
scaler = AP_OPTICALFLOW_ADNS3080_SCALER;
|
scaler = AP_OPTICALFLOW_ADNS3080_SCALER;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// Public Methods //////////////////////////////////////////////////////////////
|
// Public Methods //////////////////////////////////////////////////////////////
|
||||||
// init - initialise sensor
|
// init - initialise sensor
|
||||||
// initCommAPI parameter controls whether SPI interface is initialised (set to false if other devices are on the SPI bus and have already initialised the interface)
|
// assumes SPI bus has been initialised but will attempt to initialise nonstandard SPI3 bus if required
|
||||||
bool
|
bool
|
||||||
AP_OpticalFlow_ADNS3080::init(bool initCommAPI)
|
AP_OpticalFlow_ADNS3080::init(bool initCommAPI, AP_PeriodicProcess *scheduler)
|
||||||
{
|
{
|
||||||
int retry = 0;
|
int8_t retry = 0;
|
||||||
|
bool retvalue = false;
|
||||||
|
|
||||||
|
// suspend timer while we set-up SPI communication
|
||||||
|
scheduler->suspend_timer();
|
||||||
|
|
||||||
pinMode(AP_SPI_DATAOUT,OUTPUT);
|
|
||||||
pinMode(AP_SPI_DATAIN,INPUT);
|
|
||||||
pinMode(AP_SPI_CLOCK,OUTPUT);
|
|
||||||
pinMode(_cs_pin,OUTPUT);
|
pinMode(_cs_pin,OUTPUT);
|
||||||
if( _reset_pin != 0)
|
if( _reset_pin != 0)
|
||||||
pinMode(ADNS3080_RESET,OUTPUT);
|
pinMode(ADNS3080_RESET,OUTPUT);
|
||||||
|
@ -82,77 +72,127 @@ AP_OpticalFlow_ADNS3080::init(bool initCommAPI)
|
||||||
|
|
||||||
// start the SPI library:
|
// start the SPI library:
|
||||||
if( initCommAPI ) {
|
if( initCommAPI ) {
|
||||||
|
pinMode(ADNS3080_SPI_MOSI,OUTPUT);
|
||||||
|
pinMode(ADNS3080_SPI_MISO,INPUT);
|
||||||
|
pinMode(ADNS3080_SPI_SCK,OUTPUT);
|
||||||
SPI.begin();
|
SPI.begin();
|
||||||
|
SPI.setClockDivider(SPI_CLOCK_DIV8); // 2MHZ SPI rate
|
||||||
}
|
}
|
||||||
|
|
||||||
// check the sensor is functioning
|
// check 3 times for the sensor on standard SPI bus
|
||||||
while( retry < 3 ) {
|
_spi_bus = ADNS3080_SPIBUS_1;
|
||||||
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 )
|
while( retvalue == false && retry < 3 ) {
|
||||||
return true;
|
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) {
|
||||||
|
retvalue = true;
|
||||||
|
}
|
||||||
retry++;
|
retry++;
|
||||||
}
|
}
|
||||||
|
|
||||||
return false;
|
// if not found, check 3 times on SPI3
|
||||||
|
if( !retvalue ) {
|
||||||
|
|
||||||
|
// start the SPI3 library:
|
||||||
|
if( initCommAPI ) {
|
||||||
|
SPI3.begin();
|
||||||
|
SPI3.setDataMode(SPI3_MODE3); // Mode3
|
||||||
|
SPI3.setSpeed(SPI3_SPEED_2MHZ); // 2MHZ SPI rate
|
||||||
|
}
|
||||||
|
|
||||||
|
_spi_bus = ADNS3080_SPIBUS_3;
|
||||||
|
retry = 0;
|
||||||
|
while( retvalue == false && retry < 3 ) {
|
||||||
|
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) {
|
||||||
|
retvalue = true;
|
||||||
|
}
|
||||||
|
retry++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// resume timer
|
||||||
|
scheduler->resume_timer();
|
||||||
|
|
||||||
|
// if device is working register the global static read function to be called at 1khz
|
||||||
|
if( retvalue ) {
|
||||||
|
scheduler->register_process( AP_OpticalFlow_ADNS3080::read );
|
||||||
|
}else{
|
||||||
|
_spi_bus = ADNS3080_SPI_UNKNOWN;
|
||||||
|
}
|
||||||
|
|
||||||
|
return retvalue;
|
||||||
}
|
}
|
||||||
|
|
||||||
//
|
//
|
||||||
// backup_spi_settings - checks current SPI settings (clock speed, etc), sets values to what we need
|
// backup_spi_settings - checks current SPI settings (clock speed, etc), sets values to what we need
|
||||||
//
|
//
|
||||||
byte
|
void
|
||||||
AP_OpticalFlow_ADNS3080::backup_spi_settings()
|
AP_OpticalFlow_ADNS3080::backup_spi_settings()
|
||||||
{
|
{
|
||||||
// store current spi values
|
if( _spi_bus == ADNS3080_SPIBUS_1 ) {
|
||||||
orig_spi_settings_spcr = SPCR & (DORD | CPOL | CPHA);
|
// store current spi mode
|
||||||
orig_spi_settings_spsr = SPSR & SPI2X;
|
orig_spi_settings_spcr = SPCR & (CPOL | CPHA);
|
||||||
|
|
||||||
// set the values that we need
|
// set to our required values
|
||||||
SPI.setBitOrder(MSBFIRST);
|
|
||||||
SPI.setDataMode(SPI_MODE3);
|
SPI.setDataMode(SPI_MODE3);
|
||||||
SPI.setClockDivider(SPI_CLOCK_DIV8); // sensor running at 2Mhz. this is it's maximum speed
|
// we do not set speed to 2Mhz because we assume it is that already no more than 2Mhz
|
||||||
|
|
||||||
return orig_spi_settings_spcr;
|
}else if( _spi_bus == ADNS3080_SPIBUS_3 ) {
|
||||||
|
/* Wait for empty transmit buffer */
|
||||||
|
while ( !( UCSR3A & (1<<UDRE3)) ) ;
|
||||||
|
|
||||||
|
// store current spi values
|
||||||
|
orig_spi3_settings_ucsr3c = UCSR3C;
|
||||||
|
orig_spi3_settings_ubrr3 = UBRR3;
|
||||||
|
|
||||||
|
// set to our required values
|
||||||
|
SPI3.setDataMode(SPI3_MODE3);
|
||||||
|
SPI3.setSpeed(SPI3_SPEED_2MHZ); // 2MHZ SPI rate
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// restore_spi_settings - restores SPI settings (clock speed, etc) to what their values were before the sensor used the bus
|
// restore_spi_settings - restores SPI settings (clock speed, etc) to what their values were before the sensor used the bus
|
||||||
byte
|
void
|
||||||
AP_OpticalFlow_ADNS3080::restore_spi_settings()
|
AP_OpticalFlow_ADNS3080::restore_spi_settings()
|
||||||
{
|
{
|
||||||
byte temp;
|
byte temp;
|
||||||
|
|
||||||
// restore SPSR
|
if( _spi_bus == ADNS3080_SPIBUS_1 ) {
|
||||||
temp = SPSR;
|
// split off the two bits we need to write
|
||||||
temp &= ~SPI2X;
|
temp = SPCR & ~(CPOL | CPHA);
|
||||||
temp |= orig_spi_settings_spsr;
|
temp |= orig_spi_settings_spcr;
|
||||||
SPSR = temp;
|
|
||||||
|
|
||||||
// restore SPCR
|
// write back the bits
|
||||||
temp = SPCR;
|
|
||||||
temp &= ~(DORD | CPOL | CPHA); // zero out the important bits
|
|
||||||
temp |= orig_spi_settings_spcr; // restore important bits
|
|
||||||
SPCR = temp;
|
SPCR = temp;
|
||||||
|
}else if( _spi_bus == ADNS3080_SPIBUS_3 ) {
|
||||||
|
/* Wait for empty transmit buffer */
|
||||||
|
while ( !( UCSR3A & (1<<UDRE3)) ) ;
|
||||||
|
|
||||||
return temp;
|
// restore UCSRC3C (spi mode) and UBBR3 (speed)
|
||||||
|
UCSR3C = orig_spi3_settings_ucsr3c;
|
||||||
|
UBRR3 = orig_spi3_settings_ubrr3;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Read a register from the sensor
|
// Read a register from the sensor
|
||||||
byte
|
byte
|
||||||
AP_OpticalFlow_ADNS3080::read_register(byte address)
|
AP_OpticalFlow_ADNS3080::read_register(byte address)
|
||||||
{
|
{
|
||||||
byte result = 0, junk = 0;
|
uint8_t result = 0;
|
||||||
|
uint8_t junk = 0;
|
||||||
|
|
||||||
backup_spi_settings();
|
backup_spi_settings();
|
||||||
|
|
||||||
// take the chip select low to select the device
|
// take the chip select low to select the device
|
||||||
digitalWrite(_cs_pin, LOW);
|
digitalWrite(_cs_pin, LOW);
|
||||||
|
|
||||||
// send the device the register you want to read:
|
if( _spi_bus == ADNS3080_SPIBUS_1 ) {
|
||||||
junk = SPI.transfer(address);
|
junk = SPI.transfer(address); // send the device the register you want to read:
|
||||||
|
delayMicroseconds(50); // small delay
|
||||||
// small delay
|
result = SPI.transfer(0x00); // send a value of 0 to read the first byte returned:
|
||||||
delayMicroseconds(50);
|
}else if( _spi_bus == ADNS3080_SPIBUS_3 ) {
|
||||||
|
junk = SPI3.transfer(address); // send the device the register you want to read:
|
||||||
// send a value of 0 to read the first byte returned:
|
delayMicroseconds(50); // small delay
|
||||||
result = SPI.transfer(0x00);
|
result = SPI3.transfer(0x00); // send a value of 0 to read the first byte returned:
|
||||||
|
}
|
||||||
|
|
||||||
// take the chip select high to de-select:
|
// take the chip select high to de-select:
|
||||||
digitalWrite(_cs_pin, HIGH);
|
digitalWrite(_cs_pin, HIGH);
|
||||||
|
@ -173,14 +213,15 @@ AP_OpticalFlow_ADNS3080::write_register(byte address, byte value)
|
||||||
// take the chip select low to select the device
|
// take the chip select low to select the device
|
||||||
digitalWrite(_cs_pin, LOW);
|
digitalWrite(_cs_pin, LOW);
|
||||||
|
|
||||||
// send register address
|
if( _spi_bus == ADNS3080_SPIBUS_1 ) {
|
||||||
junk = SPI.transfer(address | 0x80 );
|
junk = SPI.transfer(address | 0x80 ); // send register address
|
||||||
|
delayMicroseconds(50); // small delay
|
||||||
// small delay
|
junk = SPI.transfer(value); // send data
|
||||||
delayMicroseconds(50);
|
}else if( _spi_bus == ADNS3080_SPIBUS_3 ) {
|
||||||
|
junk = SPI3.transfer(address | 0x80 ); // send register address
|
||||||
// send data
|
delayMicroseconds(50); // small delay
|
||||||
junk = SPI.transfer(value);
|
junk = SPI3.transfer(value); // send data
|
||||||
|
}
|
||||||
|
|
||||||
// take the chip select high to de-select:
|
// take the chip select high to de-select:
|
||||||
digitalWrite(_cs_pin, HIGH);
|
digitalWrite(_cs_pin, HIGH);
|
||||||
|
@ -202,20 +243,20 @@ AP_OpticalFlow_ADNS3080::reset()
|
||||||
}
|
}
|
||||||
|
|
||||||
// read latest values from sensor and fill in x,y and totals
|
// read latest values from sensor and fill in x,y and totals
|
||||||
bool
|
void
|
||||||
AP_OpticalFlow_ADNS3080::update()
|
AP_OpticalFlow_ADNS3080::update(uint32_t now)
|
||||||
{
|
{
|
||||||
byte motion_reg;
|
byte motion_reg;
|
||||||
surface_quality = (unsigned int)read_register(ADNS3080_SQUAL);
|
surface_quality = (uint16_t)read_register(ADNS3080_SQUAL);
|
||||||
delayMicroseconds(50); // small delay
|
delayMicroseconds(50); // small delay
|
||||||
|
|
||||||
// check for movement, update x,y values
|
// check for movement, update x,y values
|
||||||
motion_reg = read_register(ADNS3080_MOTION);
|
motion_reg = read_register(ADNS3080_MOTION);
|
||||||
_overflow = ((motion_reg & 0x10) != 0); // check if we've had an overflow
|
_overflow = ((motion_reg & 0x10) != 0); // check if we've had an overflow
|
||||||
if( (motion_reg & 0x80) != 0 ) {
|
if( (motion_reg & 0x80) != 0 ) {
|
||||||
raw_dx = ((char)read_register(ADNS3080_DELTA_X));
|
raw_dx = ((int8_t)read_register(ADNS3080_DELTA_X));
|
||||||
delayMicroseconds(50); // small delay
|
delayMicroseconds(50); // small delay
|
||||||
raw_dy = ((char)read_register(ADNS3080_DELTA_Y));
|
raw_dy = ((int8_t)read_register(ADNS3080_DELTA_Y));
|
||||||
_motion = true;
|
_motion = true;
|
||||||
}else{
|
}else{
|
||||||
raw_dx = 0;
|
raw_dx = 0;
|
||||||
|
@ -225,8 +266,6 @@ AP_OpticalFlow_ADNS3080::update()
|
||||||
last_update = millis();
|
last_update = millis();
|
||||||
|
|
||||||
apply_orientation_matrix();
|
apply_orientation_matrix();
|
||||||
|
|
||||||
return true;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
@ -256,7 +295,7 @@ AP_OpticalFlow_ADNS3080::set_led_always_on( bool alwaysOn )
|
||||||
}
|
}
|
||||||
|
|
||||||
// returns resolution (either 400 or 1600 counts per inch)
|
// returns resolution (either 400 or 1600 counts per inch)
|
||||||
int
|
int16_t
|
||||||
AP_OpticalFlow_ADNS3080::get_resolution()
|
AP_OpticalFlow_ADNS3080::get_resolution()
|
||||||
{
|
{
|
||||||
if( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x10) == 0 )
|
if( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x10) == 0 )
|
||||||
|
@ -267,7 +306,7 @@ AP_OpticalFlow_ADNS3080::get_resolution()
|
||||||
|
|
||||||
// set parameter to 400 or 1600 counts per inch
|
// set parameter to 400 or 1600 counts per inch
|
||||||
void
|
void
|
||||||
AP_OpticalFlow_ADNS3080::set_resolution(int resolution)
|
AP_OpticalFlow_ADNS3080::set_resolution(uint16_t resolution)
|
||||||
{
|
{
|
||||||
byte regVal = read_register(ADNS3080_CONFIGURATION_BITS);
|
byte regVal = read_register(ADNS3080_CONFIGURATION_BITS);
|
||||||
|
|
||||||
|
@ -321,7 +360,7 @@ AP_OpticalFlow_ADNS3080::set_frame_rate_auto(bool auto_frame_rate)
|
||||||
}
|
}
|
||||||
|
|
||||||
// get frame period
|
// get frame period
|
||||||
unsigned int
|
uint16_t
|
||||||
AP_OpticalFlow_ADNS3080::get_frame_period()
|
AP_OpticalFlow_ADNS3080::get_frame_period()
|
||||||
{
|
{
|
||||||
NumericIntType aNum;
|
NumericIntType aNum;
|
||||||
|
@ -349,19 +388,19 @@ AP_OpticalFlow_ADNS3080::set_frame_period(unsigned int period)
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
unsigned int
|
uint16_t
|
||||||
AP_OpticalFlow_ADNS3080::get_frame_rate()
|
AP_OpticalFlow_ADNS3080::get_frame_rate()
|
||||||
{
|
{
|
||||||
unsigned long clockSpeed = ADNS3080_CLOCK_SPEED;
|
uint32_t clockSpeed = ADNS3080_CLOCK_SPEED;
|
||||||
unsigned int rate = clockSpeed / get_frame_period();
|
uint16_t rate = clockSpeed / get_frame_period();
|
||||||
return rate;
|
return rate;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
AP_OpticalFlow_ADNS3080::set_frame_rate(unsigned int rate)
|
AP_OpticalFlow_ADNS3080::set_frame_rate(uint16_t rate)
|
||||||
{
|
{
|
||||||
unsigned long clockSpeed = ADNS3080_CLOCK_SPEED;
|
uint32_t clockSpeed = ADNS3080_CLOCK_SPEED;
|
||||||
unsigned int period = (unsigned int)(clockSpeed / (unsigned long)rate);
|
uint16_t period = (uint16_t)(clockSpeed / (uint32_t)rate);
|
||||||
|
|
||||||
set_frame_period(period);
|
set_frame_period(period);
|
||||||
}
|
}
|
||||||
|
@ -370,7 +409,7 @@ AP_OpticalFlow_ADNS3080::set_frame_rate(unsigned int rate)
|
||||||
bool
|
bool
|
||||||
AP_OpticalFlow_ADNS3080::get_shutter_speed_auto()
|
AP_OpticalFlow_ADNS3080::get_shutter_speed_auto()
|
||||||
{
|
{
|
||||||
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
|
uint8_t regVal = read_register(ADNS3080_EXTENDED_CONFIG);
|
||||||
if( (regVal & 0x02) > 0 ) {
|
if( (regVal & 0x02) > 0 ) {
|
||||||
return false;
|
return false;
|
||||||
}else{
|
}else{
|
||||||
|
@ -382,7 +421,7 @@ AP_OpticalFlow_ADNS3080::get_shutter_speed_auto()
|
||||||
void
|
void
|
||||||
AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool auto_shutter_speed)
|
AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool auto_shutter_speed)
|
||||||
{
|
{
|
||||||
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
|
uint8_t regVal = read_register(ADNS3080_EXTENDED_CONFIG);
|
||||||
delayMicroseconds(50); // small delay
|
delayMicroseconds(50); // small delay
|
||||||
if( auto_shutter_speed ) {
|
if( auto_shutter_speed ) {
|
||||||
// return shutter speed max to default
|
// return shutter speed max to default
|
||||||
|
@ -402,7 +441,7 @@ AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool auto_shutter_speed)
|
||||||
}
|
}
|
||||||
|
|
||||||
// get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual
|
// get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual
|
||||||
unsigned int
|
uint16_t
|
||||||
AP_OpticalFlow_ADNS3080::get_shutter_speed()
|
AP_OpticalFlow_ADNS3080::get_shutter_speed()
|
||||||
{
|
{
|
||||||
NumericIntType aNum;
|
NumericIntType aNum;
|
||||||
|
@ -460,10 +499,10 @@ AP_OpticalFlow_ADNS3080::clear_motion()
|
||||||
void
|
void
|
||||||
AP_OpticalFlow_ADNS3080::print_pixel_data(Stream *serPort)
|
AP_OpticalFlow_ADNS3080::print_pixel_data(Stream *serPort)
|
||||||
{
|
{
|
||||||
int i,j;
|
int16_t i,j;
|
||||||
bool isFirstPixel = true;
|
bool isFirstPixel = true;
|
||||||
byte regValue;
|
uint8_t regValue;
|
||||||
byte pixelValue;
|
uint8_t pixelValue;
|
||||||
|
|
||||||
// write to frame capture register to force capture of frame
|
// write to frame capture register to force capture of frame
|
||||||
write_register(ADNS3080_FRAME_CAPTURE,0x83);
|
write_register(ADNS3080_FRAME_CAPTURE,0x83);
|
||||||
|
|
|
@ -74,62 +74,70 @@
|
||||||
#define ADNS3080_FRAME_RATE_MAX 6469
|
#define ADNS3080_FRAME_RATE_MAX 6469
|
||||||
#define ADNS3080_FRAME_RATE_MIN 2000
|
#define ADNS3080_FRAME_RATE_MIN 2000
|
||||||
|
|
||||||
|
// SPI bus definitions
|
||||||
|
#define ADNS3080_SPI_UNKNOWN 0
|
||||||
|
#define ADNS3080_SPIBUS_1 1 // standard SPI bus
|
||||||
|
#define ADNS3080_SPIBUS_3 3 // SPI3
|
||||||
|
|
||||||
class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow
|
class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow
|
||||||
{
|
{
|
||||||
private:
|
|
||||||
// bytes to store SPI settings
|
|
||||||
byte orig_spi_settings_spcr;
|
|
||||||
byte orig_spi_settings_spsr;
|
|
||||||
|
|
||||||
// save and restore SPI settings
|
|
||||||
byte backup_spi_settings();
|
|
||||||
byte restore_spi_settings();
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
int _cs_pin; // pin used for chip select
|
AP_OpticalFlow_ADNS3080(int16_t cs_pin = ADNS3080_CHIP_SELECT, int16_t reset_pin = ADNS3080_RESET);
|
||||||
int _reset_pin; // pin used for chip 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)
|
||||||
bool _motion; // true if there has been motion
|
uint8_t read_register(uint8_t address);
|
||||||
bool _overflow; // true if the x or y data buffers overflowed
|
void write_register(uint8_t address, uint8_t value);
|
||||||
|
|
||||||
public:
|
|
||||||
AP_OpticalFlow_ADNS3080(int cs_pin = ADNS3080_CHIP_SELECT, int reset_pin = ADNS3080_RESET);
|
|
||||||
bool init(bool initCommAPI = true); // 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)
|
|
||||||
byte read_register(byte address);
|
|
||||||
void write_register(byte address, byte value);
|
|
||||||
void reset(); // reset sensor by holding a pin high (or is it low?) for 10us.
|
void reset(); // reset sensor by holding a pin high (or is it low?) for 10us.
|
||||||
bool update(); // read latest values from sensor and fill in x,y and totals, return true on successful read
|
void update(uint32_t now); // read latest values from sensor and fill in x,y and totals, return true on successful read
|
||||||
|
|
||||||
// ADNS3080 specific features
|
// ADNS3080 specific features
|
||||||
bool motion() {
|
|
||||||
if( _motion ) { _motion = false; return true; }else{ return false; }
|
// return true if there has been motion since the last time this was called
|
||||||
} // return true if there has been motion since the last time this was called
|
bool motion() { if( _motion ) { _motion = false; return true; }else{ return false; } }
|
||||||
|
|
||||||
|
bool overflow() { return _overflow; } // true if there has been an overflow
|
||||||
|
|
||||||
void disable_serial_pullup();
|
void disable_serial_pullup();
|
||||||
|
|
||||||
bool get_led_always_on(); // returns true if LED is always on, false if only on when required
|
bool get_led_always_on(); // returns true if LED is always on, false if only on when required
|
||||||
void set_led_always_on( bool alwaysOn ); // set parameter to true if you want LED always on, otherwise false for only when required
|
void set_led_always_on( bool alwaysOn ); // set parameter to true if you want LED always on, otherwise false for only when required
|
||||||
|
|
||||||
int get_resolution(); // returns resolution (either 400 or 1600 counts per inch)
|
int16_t get_resolution(); // returns resolution (either 400 or 1600 counts per inch)
|
||||||
void set_resolution(int resolution); // set parameter to 400 or 1600 counts per inch
|
void set_resolution(uint16_t resolution); // set parameter to 400 or 1600 counts per inch
|
||||||
|
|
||||||
bool get_frame_rate_auto(); // get_frame_rate_auto - return true if frame rate is set to "auto", false if manual
|
bool get_frame_rate_auto(); // get_frame_rate_auto - return true if frame rate is set to "auto", false if manual
|
||||||
void set_frame_rate_auto(bool auto_frame_rate); // set_frame_rate_auto(bool) - set frame rate to auto (true), or manual (false)
|
void set_frame_rate_auto(bool auto_frame_rate); // set_frame_rate_auto(bool) - set frame rate to auto (true), or manual (false)
|
||||||
|
|
||||||
unsigned int get_frame_period(); // get_frame_period -
|
uint16_t get_frame_period(); // get_frame_period
|
||||||
void set_frame_period(unsigned int period);
|
void set_frame_period(uint16_t period);
|
||||||
|
|
||||||
unsigned int get_frame_rate();
|
uint16_t get_frame_rate();
|
||||||
void set_frame_rate(unsigned int rate);
|
void set_frame_rate(uint16_t rate);
|
||||||
|
|
||||||
bool get_shutter_speed_auto(); // get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual
|
bool get_shutter_speed_auto(); // get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual
|
||||||
void set_shutter_speed_auto(bool auto_shutter_speed); // set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
|
void set_shutter_speed_auto(bool auto_shutter_speed); // set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
|
||||||
|
|
||||||
unsigned int get_shutter_speed();
|
uint16_t get_shutter_speed();
|
||||||
void set_shutter_speed(unsigned int shutter_speed);
|
void set_shutter_speed(uint16_t shutter_speed);
|
||||||
|
|
||||||
void clear_motion(); // will cause the x,y, dx, dy, and the sensor's motion registers to be cleared
|
void clear_motion(); // will cause the x,y, dx, dy, and the sensor's motion registers to be cleared
|
||||||
|
|
||||||
void print_pixel_data(Stream *serPort); // dumps a 30x30 image to the Serial port
|
void print_pixel_data(Stream *serPort); // dumps a 30x30 image to the Serial port
|
||||||
|
|
||||||
|
private:
|
||||||
|
// bytes to store SPI settings
|
||||||
|
uint8_t orig_spi_settings_spcr; // spi1's mode
|
||||||
|
uint8_t orig_spi3_settings_ucsr3c; // spi3's mode
|
||||||
|
uint8_t orig_spi3_settings_ubrr3; // spi3's speed
|
||||||
|
|
||||||
|
// save and restore SPI settings
|
||||||
|
void backup_spi_settings();
|
||||||
|
void restore_spi_settings();
|
||||||
|
|
||||||
|
int16_t _cs_pin; // pin used for chip select
|
||||||
|
int16_t _reset_pin; // pin used for chip reset
|
||||||
|
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
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
Loading…
Reference in New Issue