ardupilot/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp

421 lines
13 KiB
C++

/*
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 <avr/interrupt.h>
#include "../SPI/SPI.h"
#define AP_SPI_TIMEOUT 1000
union NumericIntType
{
int intValue;
unsigned int uintValue;
byte byteValue[2];
};
// Constructors ////////////////////////////////////////////////////////////////
AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080()
{
}
// Public Methods //////////////////////////////////////////////////////////////
// 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)
void AP_OpticalFlow_ADNS3080::init(boolean initCommAPI)
{
pinMode(AP_SPI_DATAOUT,OUTPUT);
pinMode(AP_SPI_DATAIN,INPUT);
pinMode(AP_SPI_CLOCK,OUTPUT);
pinMode(ADNS3080_CHIP_SELECT,OUTPUT);
pinMode(ADNS3080_RESET,OUTPUT);
digitalWrite(ADNS3080_CHIP_SELECT,HIGH); // disable device (Chip select is active low)
// reset the device
reset();
// start the SPI library:
if( initCommAPI ) {
SPI.begin();
//SPI.setBitOrder(MSBFIRST);
//SPI.setDataMode(SPI_MODE3);
//SPI.setClockDivider(SPI_CLOCK_DIV8); // sensor running at 2Mhz. this is it's maximum speed
}
}
//
// backup_spi_settings - checks current SPI settings (clock speed, etc), sets values to what we need
//
byte AP_OpticalFlow_ADNS3080::backup_spi_settings()
{
// store current spi values
orig_spi_settings_spcr = SPCR & (DORD | CPOL | CPHA);
orig_spi_settings_spsr = SPSR & SPI2X;
// set the values that we need
SPI.setBitOrder(MSBFIRST);
SPI.setDataMode(SPI_MODE3);
SPI.setClockDivider(SPI_CLOCK_DIV8); // sensor running at 2Mhz. this is it's maximum speed
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
byte AP_OpticalFlow_ADNS3080::restore_spi_settings()
{
byte temp;
// restore SPSR
temp = SPSR;
temp &= ~SPI2X;
temp |= orig_spi_settings_spsr;
SPSR = temp;
// restore SPCR
temp = SPCR;
temp &= ~(DORD | CPOL | CPHA); // zero out the important bits
temp |= orig_spi_settings_spcr; // restore important bits
SPCR = temp;
return temp;
}
// Read a register from the sensor
byte AP_OpticalFlow_ADNS3080::read_register(byte address)
{
byte result = 0, junk = 0;
backup_spi_settings();
// take the chip select low to select the device
digitalWrite(ADNS3080_CHIP_SELECT, LOW);
// send the device the register you want to read:
junk = SPI.transfer(address);
// small delay
delayMicroseconds(50);
// send a value of 0 to read the first byte returned:
result = SPI.transfer(0x00);
// take the chip select high to de-select:
digitalWrite(ADNS3080_CHIP_SELECT, HIGH);
restore_spi_settings();
return result;
}
// write a value to one of the sensor's registers
void AP_OpticalFlow_ADNS3080::write_register(byte address, byte value)
{
byte junk = 0;
backup_spi_settings();
// take the chip select low to select the device
digitalWrite(ADNS3080_CHIP_SELECT, LOW);
// send register address
junk = SPI.transfer(address | 0x80 );
// small delay
delayMicroseconds(50);
// send data
junk = SPI.transfer(value);
// take the chip select high to de-select:
digitalWrite(ADNS3080_CHIP_SELECT, HIGH);
restore_spi_settings();
}
// reset sensor by holding a pin high (or is it low?) for 10us.
void AP_OpticalFlow_ADNS3080::reset()
{
digitalWrite(ADNS3080_RESET,HIGH); // reset sensor
delayMicroseconds(10);
digitalWrite(ADNS3080_RESET,LOW); // return sensor to normal
}
// read latest values from sensor and fill in x,y and totals
int AP_OpticalFlow_ADNS3080::read()
{
surface_quality = (unsigned int)read_register(ADNS3080_SQUAL);
delayMicroseconds(50); // small delay
// check for movement, update x,y values
if( (read_register(ADNS3080_MOTION) & 0x80) != 0 ) {
dx = ((char)read_register(ADNS3080_DELTA_X));
delayMicroseconds(50); // small delay
dy = ((char)read_register(ADNS3080_DELTA_Y));
x+=dx;
y+=dy;
_motion = true;
}else{
dx = 0;
dy = 0;
}
return OPTICALFLOW_SUCCESS;
}
// get_led_always_on - returns true if LED is always on, false if only on when required
boolean AP_OpticalFlow_ADNS3080::get_led_always_on()
{
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
void AP_OpticalFlow_ADNS3080::set_led_always_on( boolean alwaysOn )
{
byte regVal = read_register(ADNS3080_CONFIGURATION_BITS);
regVal = regVal & 0xBf | (alwaysOn << 6);
delayMicroseconds(50); // small delay
write_register(ADNS3080_CONFIGURATION_BITS, regVal);
}
// returns resolution (either 400 or 1200 counts per inch)
int AP_OpticalFlow_ADNS3080::get_resolution()
{
if( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x10) == 0 )
return 400;
else
return 1200;
}
// set parameter to 400 or 1200 counts per inch
void AP_OpticalFlow_ADNS3080::set_resolution(int resolution)
{
byte regVal = read_register(ADNS3080_CONFIGURATION_BITS);
if( resolution == ADNS3080_RESOLUTION_400 ) {
regVal &= ~0x10;
}else if( resolution == ADNS3080_RESOLUTION_1200) {
regVal |= 0x10;
}
delayMicroseconds(50); // small delay
write_register(ADNS3080_CONFIGURATION_BITS, regVal);
}
// get_frame_rate_auto - return whether frame rate is set to "auto" or manual
boolean AP_OpticalFlow_ADNS3080::get_frame_rate_auto()
{
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
if( regVal & 0x01 > 0 ) {
return false;
}else{
return true;
}
}
// set_frame_rate_auto - set frame rate to auto (true) or manual (false)
void AP_OpticalFlow_ADNS3080::set_frame_rate_auto(boolean auto_frame_rate)
{
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
delayMicroseconds(50); // small delay
if( auto_frame_rate == true ) {
// set specific frame period
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,0xE0);
delayMicroseconds(50); // small delay
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,0x1A);
delayMicroseconds(50); // small delay
// 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);
}
// get frame period
unsigned int AP_OpticalFlow_ADNS3080::get_frame_period()
{
NumericIntType aNum;
aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
delayMicroseconds(50); // small delay
aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER);
return aNum.uintValue;
}
// set frame period
void AP_OpticalFlow_ADNS3080::set_frame_period(unsigned int period)
{
NumericIntType aNum;
aNum.uintValue = period;
// set frame rate to manual
set_frame_rate_auto(false);
delayMicroseconds(50); // small delay
// set specific frame period
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]);
delayMicroseconds(50); // small delay
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]);
}
unsigned int AP_OpticalFlow_ADNS3080::get_frame_rate()
{
unsigned long clockSpeed = ADNS3080_CLOCK_SPEED;
unsigned int rate = clockSpeed / get_frame_period();
return rate;
}
void AP_OpticalFlow_ADNS3080::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);
}
// get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual
boolean AP_OpticalFlow_ADNS3080::get_shutter_speed_auto()
{
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
if( (regVal & 0x02) > 0 ) {
return false;
}else{
return true;
}
}
// set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
void AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(boolean auto_shutter_speed)
{
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
delayMicroseconds(50); // small delay
if( auto_shutter_speed ) {
// return shutter speed max to default
write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,0x8c);
delayMicroseconds(50); // small delay
write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,0x20);
delayMicroseconds(50); // small delay
// determine value to put into extended config
regVal = regVal & ~0x02;
}else{
// determine value to put into extended config
regVal = regVal & ~0x02 | 0x02;
}
write_register(ADNS3080_EXTENDED_CONFIG, regVal);
delayMicroseconds(50); // small delay
}
// get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual
unsigned int AP_OpticalFlow_ADNS3080::get_shutter_speed()
{
NumericIntType aNum;
aNum.byteValue[1] = read_register(ADNS3080_SHUTTER_UPPER);
delayMicroseconds(50); // small delay
aNum.byteValue[0] = read_register(ADNS3080_SHUTTER_LOWER);
return aNum.uintValue;
}
// set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
unsigned int AP_OpticalFlow_ADNS3080::set_shutter_speed(unsigned int shutter_speed)
{
NumericIntType aNum;
aNum.uintValue = shutter_speed;
// set shutter speed to manual
set_shutter_speed_auto(false);
delayMicroseconds(50); // small delay
// set specific shutter speed
write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]);
delayMicroseconds(50); // small delay
write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,aNum.byteValue[1]);
delayMicroseconds(50); // small delay
// 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(50); // small delay
aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER);
delayMicroseconds(50); // small delay
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]);
delayMicroseconds(50); // small delay
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]);
delayMicroseconds(50); // small delay
}
// clear_motion - will cause the Delta_X, Delta_Y, and internal motion registers to be cleared
void AP_OpticalFlow_ADNS3080::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;
}
// get_pixel_data - captures an image from the sensor and stores it to the pixe_data array
int AP_OpticalFlow_ADNS3080::print_pixel_data(HardwareSerial *serPort)
{
int i,j;
boolean isFirstPixel = true;
byte regValue;
byte pixelValue;
// write to frame capture register to force capture of frame
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
// 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(50);
}
serPort->println();
}
// hardware reset to restore sensor to normal operation
reset();
}