mirror of https://github.com/ArduPilot/ardupilot
uncrustify libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080_APM2.cpp
This commit is contained in:
uncrustify
Pat Hickey
parent
8f5c22d448
commit
dcee71d442
|
|
@ -1,52 +1,52 @@
|
|||
/*
|
||||
AP_OpticalFlow_ADNS3080.cpp - ADNS3080 OpticalFlow Library for Ardupilot Mega
|
||||
Code by Randy Mackay. DIYDrones.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
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
|
||||
|
||||
*/
|
||||
* AP_OpticalFlow_ADNS3080.cpp - ADNS3080 OpticalFlow Library for Ardupilot Mega
|
||||
* Code by Randy Mackay. DIYDrones.com
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* 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 "SPI.h"
|
||||
#if defined(ARDUINO) && ARDUINO >= 100
|
||||
#include "Arduino.h"
|
||||
#include "Arduino.h"
|
||||
#else
|
||||
#include "WProgram.h"
|
||||
#include "WProgram.h"
|
||||
#endif
|
||||
|
||||
#define AP_SPI_TIMEOUT 1000
|
||||
|
||||
// We use Serial Port 2 in SPI Mode
|
||||
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
|
||||
#define AP_SPI_DATAIN 15 // MISO
|
||||
#define AP_SPI_DATAOUT 14 // MOSI
|
||||
#define AP_SPI_CLOCK PJ2 // SCK
|
||||
#define AP_SPI_DATAIN 15 // MISO
|
||||
#define AP_SPI_DATAOUT 14 // MOSI
|
||||
#define AP_SPI_CLOCK PJ2 // SCK
|
||||
#else // normal arduino SPI pins...these need to be checked
|
||||
# error Please check the Tools/Board menu to ensure you have selected Arduino Mega as your target.
|
||||
# error Please check the Tools/Board menu to ensure you have selected Arduino Mega as your target.
|
||||
#endif
|
||||
|
||||
// mask for saving bit order and data mode to avoid interference with other users of the bus
|
||||
#define UCSR3C_MASK 0x07
|
||||
|
||||
// SPI3 setting for UCSR3C
|
||||
#define SPI3_MODE_SPI 0xC0 // UMSEL31 = 1, UMSEL30 = 1
|
||||
#define SPI3_MODE_SPI 0xC0 // UMSEL31 = 1, UMSEL30 = 1
|
||||
|
||||
// settings for phase and polarity bits of UCSR3C
|
||||
#define SPI3_MODE_MASK 0x03
|
||||
|
|
@ -63,22 +63,22 @@
|
|||
|
||||
#define SPI3_SPEED 0x04 // 2 megahertz?
|
||||
|
||||
#define SPI3_DELAY 20 // delay in microseconds after sending data
|
||||
#define SPI3_DELAY 20 // delay in microseconds after sending data
|
||||
|
||||
|
||||
union NumericIntType
|
||||
{
|
||||
int intValue;
|
||||
unsigned int uintValue;
|
||||
byte byteValue[2];
|
||||
int intValue;
|
||||
unsigned int uintValue;
|
||||
byte byteValue[2];
|
||||
};
|
||||
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
AP_OpticalFlow_ADNS3080_APM2::AP_OpticalFlow_ADNS3080_APM2(int cs_pin, int reset_pin) : _cs_pin(cs_pin), _reset_pin(reset_pin)
|
||||
{
|
||||
num_pixels = ADNS3080_PIXELS_X;
|
||||
field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV;
|
||||
scaler = AP_OPTICALFLOW_ADNS3080_SCALER;
|
||||
num_pixels = ADNS3080_PIXELS_X;
|
||||
field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV;
|
||||
scaler = AP_OPTICALFLOW_ADNS3080_SCALER;
|
||||
}
|
||||
|
||||
// SPI Methods
|
||||
|
|
@ -86,17 +86,17 @@ AP_OpticalFlow_ADNS3080_APM2::AP_OpticalFlow_ADNS3080_APM2(int cs_pin, int reset
|
|||
unsigned char AP_OpticalFlow_ADNS3080_APM2::SPI_transfer(uint8_t data)
|
||||
{
|
||||
|
||||
/* Wait for empty transmit buffer */
|
||||
while ( !( UCSR3A & (1<<UDRE3)) );
|
||||
/* Wait for empty transmit buffer */
|
||||
while ( !( UCSR3A & (1<<UDRE3)) ) ;
|
||||
|
||||
/* Put data into buffer, sends the data */
|
||||
UDR3 = data;
|
||||
/* Put data into buffer, sends the data */
|
||||
UDR3 = data;
|
||||
|
||||
/* Wait for data to be received */
|
||||
while ( !(UCSR3A & (1<<RXC3)) );
|
||||
|
||||
/* Get and return received data from buffer */
|
||||
return UDR3;
|
||||
/* Wait for data to be received */
|
||||
while ( !(UCSR3A & (1<<RXC3)) ) ;
|
||||
|
||||
/* Get and return received data from buffer */
|
||||
return UDR3;
|
||||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
|
|
@ -107,42 +107,42 @@ AP_OpticalFlow_ADNS3080_APM2::init(bool initCommAPI)
|
|||
{
|
||||
int retry = 0;
|
||||
|
||||
pinMode(AP_SPI_DATAOUT,OUTPUT);
|
||||
pinMode(AP_SPI_DATAIN,INPUT);
|
||||
pinMode(AP_SPI_CLOCK,OUTPUT);
|
||||
pinMode(_cs_pin,OUTPUT);
|
||||
if( _reset_pin != 0)
|
||||
pinMode(ADNS3080_RESET,OUTPUT);
|
||||
pinMode(AP_SPI_DATAOUT,OUTPUT);
|
||||
pinMode(AP_SPI_DATAIN,INPUT);
|
||||
pinMode(AP_SPI_CLOCK,OUTPUT);
|
||||
pinMode(_cs_pin,OUTPUT);
|
||||
if( _reset_pin != 0)
|
||||
pinMode(ADNS3080_RESET,OUTPUT);
|
||||
|
||||
digitalWrite(_cs_pin,HIGH); // disable device (Chip select is active low)
|
||||
digitalWrite(_cs_pin,HIGH); // disable device (Chip select is active low)
|
||||
|
||||
// reset the device
|
||||
reset();
|
||||
// reset the device
|
||||
reset();
|
||||
|
||||
// start the SPI library:
|
||||
if( initCommAPI ) {
|
||||
// Setup Serial Port3 in SPI mode (MSPI), Mode 0, Clock: 8Mhz
|
||||
UBRR3 = 0;
|
||||
DDRJ |= (1<<PJ2); // SPI clock XCK3 (PJ2) as output. This enable SPI Master mode
|
||||
// put UART3 into SPI master mode
|
||||
UCSR3C = SPI3_MODE_SPI | SPI3_MODE;
|
||||
// Enable receiver and transmitter.
|
||||
UCSR3B = (1<<RXEN3)|(1<<TXEN3);
|
||||
// Set Baud rate
|
||||
UBRR3 = SPI3_SPEED; // SPI running at 8Mhz
|
||||
}
|
||||
|
||||
delay(10);
|
||||
// start the SPI library:
|
||||
if( initCommAPI ) {
|
||||
// Setup Serial Port3 in SPI mode (MSPI), Mode 0, Clock: 8Mhz
|
||||
UBRR3 = 0;
|
||||
DDRJ |= (1<<PJ2); // SPI clock XCK3 (PJ2) as output. This enable SPI Master mode
|
||||
// put UART3 into SPI master mode
|
||||
UCSR3C = SPI3_MODE_SPI | SPI3_MODE;
|
||||
// Enable receiver and transmitter.
|
||||
UCSR3B = (1<<RXEN3)|(1<<TXEN3);
|
||||
// Set Baud rate
|
||||
UBRR3 = SPI3_SPEED; // SPI running at 8Mhz
|
||||
}
|
||||
|
||||
// check the sensor is functioning
|
||||
while( retry < 3 ) {
|
||||
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) {
|
||||
return true;
|
||||
}
|
||||
retry++;
|
||||
}
|
||||
delay(10);
|
||||
|
||||
return false;
|
||||
// check the sensor is functioning
|
||||
while( retry < 3 ) {
|
||||
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) {
|
||||
return true;
|
||||
}
|
||||
retry++;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
//
|
||||
|
|
@ -151,29 +151,29 @@ AP_OpticalFlow_ADNS3080_APM2::init(bool initCommAPI)
|
|||
void AP_OpticalFlow_ADNS3080_APM2::backup_spi_settings()
|
||||
{
|
||||
|
||||
uint8_t temp;
|
||||
uint8_t temp;
|
||||
|
||||
/* Wait for empty transmit buffer */
|
||||
while ( !( UCSR3A & (1<<UDRE3)) );
|
||||
/* Wait for empty transmit buffer */
|
||||
while ( !( UCSR3A & (1<<UDRE3)) ) ;
|
||||
|
||||
// store current spi values
|
||||
orig_spi_settings_ucsr3c = UCSR3C;
|
||||
orig_spi_settings_ubrr3 = UBRR3;
|
||||
// store current spi values
|
||||
orig_spi_settings_ucsr3c = UCSR3C;
|
||||
orig_spi_settings_ubrr3 = UBRR3;
|
||||
|
||||
// decide new value for UCSR3C
|
||||
temp = (orig_spi_settings_ucsr3c & ~UCSR3C_MASK) | SPI3_MODE | SPI3_MSBFIRST;
|
||||
UCSR3C = temp;
|
||||
UBRR3 = SPI3_SPEED; // SPI running at 1Mhz
|
||||
// decide new value for UCSR3C
|
||||
temp = (orig_spi_settings_ucsr3c & ~UCSR3C_MASK) | SPI3_MODE | SPI3_MSBFIRST;
|
||||
UCSR3C = temp;
|
||||
UBRR3 = SPI3_SPEED; // SPI running at 1Mhz
|
||||
}
|
||||
|
||||
// restore_spi_settings - restores SPI settings (clock speed, etc) to what their values were before the sensor used the bus
|
||||
void AP_OpticalFlow_ADNS3080_APM2::restore_spi_settings()
|
||||
{
|
||||
/* Wait for empty transmit buffer */
|
||||
while ( !( UCSR3A & (1<<UDRE3)) );
|
||||
/* Wait for empty transmit buffer */
|
||||
while ( !( UCSR3A & (1<<UDRE3)) ) ;
|
||||
|
||||
// restore UCSRC3C and UBRR3
|
||||
UCSR3C = orig_spi_settings_ucsr3c;
|
||||
// restore UCSRC3C and UBRR3
|
||||
UCSR3C = orig_spi_settings_ucsr3c;
|
||||
UBRR3 = orig_spi_settings_ubrr3;
|
||||
}
|
||||
|
||||
|
|
@ -185,24 +185,24 @@ AP_OpticalFlow_ADNS3080_APM2::read_register(byte address)
|
|||
|
||||
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);
|
||||
|
||||
|
||||
// send the device the register you want to read:
|
||||
junk = SPI_transfer(address);
|
||||
|
||||
// small delay
|
||||
delayMicroseconds(SPI3_DELAY);
|
||||
|
||||
// send a value of 0 to read the first byte returned:
|
||||
|
||||
// small delay
|
||||
delayMicroseconds(SPI3_DELAY);
|
||||
|
||||
// send a value of 0 to read the first byte returned:
|
||||
result = SPI_transfer(0x00);
|
||||
|
||||
// take the chip select high to de-select:
|
||||
digitalWrite(_cs_pin, HIGH);
|
||||
|
||||
restore_spi_settings();
|
||||
restore_spi_settings();
|
||||
|
||||
return result;
|
||||
return result;
|
||||
}
|
||||
|
||||
// write a value to one of the sensor's registers
|
||||
|
|
@ -213,22 +213,22 @@ AP_OpticalFlow_ADNS3080_APM2::write_register(byte address, byte value)
|
|||
|
||||
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);
|
||||
|
||||
// send register address
|
||||
// send register address
|
||||
junk = SPI_transfer(address | 0x80 );
|
||||
|
||||
// small delay
|
||||
delayMicroseconds(SPI3_DELAY);
|
||||
// small delay
|
||||
delayMicroseconds(SPI3_DELAY);
|
||||
|
||||
// send data
|
||||
junk = SPI_transfer(value);
|
||||
// send data
|
||||
junk = SPI_transfer(value);
|
||||
|
||||
// take the chip select high to de-select:
|
||||
digitalWrite(_cs_pin, HIGH);
|
||||
|
||||
restore_spi_settings();
|
||||
restore_spi_settings();
|
||||
}
|
||||
|
||||
// reset sensor by holding a pin high (or is it low?) for 10us.
|
||||
|
|
@ -237,11 +237,11 @@ AP_OpticalFlow_ADNS3080_APM2::reset()
|
|||
{
|
||||
// return immediately if the reset pin is not defined
|
||||
if( _reset_pin == 0)
|
||||
return;
|
||||
return;
|
||||
|
||||
digitalWrite(_reset_pin,HIGH); // reset sensor
|
||||
delayMicroseconds(10);
|
||||
digitalWrite(_reset_pin,LOW); // return sensor to normal
|
||||
delayMicroseconds(10);
|
||||
digitalWrite(_reset_pin,LOW); // return sensor to normal
|
||||
}
|
||||
|
||||
// read latest values from sensor and fill in x,y and totals
|
||||
|
|
@ -250,42 +250,42 @@ AP_OpticalFlow_ADNS3080_APM2::update()
|
|||
{
|
||||
byte motion_reg;
|
||||
surface_quality = (unsigned int)read_register(ADNS3080_SQUAL);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
|
||||
// check for movement, update x,y values
|
||||
motion_reg = read_register(ADNS3080_MOTION);
|
||||
_overflow = ((motion_reg & 0x10) != 0); // check if we've had an overflow
|
||||
if( (motion_reg & 0x80) != 0 ) {
|
||||
raw_dx = ((char)read_register(ADNS3080_DELTA_X));
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
raw_dy = ((char)read_register(ADNS3080_DELTA_Y));
|
||||
_motion = true;
|
||||
}else{
|
||||
raw_dx = 0;
|
||||
raw_dy = 0;
|
||||
}
|
||||
motion_reg = read_register(ADNS3080_MOTION);
|
||||
_overflow = ((motion_reg & 0x10) != 0); // check if we've had an overflow
|
||||
if( (motion_reg & 0x80) != 0 ) {
|
||||
raw_dx = ((char)read_register(ADNS3080_DELTA_X));
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
raw_dy = ((char)read_register(ADNS3080_DELTA_Y));
|
||||
_motion = true;
|
||||
}else{
|
||||
raw_dx = 0;
|
||||
raw_dy = 0;
|
||||
}
|
||||
|
||||
last_update = millis();
|
||||
last_update = millis();
|
||||
|
||||
apply_orientation_matrix();
|
||||
apply_orientation_matrix();
|
||||
|
||||
return true;
|
||||
return true;
|
||||
}
|
||||
|
||||
void
|
||||
AP_OpticalFlow_ADNS3080_APM2::disable_serial_pullup()
|
||||
{
|
||||
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
|
||||
regVal = (regVal | ADNS3080_SERIALNPU_OFF);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
write_register(ADNS3080_EXTENDED_CONFIG, regVal);
|
||||
regVal = (regVal | ADNS3080_SERIALNPU_OFF);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
write_register(ADNS3080_EXTENDED_CONFIG, regVal);
|
||||
}
|
||||
|
||||
// get_led_always_on - returns true if LED is always on, false if only on when required
|
||||
bool
|
||||
AP_OpticalFlow_ADNS3080_APM2::get_led_always_on()
|
||||
{
|
||||
return ( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x40) > 0 );
|
||||
return ( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x40) > 0 );
|
||||
}
|
||||
|
||||
// set_led_always_on - set parameter to true if you want LED always on, otherwise false for only when required
|
||||
|
|
@ -294,8 +294,8 @@ AP_OpticalFlow_ADNS3080_APM2::set_led_always_on( bool alwaysOn )
|
|||
{
|
||||
byte regVal = read_register(ADNS3080_CONFIGURATION_BITS);
|
||||
regVal = (regVal & 0xbf) | (alwaysOn << 6);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
write_register(ADNS3080_CONFIGURATION_BITS, regVal);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
write_register(ADNS3080_CONFIGURATION_BITS, regVal);
|
||||
}
|
||||
|
||||
// returns resolution (either 400 or 1600 counts per inch)
|
||||
|
|
@ -303,9 +303,9 @@ int
|
|||
AP_OpticalFlow_ADNS3080_APM2::get_resolution()
|
||||
{
|
||||
if( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x10) == 0 )
|
||||
return 400;
|
||||
else
|
||||
return 1600;
|
||||
return 400;
|
||||
else
|
||||
return 1600;
|
||||
}
|
||||
|
||||
// set parameter to 400 or 1600 counts per inch
|
||||
|
|
@ -314,19 +314,19 @@ AP_OpticalFlow_ADNS3080_APM2::set_resolution(int resolution)
|
|||
{
|
||||
byte regVal = read_register(ADNS3080_CONFIGURATION_BITS);
|
||||
|
||||
if( resolution == ADNS3080_RESOLUTION_400 ) {
|
||||
regVal &= ~0x10;
|
||||
scaler = AP_OPTICALFLOW_ADNS3080_SCALER;
|
||||
}else if( resolution == ADNS3080_RESOLUTION_1600) {
|
||||
regVal |= 0x10;
|
||||
scaler = AP_OPTICALFLOW_ADNS3080_SCALER * 4;
|
||||
}
|
||||
if( resolution == ADNS3080_RESOLUTION_400 ) {
|
||||
regVal &= ~0x10;
|
||||
scaler = AP_OPTICALFLOW_ADNS3080_SCALER;
|
||||
}else if( resolution == ADNS3080_RESOLUTION_1600) {
|
||||
regVal |= 0x10;
|
||||
scaler = AP_OPTICALFLOW_ADNS3080_SCALER * 4;
|
||||
}
|
||||
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
write_register(ADNS3080_CONFIGURATION_BITS, regVal);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
write_register(ADNS3080_CONFIGURATION_BITS, regVal);
|
||||
|
||||
// this will affect conversion factors so update them
|
||||
update_conversion_factors();
|
||||
// this will affect conversion factors so update them
|
||||
update_conversion_factors();
|
||||
}
|
||||
|
||||
// get_frame_rate_auto - return whether frame rate is set to "auto" or manual
|
||||
|
|
@ -335,10 +335,10 @@ AP_OpticalFlow_ADNS3080_APM2::get_frame_rate_auto()
|
|||
{
|
||||
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
|
||||
if( (regVal & 0x01) != 0 ) {
|
||||
return false;
|
||||
}else{
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}else{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
// set_frame_rate_auto - set frame rate to auto (true) or manual (false)
|
||||
|
|
@ -346,21 +346,21 @@ void
|
|||
AP_OpticalFlow_ADNS3080_APM2::set_frame_rate_auto(bool auto_frame_rate)
|
||||
{
|
||||
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
if( auto_frame_rate == true ) {
|
||||
// set specific frame period
|
||||
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,0xE0);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,0x1A);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
if( auto_frame_rate == true ) {
|
||||
// set specific frame period
|
||||
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,0xE0);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,0x1A);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
|
||||
// decide what value to update in extended config
|
||||
// decide what value to update in extended config
|
||||
regVal = (regVal & ~0x01);
|
||||
}else{
|
||||
// decide what value to update in extended config
|
||||
regVal = (regVal & ~0x01) | 0x01;
|
||||
}
|
||||
write_register(ADNS3080_EXTENDED_CONFIG, regVal);
|
||||
}else{
|
||||
// decide what value to update in extended config
|
||||
regVal = (regVal & ~0x01) | 0x01;
|
||||
}
|
||||
write_register(ADNS3080_EXTENDED_CONFIG, regVal);
|
||||
}
|
||||
|
||||
// get frame period
|
||||
|
|
@ -368,10 +368,10 @@ unsigned int
|
|||
AP_OpticalFlow_ADNS3080_APM2::get_frame_period()
|
||||
{
|
||||
NumericIntType aNum;
|
||||
aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER);
|
||||
return aNum.uintValue;
|
||||
return aNum.uintValue;
|
||||
}
|
||||
|
||||
// set frame period
|
||||
|
|
@ -379,16 +379,16 @@ void
|
|||
AP_OpticalFlow_ADNS3080_APM2::set_frame_period(unsigned int period)
|
||||
{
|
||||
NumericIntType aNum;
|
||||
aNum.uintValue = period;
|
||||
aNum.uintValue = period;
|
||||
|
||||
// set frame rate to manual
|
||||
set_frame_rate_auto(false);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
// set frame rate to manual
|
||||
set_frame_rate_auto(false);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
|
||||
// set specific frame period
|
||||
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]);
|
||||
// set specific frame period
|
||||
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]);
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -396,8 +396,8 @@ unsigned int
|
|||
AP_OpticalFlow_ADNS3080_APM2::get_frame_rate()
|
||||
{
|
||||
unsigned long clockSpeed = ADNS3080_CLOCK_SPEED;
|
||||
unsigned int rate = clockSpeed / get_frame_period();
|
||||
return rate;
|
||||
unsigned int rate = clockSpeed / get_frame_period();
|
||||
return rate;
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -406,7 +406,7 @@ AP_OpticalFlow_ADNS3080_APM2::set_frame_rate(unsigned int rate)
|
|||
unsigned long clockSpeed = ADNS3080_CLOCK_SPEED;
|
||||
unsigned int period = (unsigned int)(clockSpeed / (unsigned long)rate);
|
||||
|
||||
set_frame_period(period);
|
||||
set_frame_period(period);
|
||||
}
|
||||
|
||||
// get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual
|
||||
|
|
@ -414,11 +414,11 @@ bool
|
|||
AP_OpticalFlow_ADNS3080_APM2::get_shutter_speed_auto()
|
||||
{
|
||||
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
|
||||
if( (regVal & 0x02) > 0 ) {
|
||||
return false;
|
||||
}else{
|
||||
return true;
|
||||
}
|
||||
if( (regVal & 0x02) > 0 ) {
|
||||
return false;
|
||||
}else{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
// set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
|
||||
|
|
@ -426,22 +426,22 @@ void
|
|||
AP_OpticalFlow_ADNS3080_APM2::set_shutter_speed_auto(bool auto_shutter_speed)
|
||||
{
|
||||
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
if( auto_shutter_speed ) {
|
||||
// return shutter speed max to default
|
||||
write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,0x8c);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,0x20);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
if( auto_shutter_speed ) {
|
||||
// return shutter speed max to default
|
||||
write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,0x8c);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,0x20);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
|
||||
// determine value to put into extended config
|
||||
// determine value to put into extended config
|
||||
regVal &= ~0x02;
|
||||
}else{
|
||||
// determine value to put into extended config
|
||||
regVal |= 0x02;
|
||||
}
|
||||
write_register(ADNS3080_EXTENDED_CONFIG, regVal);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
}else{
|
||||
// determine value to put into extended config
|
||||
regVal |= 0x02;
|
||||
}
|
||||
write_register(ADNS3080_EXTENDED_CONFIG, regVal);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
}
|
||||
|
||||
// get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual
|
||||
|
|
@ -449,10 +449,10 @@ unsigned int
|
|||
AP_OpticalFlow_ADNS3080_APM2::get_shutter_speed()
|
||||
{
|
||||
NumericIntType aNum;
|
||||
aNum.byteValue[1] = read_register(ADNS3080_SHUTTER_UPPER);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
aNum.byteValue[1] = read_register(ADNS3080_SHUTTER_UPPER);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
aNum.byteValue[0] = read_register(ADNS3080_SHUTTER_LOWER);
|
||||
return aNum.uintValue;
|
||||
return aNum.uintValue;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -461,30 +461,30 @@ void
|
|||
AP_OpticalFlow_ADNS3080_APM2::set_shutter_speed(unsigned int shutter_speed)
|
||||
{
|
||||
NumericIntType aNum;
|
||||
aNum.uintValue = shutter_speed;
|
||||
aNum.uintValue = shutter_speed;
|
||||
|
||||
// set shutter speed to manual
|
||||
// set shutter speed to manual
|
||||
set_shutter_speed_auto(false);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
|
||||
// set specific shutter speed
|
||||
write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,aNum.byteValue[1]);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
// set specific shutter speed
|
||||
write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,aNum.byteValue[1]);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
|
||||
// larger delay
|
||||
delay(50);
|
||||
// larger delay
|
||||
delay(50);
|
||||
|
||||
// need to update frame period to cause shutter value to take effect
|
||||
aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
// need to update frame period to cause shutter value to take effect
|
||||
aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]);
|
||||
delayMicroseconds(SPI3_DELAY); // small delay
|
||||
}
|
||||
|
||||
// clear_motion - will cause the Delta_X, Delta_Y, and internal motion registers to be cleared
|
||||
|
|
@ -492,11 +492,11 @@ void
|
|||
AP_OpticalFlow_ADNS3080_APM2::clear_motion()
|
||||
{
|
||||
write_register(ADNS3080_MOTION_CLEAR,0xFF); // writing anything to this register will clear the sensor's motion registers
|
||||
x = 0;
|
||||
y = 0;
|
||||
dx = 0;
|
||||
dy = 0;
|
||||
_motion = false;
|
||||
x = 0;
|
||||
y = 0;
|
||||
dx = 0;
|
||||
dy = 0;
|
||||
_motion = false;
|
||||
}
|
||||
|
||||
// get_pixel_data - captures an image from the sensor and stores it to the pixe_data array
|
||||
|
|
@ -504,33 +504,33 @@ void
|
|||
AP_OpticalFlow_ADNS3080_APM2::print_pixel_data(Stream *serPort)
|
||||
{
|
||||
int i,j;
|
||||
bool isFirstPixel = true;
|
||||
byte regValue;
|
||||
byte pixelValue;
|
||||
bool isFirstPixel = true;
|
||||
byte regValue;
|
||||
byte pixelValue;
|
||||
|
||||
// write to frame capture register to force capture of frame
|
||||
write_register(ADNS3080_FRAME_CAPTURE,0x83);
|
||||
write_register(ADNS3080_FRAME_CAPTURE,0x83);
|
||||
|
||||
// wait 3 frame periods + 10 nanoseconds for frame to be captured
|
||||
delayMicroseconds(1510); // min frame speed is 2000 frames/second so 1 frame = 500 nano seconds. so 500 x 3 + 10 = 1510
|
||||
// wait 3 frame periods + 10 nanoseconds for frame to be captured
|
||||
delayMicroseconds(1510); // min frame speed is 2000 frames/second so 1 frame = 500 nano seconds. so 500 x 3 + 10 = 1510
|
||||
|
||||
// display the pixel data
|
||||
for( i=0; i<ADNS3080_PIXELS_Y; i++ ) {
|
||||
for( j=0; j<ADNS3080_PIXELS_X; j++ ) {
|
||||
regValue = read_register(ADNS3080_FRAME_CAPTURE);
|
||||
if( isFirstPixel && (regValue & 0x40) == 0 ) {
|
||||
serPort->println("failed to find first pixel");
|
||||
}
|
||||
isFirstPixel = false;
|
||||
pixelValue = ( regValue << 2);
|
||||
serPort->print(pixelValue,DEC);
|
||||
if( j!= ADNS3080_PIXELS_X-1 )
|
||||
serPort->print(",");
|
||||
delayMicroseconds(SPI3_DELAY);
|
||||
}
|
||||
serPort->println();
|
||||
}
|
||||
for( i=0; i<ADNS3080_PIXELS_Y; i++ ) {
|
||||
for( j=0; j<ADNS3080_PIXELS_X; j++ ) {
|
||||
regValue = read_register(ADNS3080_FRAME_CAPTURE);
|
||||
if( isFirstPixel && (regValue & 0x40) == 0 ) {
|
||||
serPort->println("failed to find first pixel");
|
||||
}
|
||||
isFirstPixel = false;
|
||||
pixelValue = ( regValue << 2);
|
||||
serPort->print(pixelValue,DEC);
|
||||
if( j!= ADNS3080_PIXELS_X-1 )
|
||||
serPort->print(",");
|
||||
delayMicroseconds(SPI3_DELAY);
|
||||
}
|
||||
serPort->println();
|
||||
}
|
||||
|
||||
// hardware reset to restore sensor to normal operation
|
||||
reset();
|
||||
// hardware reset to restore sensor to normal operation
|
||||
reset();
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue