Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

uncrustify libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp

Browse Source
This commit is contained in:
uncrustify 2012-08-16 23:20:51 -07:00 committed by Pat Hickey
parent 491ad278fb
commit 8f5c22d448
1 changed files with 224 additions and 224 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

448
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp
View File

@ -1,62 +1,62 @@
/* /*
AP_OpticalFlow_ADNS3080.cpp - ADNS3080 OpticalFlow Library for Ardupilot Mega * AP_OpticalFlow_ADNS3080.cpp - ADNS3080 OpticalFlow Library for Ardupilot Mega
Code by Randy Mackay. DIYDrones.com * Code by Randy Mackay. DIYDrones.com
*
This library is free software; you can redistribute it and/or * This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public * modify it under the terms of the GNU Lesser General Public
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) * External ADNS3080 OpticalFlow is connected via Serial port 2 (in SPI mode)
TXD2 = MOSI = pin PH1 * TXD2 = MOSI = pin PH1
RXD2 = MISO = pin PH0 * RXD2 = MISO = pin PH0
XCK2 = SCK = pin PH2 * XCK2 = SCK = pin PH2
Chip Select pin is PC4 (33) [PH6 (9)] * Chip Select pin is PC4 (33) [PH6 (9)]
We are using the 16 clocks per conversion timming to increase efficiency (fast) * We are using the 16 clocks per conversion timming to increase efficiency (fast)
The sampling frequency is 400Hz (Timer2 overflow interrupt) * The sampling frequency is 400Hz (Timer2 overflow interrupt)
So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so * So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so
we have an 4x oversampling and averaging. * we have an 4x oversampling and averaging.
*
Methods: * Methods:
Init() : Initialization of interrupts an Timers (Timer2 overflow interrupt) * Init() : Initialization of interrupts an Timers (Timer2 overflow interrupt)
Read() : Read latest values from OpticalFlow and store to x,y, surface_quality parameters * 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"
#if defined(ARDUINO) && ARDUINO >= 100 #if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h" #include "Arduino.h"
#else #else
#include "WProgram.h" #include "WProgram.h"
#endif #endif
#define AP_SPI_TIMEOUT 1000 #define AP_SPI_TIMEOUT 1000
// 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 AP_SPI_DATAIN 50 // MISO // PB3
#define AP_SPI_DATAOUT 51 // MOSI // PB2 #define AP_SPI_DATAOUT 51 // MOSI // PB2
#define AP_SPI_CLOCK 52 // SCK // PB1 #define AP_SPI_CLOCK 52 // SCK // 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 AP_SPI_DATAIN 12 //MISO
#define AP_SPI_DATAOUT 11 //MOSI #define AP_SPI_DATAOUT 11 //MOSI
#define AP_SPI_CLOCK 13 //SCK #define AP_SPI_CLOCK 13 //SCK
#endif #endif
union NumericIntType union NumericIntType
{ {
int intValue; int intValue;
unsigned int uintValue; unsigned int uintValue;
byte byteValue[2]; byte 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(int cs_pin, int reset_pin) : _cs_pin(cs_pin), _reset_pin(reset_pin)
{ {
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;
} }
@ -68,31 +68,31 @@ AP_OpticalFlow_ADNS3080::init(bool initCommAPI)
{ {
int retry = 0; int retry = 0;
pinMode(AP_SPI_DATAOUT,OUTPUT); pinMode(AP_SPI_DATAOUT,OUTPUT);
pinMode(AP_SPI_DATAIN,INPUT); pinMode(AP_SPI_DATAIN,INPUT);
pinMode(AP_SPI_CLOCK,OUTPUT); 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);
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 the device
reset(); reset();
// start the SPI library: // start the SPI library:
if( initCommAPI ) { if( initCommAPI ) {
SPI.begin(); SPI.begin();
} }
// check the sensor is functioning // check the sensor is functioning
while( retry < 3 ) { while( retry < 3 ) {
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) if( read_register(ADNS3080_PRODUCT_ID) == 0x17 )
return true; return true;
retry++; retry++;
} }
return false; return false;
} }
// //
@ -101,16 +101,16 @@ AP_OpticalFlow_ADNS3080::init(bool initCommAPI)
byte byte
AP_OpticalFlow_ADNS3080::backup_spi_settings() AP_OpticalFlow_ADNS3080::backup_spi_settings()
{ {
// store current spi values // store current spi values
orig_spi_settings_spcr = SPCR & (DORD | CPOL | CPHA); orig_spi_settings_spcr = SPCR & (DORD | CPOL | CPHA);
orig_spi_settings_spsr = SPSR & SPI2X; orig_spi_settings_spsr = SPSR & SPI2X;
// set the values that we need // set the values that we need
SPI.setBitOrder(MSBFIRST); 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 SPI.setClockDivider(SPI_CLOCK_DIV8); // sensor running at 2Mhz. this is it's maximum speed
return orig_spi_settings_spcr; return orig_spi_settings_spcr;
} }
// 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
@ -119,19 +119,19 @@ AP_OpticalFlow_ADNS3080::restore_spi_settings()
{ {
byte temp; byte temp;
// restore SPSR // restore SPSR
temp = SPSR; temp = SPSR;
temp &= ~SPI2X; temp &= ~SPI2X;
temp |= orig_spi_settings_spsr; temp |= orig_spi_settings_spsr;
SPSR = temp; SPSR = temp;
// restore SPCR // restore SPCR
temp = SPCR; temp = SPCR;
temp &= ~(DORD | CPOL | CPHA); // zero out the important bits temp &= ~(DORD | CPOL | CPHA); // zero out the important bits
temp |= orig_spi_settings_spcr; // restore important bits temp |= orig_spi_settings_spcr; // restore important bits
SPCR = temp; SPCR = temp;
return temp; return temp;
} }
// Read a register from the sensor // Read a register from the sensor
@ -142,24 +142,24 @@ AP_OpticalFlow_ADNS3080::read_register(byte address)
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: // send the device the register you want to read:
junk = SPI.transfer(address); junk = SPI.transfer(address);
// small delay // small delay
delayMicroseconds(50); delayMicroseconds(50);
// send a value of 0 to read the first byte returned: // send a value of 0 to read the first byte returned:
result = SPI.transfer(0x00); result = SPI.transfer(0x00);
// take the chip select high to de-select: // take the chip select high to de-select:
digitalWrite(_cs_pin, HIGH); digitalWrite(_cs_pin, HIGH);
restore_spi_settings(); restore_spi_settings();
return result; return result;
} }
// write a value to one of the sensor's registers // write a value to one of the sensor's registers
@ -170,22 +170,22 @@ AP_OpticalFlow_ADNS3080::write_register(byte address, byte value)
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 register address // send register address
junk = SPI.transfer(address | 0x80 ); junk = SPI.transfer(address | 0x80 );
// small delay // small delay
delayMicroseconds(50); delayMicroseconds(50);
// send data // send data
junk = SPI.transfer(value); junk = SPI.transfer(value);
// take the chip select high to de-select: // take the chip select high to de-select:
digitalWrite(_cs_pin, HIGH); digitalWrite(_cs_pin, HIGH);
restore_spi_settings(); restore_spi_settings();
} }
// reset sensor by holding a pin high (or is it low?) for 10us. // reset sensor by holding a pin high (or is it low?) for 10us.
@ -194,11 +194,11 @@ AP_OpticalFlow_ADNS3080::reset()
{ {
// return immediately if the reset pin is not defined // return immediately if the reset pin is not defined
if( _reset_pin == 0) if( _reset_pin == 0)
return; return;
digitalWrite(_reset_pin,HIGH); // reset sensor digitalWrite(_reset_pin,HIGH); // reset sensor
delayMicroseconds(10); delayMicroseconds(10);
digitalWrite(_reset_pin,LOW); // return sensor to normal digitalWrite(_reset_pin,LOW); // return sensor to normal
} }
// read latest values from sensor and fill in x,y and totals // read latest values from sensor and fill in x,y and totals
@ -207,42 +207,42 @@ AP_OpticalFlow_ADNS3080::update()
{ {
byte motion_reg; byte motion_reg;
surface_quality = (unsigned int)read_register(ADNS3080_SQUAL); surface_quality = (unsigned int)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 = ((char)read_register(ADNS3080_DELTA_X));
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
raw_dy = ((char)read_register(ADNS3080_DELTA_Y)); raw_dy = ((char)read_register(ADNS3080_DELTA_Y));
_motion = true; _motion = true;
}else{ }else{
raw_dx = 0; raw_dx = 0;
raw_dy = 0; raw_dy = 0;
} }
last_update = millis(); last_update = millis();
apply_orientation_matrix(); apply_orientation_matrix();
return true; return true;
} }
void void
AP_OpticalFlow_ADNS3080::disable_serial_pullup() AP_OpticalFlow_ADNS3080::disable_serial_pullup()
{ {
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG); byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
regVal = (regVal | ADNS3080_SERIALNPU_OFF); regVal = (regVal | ADNS3080_SERIALNPU_OFF);
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
write_register(ADNS3080_EXTENDED_CONFIG, regVal); write_register(ADNS3080_EXTENDED_CONFIG, regVal);
} }
// get_led_always_on - returns true if LED is always on, false if only on when required // get_led_always_on - returns true if LED is always on, false if only on when required
bool bool
AP_OpticalFlow_ADNS3080::get_led_always_on() AP_OpticalFlow_ADNS3080::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 // set_led_always_on - set parameter to true if you want LED always on, otherwise false for only when required
@ -251,8 +251,8 @@ AP_OpticalFlow_ADNS3080::set_led_always_on( bool alwaysOn )
{ {
byte regVal = read_register(ADNS3080_CONFIGURATION_BITS); byte regVal = read_register(ADNS3080_CONFIGURATION_BITS);
regVal = (regVal & 0xbf) | (alwaysOn << 6); regVal = (regVal & 0xbf) | (alwaysOn << 6);
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
write_register(ADNS3080_CONFIGURATION_BITS, regVal); write_register(ADNS3080_CONFIGURATION_BITS, regVal);
} }
// returns resolution (either 400 or 1600 counts per inch) // returns resolution (either 400 or 1600 counts per inch)
@ -260,9 +260,9 @@ int
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 )
return 400; return 400;
else else
return 1600; return 1600;
} }
// set parameter to 400 or 1600 counts per inch // set parameter to 400 or 1600 counts per inch
@ -271,19 +271,19 @@ AP_OpticalFlow_ADNS3080::set_resolution(int resolution)
{ {
byte regVal = read_register(ADNS3080_CONFIGURATION_BITS); byte regVal = read_register(ADNS3080_CONFIGURATION_BITS);
if( resolution == ADNS3080_RESOLUTION_400 ) { if( resolution == ADNS3080_RESOLUTION_400 ) {
regVal &= ~0x10; regVal &= ~0x10;
scaler = AP_OPTICALFLOW_ADNS3080_SCALER; scaler = AP_OPTICALFLOW_ADNS3080_SCALER;
}else if( resolution == ADNS3080_RESOLUTION_1600) { }else if( resolution == ADNS3080_RESOLUTION_1600) {
regVal |= 0x10; regVal |= 0x10;
scaler = AP_OPTICALFLOW_ADNS3080_SCALER * 4; scaler = AP_OPTICALFLOW_ADNS3080_SCALER * 4;
} }
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
write_register(ADNS3080_CONFIGURATION_BITS, regVal); write_register(ADNS3080_CONFIGURATION_BITS, regVal);
// this will affect conversion factors so update them // this will affect conversion factors so update them
update_conversion_factors(); update_conversion_factors();
} }
// get_frame_rate_auto - return whether frame rate is set to "auto" or manual // get_frame_rate_auto - return whether frame rate is set to "auto" or manual
@ -292,10 +292,10 @@ AP_OpticalFlow_ADNS3080::get_frame_rate_auto()
{ {
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG); byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
if( (regVal & 0x01) != 0 ) { if( (regVal & 0x01) != 0 ) {
return false; return false;
}else{ }else{
return true; return true;
} }
} }
// set_frame_rate_auto - set frame rate to auto (true) or manual (false) // set_frame_rate_auto - set frame rate to auto (true) or manual (false)
@ -303,21 +303,21 @@ void
AP_OpticalFlow_ADNS3080::set_frame_rate_auto(bool auto_frame_rate) AP_OpticalFlow_ADNS3080::set_frame_rate_auto(bool auto_frame_rate)
{ {
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG); byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
if( auto_frame_rate == true ) { if( auto_frame_rate == true ) {
// set specific frame period // set specific frame period
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,0xE0); write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,0xE0);
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,0x1A); write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,0x1A);
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
// decide what value to update in extended config // decide what value to update in extended config
regVal = (regVal & ~0x01); regVal = (regVal & ~0x01);
}else{ }else{
// decide what value to update in extended config // decide what value to update in extended config
regVal = (regVal & ~0x01) | 0x01; regVal = (regVal & ~0x01) | 0x01;
} }
write_register(ADNS3080_EXTENDED_CONFIG, regVal); write_register(ADNS3080_EXTENDED_CONFIG, regVal);
} }
// get frame period // get frame period
@ -325,10 +325,10 @@ unsigned int
AP_OpticalFlow_ADNS3080::get_frame_period() AP_OpticalFlow_ADNS3080::get_frame_period()
{ {
NumericIntType aNum; NumericIntType aNum;
aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER); aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER); aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER);
return aNum.uintValue; return aNum.uintValue;
} }
// set frame period // set frame period
@ -336,16 +336,16 @@ void
AP_OpticalFlow_ADNS3080::set_frame_period(unsigned int period) AP_OpticalFlow_ADNS3080::set_frame_period(unsigned int period)
{ {
NumericIntType aNum; NumericIntType aNum;
aNum.uintValue = period; aNum.uintValue = period;
// set frame rate to manual // set frame rate to manual
set_frame_rate_auto(false); set_frame_rate_auto(false);
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
// set specific frame period // set specific frame period
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]); write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]);
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]); write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]);
} }
@ -353,8 +353,8 @@ unsigned int
AP_OpticalFlow_ADNS3080::get_frame_rate() AP_OpticalFlow_ADNS3080::get_frame_rate()
{ {
unsigned long clockSpeed = ADNS3080_CLOCK_SPEED; unsigned long clockSpeed = ADNS3080_CLOCK_SPEED;
unsigned int rate = clockSpeed / get_frame_period(); unsigned int rate = clockSpeed / get_frame_period();
return rate; return rate;
} }
void void
@ -363,7 +363,7 @@ AP_OpticalFlow_ADNS3080::set_frame_rate(unsigned int rate)
unsigned long clockSpeed = ADNS3080_CLOCK_SPEED; unsigned long clockSpeed = ADNS3080_CLOCK_SPEED;
unsigned int period = (unsigned int)(clockSpeed / (unsigned long)rate); 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 // get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual
@ -371,11 +371,11 @@ bool
AP_OpticalFlow_ADNS3080::get_shutter_speed_auto() AP_OpticalFlow_ADNS3080::get_shutter_speed_auto()
{ {
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG); byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
if( (regVal & 0x02) > 0 ) { if( (regVal & 0x02) > 0 ) {
return false; return false;
}else{ }else{
return true; return true;
} }
} }
// set_shutter_speed_auto - set shutter speed to auto (true), or manual (false) // set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
@ -383,22 +383,22 @@ 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); byte 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
write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,0x8c); write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,0x8c);
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,0x20); write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,0x20);
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
// determine value to put into extended config // determine value to put into extended config
regVal &= ~0x02; regVal &= ~0x02;
}else{ }else{
// determine value to put into extended config // determine value to put into extended config
regVal |= 0x02; regVal |= 0x02;
} }
write_register(ADNS3080_EXTENDED_CONFIG, regVal); write_register(ADNS3080_EXTENDED_CONFIG, regVal);
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
} }
// 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
@ -406,10 +406,10 @@ unsigned int
AP_OpticalFlow_ADNS3080::get_shutter_speed() AP_OpticalFlow_ADNS3080::get_shutter_speed()
{ {
NumericIntType aNum; NumericIntType aNum;
aNum.byteValue[1] = read_register(ADNS3080_SHUTTER_UPPER); aNum.byteValue[1] = read_register(ADNS3080_SHUTTER_UPPER);
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
aNum.byteValue[0] = read_register(ADNS3080_SHUTTER_LOWER); aNum.byteValue[0] = read_register(ADNS3080_SHUTTER_LOWER);
return aNum.uintValue; return aNum.uintValue;
} }
@ -418,30 +418,30 @@ void
AP_OpticalFlow_ADNS3080::set_shutter_speed(unsigned int shutter_speed) AP_OpticalFlow_ADNS3080::set_shutter_speed(unsigned int shutter_speed)
{ {
NumericIntType aNum; 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); set_shutter_speed_auto(false);
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
// set specific shutter speed // set specific shutter speed
write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]); write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]);
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,aNum.byteValue[1]); write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,aNum.byteValue[1]);
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
// larger delay // larger delay
delay(50); delay(50);
// need to update frame period to cause shutter value to take effect // need to update frame period to cause shutter value to take effect
aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER); aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER); aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER);
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]); write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]);
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]); write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]);
delayMicroseconds(50); // small delay delayMicroseconds(50); // small delay
} }
// clear_motion - will cause the Delta_X, Delta_Y, and internal motion registers to be cleared // clear_motion - will cause the Delta_X, Delta_Y, and internal motion registers to be cleared
@ -449,11 +449,11 @@ void
AP_OpticalFlow_ADNS3080::clear_motion() AP_OpticalFlow_ADNS3080::clear_motion()
{ {
write_register(ADNS3080_MOTION_CLEAR,0xFF); // writing anything to this register will clear the sensor's motion registers write_register(ADNS3080_MOTION_CLEAR,0xFF); // writing anything to this register will clear the sensor's motion registers
x = 0; x = 0;
y = 0; y = 0;
dx = 0; dx = 0;
dy = 0; dy = 0;
_motion = false; _motion = false;
} }
// get_pixel_data - captures an image from the sensor and stores it to the pixe_data array // get_pixel_data - captures an image from the sensor and stores it to the pixe_data array
@ -461,33 +461,33 @@ void
AP_OpticalFlow_ADNS3080::print_pixel_data(Stream *serPort) AP_OpticalFlow_ADNS3080::print_pixel_data(Stream *serPort)
{ {
int i,j; int i,j;
bool isFirstPixel = true; bool isFirstPixel = true;
byte regValue; byte regValue;
byte pixelValue; byte 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);
// wait 3 frame periods + 10 nanoseconds for frame to be captured // 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 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 // display the pixel data
for( i=0; i<ADNS3080_PIXELS_Y; i++ ) { for( i=0; i<ADNS3080_PIXELS_Y; i++ ) {
for( j=0; j<ADNS3080_PIXELS_X; j++ ) { for( j=0; j<ADNS3080_PIXELS_X; j++ ) {
regValue = read_register(ADNS3080_FRAME_CAPTURE); regValue = read_register(ADNS3080_FRAME_CAPTURE);
if( isFirstPixel && (regValue & 0x40) == 0 ) { if( isFirstPixel && (regValue & 0x40) == 0 ) {
serPort->println("failed to find first pixel"); serPort->println("failed to find first pixel");
} }
isFirstPixel = false; isFirstPixel = false;
pixelValue = ( regValue << 2); pixelValue = ( regValue << 2);
serPort->print(pixelValue,DEC); serPort->print(pixelValue,DEC);
if( j!= ADNS3080_PIXELS_X-1 ) if( j!= ADNS3080_PIXELS_X-1 )
serPort->print(","); serPort->print(",");
delayMicroseconds(50); delayMicroseconds(50);
} }
serPort->println(); serPort->println();
} }
// hardware reset to restore sensor to normal operation // hardware reset to restore sensor to normal operation
reset(); reset();
} }