mirror of https://github.com/ArduPilot/ardupilot
421 lines
13 KiB
C++
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();
|
|
}
|