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

OpticalFlow: fixed line endings

Browse Source 
this library was a mixture of dos and unix line endings, which makes
for very messy editing
This commit is contained in:
Andrew Tridgell 2012-03-10 13:51:28 +11:00
parent 3c145ab61c
commit 7aa6ba2c86
4 changed files with 787 additions and 787 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

204
libraries/AP_OpticalFlow/AP_OpticalFlow.cpp
View File

@ -1,109 +1,109 @@
/*
ADC.cpp - Analog Digital Converter Base Class for Ardupilot Mega
Code by James Goppert. 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.
*/
#include "AP_OpticalFlow.h"
#define FORTYFIVE_DEGREES 0.78539816
AP_OpticalFlow* AP_OpticalFlow::_sensor = NULL; // pointer to the last instantiated optical flow sensor. Will be turned into a table if we ever add support for more than one sensor
// init - initCommAPI 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
AP_OpticalFlow::init(bool initCommAPI)
{
_orientation_matrix = Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1);
update_conversion_factors();
return true; // just return true by default
}
// set_orientation - Rotation vector to transform sensor readings to the body frame.
void
AP_OpticalFlow::set_orientation(const Matrix3f &rotation_matrix)
{
_orientation_matrix = rotation_matrix;
}
// read value from the sensor. Should be overridden by derived class
bool
AP_OpticalFlow::update()
{
/*
ADC.cpp - Analog Digital Converter Base Class for Ardupilot Mega
Code by James Goppert. 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.
*/
#include "AP_OpticalFlow.h"
#define FORTYFIVE_DEGREES 0.78539816
AP_OpticalFlow* AP_OpticalFlow::_sensor = NULL; // pointer to the last instantiated optical flow sensor. Will be turned into a table if we ever add support for more than one sensor
// init - initCommAPI 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
AP_OpticalFlow::init(bool initCommAPI)
{
_orientation_matrix = Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1);
update_conversion_factors();
return true; // just return true by default
}
// set_orientation - Rotation vector to transform sensor readings to the body frame.
void
AP_OpticalFlow::set_orientation(const Matrix3f &rotation_matrix)
{
_orientation_matrix = rotation_matrix;
}
// read value from the sensor. Should be overridden by derived class
bool
AP_OpticalFlow::update()
{
return true;
}
// reads a value from the sensor (will be sensor specific)
byte
AP_OpticalFlow::read_register(byte address)
{
}
// reads a value from the sensor (will be sensor specific)
byte
AP_OpticalFlow::read_register(byte address)
{
return 0;
}
// writes a value to one of the sensor's register (will be sensor specific)
void
AP_OpticalFlow::write_register(byte address, byte value)
{
}
// rotate raw values to arrive at final x,y,dx and dy values
void
AP_OpticalFlow::apply_orientation_matrix()
{
Vector3f rot_vector;
// next rotate dx and dy
rot_vector = _orientation_matrix * Vector3f(raw_dx, raw_dy, 0);
dx = rot_vector.x;
dy = rot_vector.y;
// add rotated values to totals (perhaps this is pointless as we need to take into account yaw, roll, pitch)
x += dx;
y += dy;
}
// updatse conversion factors that are dependent upon field_of_view
void
AP_OpticalFlow::update_conversion_factors()
{
conv_factor = (1.0 / (float)(num_pixels * scaler)) * 2.0 * tan(field_of_view / 2.0); // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude)
// 0.00615
radians_to_pixels = (num_pixels * scaler) / field_of_view;
// 162.99
}
// updates internal lon and lat with estimation based on optical flow
void
AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float sin_yaw_y, float altitude)
{
float diff_roll = roll - _last_roll;
float diff_pitch = pitch - _last_pitch;
// only update position if surface quality is good and angle is not over 45 degrees
if( surface_quality >= 10 && fabs(roll) <= FORTYFIVE_DEGREES && fabs(pitch) <= FORTYFIVE_DEGREES ) {
altitude = max(altitude, 0);
// calculate expected x,y diff due to roll and pitch change
exp_change_x = diff_roll * radians_to_pixels;
exp_change_y = -diff_pitch * radians_to_pixels;
// real estimated raw change from mouse
change_x = dx - exp_change_x;
change_y = dy - exp_change_y;
}
// writes a value to one of the sensor's register (will be sensor specific)
void
AP_OpticalFlow::write_register(byte address, byte value)
{
}
// rotate raw values to arrive at final x,y,dx and dy values
void
AP_OpticalFlow::apply_orientation_matrix()
{
Vector3f rot_vector;
// next rotate dx and dy
rot_vector = _orientation_matrix * Vector3f(raw_dx, raw_dy, 0);
dx = rot_vector.x;
dy = rot_vector.y;
// add rotated values to totals (perhaps this is pointless as we need to take into account yaw, roll, pitch)
x += dx;
y += dy;
}
// updatse conversion factors that are dependent upon field_of_view
void
AP_OpticalFlow::update_conversion_factors()
{
conv_factor = (1.0 / (float)(num_pixels * scaler)) * 2.0 * tan(field_of_view / 2.0); // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude)
// 0.00615
radians_to_pixels = (num_pixels * scaler) / field_of_view;
// 162.99
}
// updates internal lon and lat with estimation based on optical flow
void
AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float sin_yaw_y, float altitude)
{
float diff_roll = roll - _last_roll;
float diff_pitch = pitch - _last_pitch;
// only update position if surface quality is good and angle is not over 45 degrees
if( surface_quality >= 10 && fabs(roll) <= FORTYFIVE_DEGREES && fabs(pitch) <= FORTYFIVE_DEGREES ) {
altitude = max(altitude, 0);
// calculate expected x,y diff due to roll and pitch change
exp_change_x = diff_roll * radians_to_pixels;
exp_change_y = -diff_pitch * radians_to_pixels;
// real estimated raw change from mouse
change_x = dx - exp_change_x;
change_y = dy - exp_change_y;
float avg_altitude = (altitude + _last_altitude)*0.5;
// convert raw change to horizontal movement in cm
x_cm = -change_x * avg_altitude * conv_factor; // perhaps this altitude should actually be the distance to the ground? i.e. if we are very rolled over it should be longer?
y_cm = -change_y * avg_altitude * conv_factor; // for example if you are leaned over at 45 deg the ground will appear farther away and motion from opt flow sensor will be less
// convert x/y movements into lon/lat movement
vlon = x_cm * sin_yaw_y + y_cm * cos_yaw_x;
vlat = y_cm * sin_yaw_y - x_cm * cos_yaw_x;
}
// convert raw change to horizontal movement in cm
x_cm = -change_x * avg_altitude * conv_factor; // perhaps this altitude should actually be the distance to the ground? i.e. if we are very rolled over it should be longer?
y_cm = -change_y * avg_altitude * conv_factor; // for example if you are leaned over at 45 deg the ground will appear farther away and motion from opt flow sensor will be less
// convert x/y movements into lon/lat movement
vlon = x_cm * sin_yaw_y + y_cm * cos_yaw_x;
vlat = y_cm * sin_yaw_y - x_cm * cos_yaw_x;
}
_last_altitude = altitude;
_last_roll = roll;

142
libraries/AP_OpticalFlow/AP_OpticalFlow.h
View File

@ -1,74 +1,74 @@
#ifndef AP_OPTICALFLOW_H
#define AP_OPTICALFLOW_H
/*
AP_OpticalFlow.cpp - OpticalFlow Base Class 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.
Methods:
init() : initializate sensor and library.
read : reads latest value from OpticalFlow and stores values in x,y, surface_quality parameter
read_register() : reads a value from the sensor (will be sensor specific)
write_register() : writes a value to one of the sensor's register (will be sensor specific)
*/
#include <FastSerial.h>
#include <AP_Math.h>
#include <AP_Common.h>
// standard rotation matrices
#define AP_OPTICALFLOW_ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_45 Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_135 Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_225 Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_315 Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1)
class AP_OpticalFlow
{
public:
int raw_dx, raw_dy; // raw sensor change in x and y position (i.e. unrotated)
int surface_quality; // image quality (below 15 you really can't trust the x,y values returned)
int x,y; // total x,y position
int dx,dy; // rotated change in x and y position
float vlon, vlat; // position as offsets from original position
unsigned long last_update; // millis() time of last update
float field_of_view; // field of view in Radians
float scaler; // number returned from sensor when moved one pixel
int num_pixels; // number of pixels of resolution in the sensor
// temp variables - delete me!
float exp_change_x, exp_change_y;
float change_x, change_y;
float x_cm, y_cm;
AP_OpticalFlow() { _sensor = this; };
~AP_OpticalFlow() { _sensor = NULL; };
virtual 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)
virtual byte read_register(byte address);
virtual void write_register(byte address, byte value);
virtual void set_orientation(const Matrix3f &rotation_matrix); // Rotation vector to transform sensor readings to the body frame.
virtual void set_field_of_view(const float fov) { field_of_view = fov; update_conversion_factors(); }; // sets field of view of sensor
static void read(uint32_t ) { if( _sensor != NULL ) _sensor->update(); }; // call to update all attached sensors
virtual bool update(); // read latest values from sensor and fill in x,y and totals. returns true on success
virtual void update_position(float roll, float pitch, float cos_yaw_x, float sin_yaw_y, float altitude); // updates internal lon and lat with estimation based on optical flow
protected:
static AP_OpticalFlow *_sensor; // pointer to the last instantiated optical flow sensor. Will be turned into a table if we ever add support for more than one sensor
Matrix3f _orientation_matrix;
float conv_factor; // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude)
float radians_to_pixels;
#ifndef AP_OPTICALFLOW_H
#define AP_OPTICALFLOW_H
/*
AP_OpticalFlow.cpp - OpticalFlow Base Class 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.
Methods:
init() : initializate sensor and library.
read : reads latest value from OpticalFlow and stores values in x,y, surface_quality parameter
read_register() : reads a value from the sensor (will be sensor specific)
write_register() : writes a value to one of the sensor's register (will be sensor specific)
*/
#include <FastSerial.h>
#include <AP_Math.h>
#include <AP_Common.h>
// standard rotation matrices
#define AP_OPTICALFLOW_ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_45 Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_135 Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_225 Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_315 Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1)
class AP_OpticalFlow
{
public:
int raw_dx, raw_dy; // raw sensor change in x and y position (i.e. unrotated)
int surface_quality; // image quality (below 15 you really can't trust the x,y values returned)
int x,y; // total x,y position
int dx,dy; // rotated change in x and y position
float vlon, vlat; // position as offsets from original position
unsigned long last_update; // millis() time of last update
float field_of_view; // field of view in Radians
float scaler; // number returned from sensor when moved one pixel
int num_pixels; // number of pixels of resolution in the sensor
// temp variables - delete me!
float exp_change_x, exp_change_y;
float change_x, change_y;
float x_cm, y_cm;
AP_OpticalFlow() { _sensor = this; };
~AP_OpticalFlow() { _sensor = NULL; };
virtual 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)
virtual byte read_register(byte address);
virtual void write_register(byte address, byte value);
virtual void set_orientation(const Matrix3f &rotation_matrix); // Rotation vector to transform sensor readings to the body frame.
virtual void set_field_of_view(const float fov) { field_of_view = fov; update_conversion_factors(); }; // sets field of view of sensor
static void read(uint32_t ) { if( _sensor != NULL ) _sensor->update(); }; // call to update all attached sensors
virtual bool update(); // read latest values from sensor and fill in x,y and totals. returns true on success
virtual void update_position(float roll, float pitch, float cos_yaw_x, float sin_yaw_y, float altitude); // updates internal lon and lat with estimation based on optical flow
protected:
static AP_OpticalFlow *_sensor; // pointer to the last instantiated optical flow sensor. Will be turned into a table if we ever add support for more than one sensor
Matrix3f _orientation_matrix;
float conv_factor; // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude)
float radians_to_pixels;
float _last_roll, _last_pitch, _last_altitude;
virtual void apply_orientation_matrix(); // rotate raw values to arrive at final x,y,dx and dy values
virtual void update_conversion_factors();
};
virtual void apply_orientation_matrix(); // rotate raw values to arrive at final x,y,dx and dy values
virtual void update_conversion_factors();
};
#include "AP_OpticalFlow_ADNS3080.h"
#endif
#endif

942
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp
View File

@ -1,482 +1,482 @@
/*
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"
#else
#include "WProgram.h"
#endif
#define AP_SPI_TIMEOUT 1000
union NumericIntType
{
int intValue;
unsigned int uintValue;
byte byteValue[2];
};
// Constructors ////////////////////////////////////////////////////////////////
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;
field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV;
scaler = AP_OPTICALFLOW_ADNS3080_SCALER;
}
// 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)
bool
AP_OpticalFlow_ADNS3080::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);
digitalWrite(_cs_pin,HIGH); // disable device (Chip select is active low)
// reset the device
reset();
// start the SPI library:
if( initCommAPI ) {
SPI.begin();
}
// check the sensor is functioning
if( retry < 3 ) {
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 )
return true;
/*
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"
#else
#include "WProgram.h"
#endif
#define AP_SPI_TIMEOUT 1000
union NumericIntType
{
int intValue;
unsigned int uintValue;
byte byteValue[2];
};
// Constructors ////////////////////////////////////////////////////////////////
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;
field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV;
scaler = AP_OPTICALFLOW_ADNS3080_SCALER;
}
// 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)
bool
AP_OpticalFlow_ADNS3080::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);
digitalWrite(_cs_pin,HIGH); // disable device (Chip select is active low)
// reset the device
reset();
// start the SPI library:
if( initCommAPI ) {
SPI.begin();
}
// check the sensor is functioning
if( retry < 3 ) {
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 )
return true;
retry++;
}
return false;
}
//
// 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(_cs_pin, 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(_cs_pin, 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(_cs_pin, 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(_cs_pin, HIGH);
restore_spi_settings();
}
// reset sensor by holding a pin high (or is it low?) for 10us.
void
AP_OpticalFlow_ADNS3080::reset()
{
// return immediately if the reset pin is not defined
if( _reset_pin == 0)
return;
digitalWrite(_reset_pin,HIGH); // reset sensor
delayMicroseconds(10);
digitalWrite(_reset_pin,LOW); // return sensor to normal
}
// read latest values from sensor and fill in x,y and totals
bool
AP_OpticalFlow_ADNS3080::update()
{
byte motion_reg;
surface_quality = (unsigned int)read_register(ADNS3080_SQUAL);
delayMicroseconds(50); // 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(50); // small delay
raw_dy = ((char)read_register(ADNS3080_DELTA_Y));
_motion = true;
}else{
raw_dx = 0;
raw_dy = 0;
}
last_update = millis();
apply_orientation_matrix();
return true;
}
void
AP_OpticalFlow_ADNS3080::disable_serial_pullup()
{
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
regVal = (regVal | ADNS3080_SERIALNPU_OFF);
delayMicroseconds(50); // 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::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( bool alwaysOn )
{
byte regVal = read_register(ADNS3080_CONFIGURATION_BITS);
}
//
// 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(_cs_pin, 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(_cs_pin, 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(_cs_pin, 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(_cs_pin, HIGH);
restore_spi_settings();
}
// reset sensor by holding a pin high (or is it low?) for 10us.
void
AP_OpticalFlow_ADNS3080::reset()
{
// return immediately if the reset pin is not defined
if( _reset_pin == 0)
return;
digitalWrite(_reset_pin,HIGH); // reset sensor
delayMicroseconds(10);
digitalWrite(_reset_pin,LOW); // return sensor to normal
}
// read latest values from sensor and fill in x,y and totals
bool
AP_OpticalFlow_ADNS3080::update()
{
byte motion_reg;
surface_quality = (unsigned int)read_register(ADNS3080_SQUAL);
delayMicroseconds(50); // 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(50); // small delay
raw_dy = ((char)read_register(ADNS3080_DELTA_Y));
_motion = true;
}else{
raw_dx = 0;
raw_dy = 0;
}
last_update = millis();
apply_orientation_matrix();
return true;
}
void
AP_OpticalFlow_ADNS3080::disable_serial_pullup()
{
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
regVal = (regVal | ADNS3080_SERIALNPU_OFF);
delayMicroseconds(50); // 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::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( bool 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 1600 counts per inch)
int
AP_OpticalFlow_ADNS3080::get_resolution()
{
if( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x10) == 0 )
return 400;
else
return 1600;
}
// set parameter to 400 or 1600 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;
scaler = AP_OPTICALFLOW_ADNS3080_SCALER;
}else if( resolution == ADNS3080_RESOLUTION_1600) {
regVal |= 0x10;
scaler = AP_OPTICALFLOW_ADNS3080_SCALER * 4;
}
delayMicroseconds(50); // small delay
write_register(ADNS3080_CONFIGURATION_BITS, regVal);
// 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
bool
AP_OpticalFlow_ADNS3080::get_frame_rate_auto()
{
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
delayMicroseconds(50); // small delay
write_register(ADNS3080_CONFIGURATION_BITS, regVal);
}
// returns resolution (either 400 or 1600 counts per inch)
int
AP_OpticalFlow_ADNS3080::get_resolution()
{
if( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x10) == 0 )
return 400;
else
return 1600;
}
// set parameter to 400 or 1600 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;
scaler = AP_OPTICALFLOW_ADNS3080_SCALER;
}else if( resolution == ADNS3080_RESOLUTION_1600) {
regVal |= 0x10;
scaler = AP_OPTICALFLOW_ADNS3080_SCALER * 4;
}
delayMicroseconds(50); // small delay
write_register(ADNS3080_CONFIGURATION_BITS, regVal);
// 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
bool
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(bool 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
bool
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(bool 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
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(bool 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
bool
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(bool 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 &= ~0x02;
}else{
// determine value to put into extended config
}else{
// determine value to put into extended config
regVal |= 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)
}
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)
void
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
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::print_pixel_data(Stream *serPort)
{
int i,j;
bool 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);
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
void
AP_OpticalFlow_ADNS3080::print_pixel_data(Stream *serPort)
{
int i,j;
bool 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();
}
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();
}

286
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h
View File

@ -1,145 +1,145 @@
#ifndef AP_OPTICALFLOW_ADNS3080_H
#define AP_OPTICALFLOW_ADNS3080_H
#include "AP_OpticalFlow.h"
// orientations for ADNS3080 sensor
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD AP_OPTICALFLOW_ROTATION_YAW_180
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_RIGHT AP_OPTICALFLOW_ROTATION_YAW_135
#define AP_OPTICALFLOW_ADNS3080_PINS_RIGHT AP_OPTICALFLOW_ROTATION_YAW_90
#define AP_OPTICALFLOW_ADNS3080_PINS_BACK_RIGHT AP_OPTICALFLOW_ROTATION_YAW_45
#define AP_OPTICALFLOW_ADNS3080_PINS_BACK AP_OPTICALFLOW_ROTATION_NONE
#define AP_OPTICALFLOW_ADNS3080_PINS_BACK_LEFT AP_OPTICALFLOW_ROTATION_YAW_315
#define AP_OPTICALFLOW_ADNS3080_PINS_LEFT AP_OPTICALFLOW_ROTATION_YAW_270
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_LEFT AP_OPTICALFLOW_ROTATION_YAW_225
// field of view of ADNS3080 sensor lenses
#define AP_OPTICALFLOW_ADNS3080_08_FOV 0.202458 // 11.6 degrees
// scaler - value returned when sensor is moved equivalent of 1 pixel
#define AP_OPTICALFLOW_ADNS3080_SCALER 1.1
// We use Serial Port 2 in SPI Mode
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
#define AP_SPI_DATAIN 50 // MISO // PB3
#define AP_SPI_DATAOUT 51 // MOSI // PB2
#define AP_SPI_CLOCK 52 // SCK // PB1
#define ADNS3080_CHIP_SELECT 34 // PC3
#define ADNS3080_RESET 0 // reset pin is unattached by default
#else // normal arduino SPI pins...these need to be checked
#define AP_SPI_DATAIN 12 //MISO
#define AP_SPI_DATAOUT 11 //MOSI
#define AP_SPI_CLOCK 13 //SCK
#define ADNS3080_CHIP_SELECT 10 //SS
#define ADNS3080_RESET 9 //RESET
#endif
// ADNS3080 hardware config
#define ADNS3080_PIXELS_X 30
#define ADNS3080_PIXELS_Y 30
#define ADNS3080_CLOCK_SPEED 24000000
// Register Map for the ADNS3080 Optical OpticalFlow Sensor
#define ADNS3080_PRODUCT_ID 0x00
#define ADNS3080_REVISION_ID 0x01
#define ADNS3080_MOTION 0x02
#define ADNS3080_DELTA_X 0x03
#define ADNS3080_DELTA_Y 0x04
#define ADNS3080_SQUAL 0x05
#define ADNS3080_PIXEL_SUM 0x06
#define ADNS3080_MAXIMUM_PIXEL 0x07
#define ADNS3080_CONFIGURATION_BITS 0x0a
#define ADNS3080_EXTENDED_CONFIG 0x0b
#define ADNS3080_DATA_OUT_LOWER 0x0c
#define ADNS3080_DATA_OUT_UPPER 0x0d
#define ADNS3080_SHUTTER_LOWER 0x0e
#define ADNS3080_SHUTTER_UPPER 0x0f
#define ADNS3080_FRAME_PERIOD_LOWER 0x10
#define ADNS3080_FRAME_PERIOD_UPPER 0x11
#define ADNS3080_MOTION_CLEAR 0x12
#define ADNS3080_FRAME_CAPTURE 0x13
#define ADNS3080_SROM_ENABLE 0x14
#define ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER 0x19
#define ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER 0x1a
#define ADNS3080_FRAME_PERIOD_MIN_BOUND_LOWER 0x1b
#define ADNS3080_FRAME_PERIOD_MIN_BOUND_UPPER 0x1c
#define ADNS3080_SHUTTER_MAX_BOUND_LOWER 0x1e
#define ADNS3080_SHUTTER_MAX_BOUND_UPPER 0x1e
#define ADNS3080_SROM_ID 0x1f
#define ADNS3080_OBSERVATION 0x3d
#define ADNS3080_INVERSE_PRODUCT_ID 0x3f
#define ADNS3080_PIXEL_BURST 0x40
#define ADNS3080_MOTION_BURST 0x50
#define ADNS3080_SROM_LOAD 0x60
// Configuration Bits
#define ADNS3080_LED_MODE_ALWAYS_ON 0x00
#define ADNS3080_LED_MODE_WHEN_REQUIRED 0x01
#define ADNS3080_RESOLUTION_400 400
#define ADNS3080_RESOLUTION_1600 1600
// Extended Configuration bits
#define ADNS3080_SERIALNPU_OFF 0x02
#define ADNS3080_FRAME_RATE_MAX 6469
#define ADNS3080_FRAME_RATE_MIN 2000
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:
int _cs_pin; // pin used for chip select
int _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
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.
bool update(); // read latest values from sensor and fill in x,y and totals, return true on successful read
// 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
void disable_serial_pullup();
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
int 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
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)
unsigned int get_frame_period(); // get_frame_period -
void set_frame_period(unsigned int period);
unsigned int get_frame_rate();
void set_frame_rate(unsigned int rate);
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)
unsigned int get_shutter_speed();
#ifndef AP_OPTICALFLOW_ADNS3080_H
#define AP_OPTICALFLOW_ADNS3080_H
#include "AP_OpticalFlow.h"
// orientations for ADNS3080 sensor
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD AP_OPTICALFLOW_ROTATION_YAW_180
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_RIGHT AP_OPTICALFLOW_ROTATION_YAW_135
#define AP_OPTICALFLOW_ADNS3080_PINS_RIGHT AP_OPTICALFLOW_ROTATION_YAW_90
#define AP_OPTICALFLOW_ADNS3080_PINS_BACK_RIGHT AP_OPTICALFLOW_ROTATION_YAW_45
#define AP_OPTICALFLOW_ADNS3080_PINS_BACK AP_OPTICALFLOW_ROTATION_NONE
#define AP_OPTICALFLOW_ADNS3080_PINS_BACK_LEFT AP_OPTICALFLOW_ROTATION_YAW_315
#define AP_OPTICALFLOW_ADNS3080_PINS_LEFT AP_OPTICALFLOW_ROTATION_YAW_270
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_LEFT AP_OPTICALFLOW_ROTATION_YAW_225
// field of view of ADNS3080 sensor lenses
#define AP_OPTICALFLOW_ADNS3080_08_FOV 0.202458 // 11.6 degrees
// scaler - value returned when sensor is moved equivalent of 1 pixel
#define AP_OPTICALFLOW_ADNS3080_SCALER 1.1
// We use Serial Port 2 in SPI Mode
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
#define AP_SPI_DATAIN 50 // MISO // PB3
#define AP_SPI_DATAOUT 51 // MOSI // PB2
#define AP_SPI_CLOCK 52 // SCK // PB1
#define ADNS3080_CHIP_SELECT 34 // PC3
#define ADNS3080_RESET 0 // reset pin is unattached by default
#else // normal arduino SPI pins...these need to be checked
#define AP_SPI_DATAIN 12 //MISO
#define AP_SPI_DATAOUT 11 //MOSI
#define AP_SPI_CLOCK 13 //SCK
#define ADNS3080_CHIP_SELECT 10 //SS
#define ADNS3080_RESET 9 //RESET
#endif
// ADNS3080 hardware config
#define ADNS3080_PIXELS_X 30
#define ADNS3080_PIXELS_Y 30
#define ADNS3080_CLOCK_SPEED 24000000
// Register Map for the ADNS3080 Optical OpticalFlow Sensor
#define ADNS3080_PRODUCT_ID 0x00
#define ADNS3080_REVISION_ID 0x01
#define ADNS3080_MOTION 0x02
#define ADNS3080_DELTA_X 0x03
#define ADNS3080_DELTA_Y 0x04
#define ADNS3080_SQUAL 0x05
#define ADNS3080_PIXEL_SUM 0x06
#define ADNS3080_MAXIMUM_PIXEL 0x07
#define ADNS3080_CONFIGURATION_BITS 0x0a
#define ADNS3080_EXTENDED_CONFIG 0x0b
#define ADNS3080_DATA_OUT_LOWER 0x0c
#define ADNS3080_DATA_OUT_UPPER 0x0d
#define ADNS3080_SHUTTER_LOWER 0x0e
#define ADNS3080_SHUTTER_UPPER 0x0f
#define ADNS3080_FRAME_PERIOD_LOWER 0x10
#define ADNS3080_FRAME_PERIOD_UPPER 0x11
#define ADNS3080_MOTION_CLEAR 0x12
#define ADNS3080_FRAME_CAPTURE 0x13
#define ADNS3080_SROM_ENABLE 0x14
#define ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER 0x19
#define ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER 0x1a
#define ADNS3080_FRAME_PERIOD_MIN_BOUND_LOWER 0x1b
#define ADNS3080_FRAME_PERIOD_MIN_BOUND_UPPER 0x1c
#define ADNS3080_SHUTTER_MAX_BOUND_LOWER 0x1e
#define ADNS3080_SHUTTER_MAX_BOUND_UPPER 0x1e
#define ADNS3080_SROM_ID 0x1f
#define ADNS3080_OBSERVATION 0x3d
#define ADNS3080_INVERSE_PRODUCT_ID 0x3f
#define ADNS3080_PIXEL_BURST 0x40
#define ADNS3080_MOTION_BURST 0x50
#define ADNS3080_SROM_LOAD 0x60
// Configuration Bits
#define ADNS3080_LED_MODE_ALWAYS_ON 0x00
#define ADNS3080_LED_MODE_WHEN_REQUIRED 0x01
#define ADNS3080_RESOLUTION_400 400
#define ADNS3080_RESOLUTION_1600 1600
// Extended Configuration bits
#define ADNS3080_SERIALNPU_OFF 0x02
#define ADNS3080_FRAME_RATE_MAX 6469
#define ADNS3080_FRAME_RATE_MIN 2000
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:
int _cs_pin; // pin used for chip select
int _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
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.
bool update(); // read latest values from sensor and fill in x,y and totals, return true on successful read
// 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
void disable_serial_pullup();
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
int 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
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)
unsigned int get_frame_period(); // get_frame_period -
void set_frame_period(unsigned int period);
unsigned int get_frame_rate();
void set_frame_rate(unsigned int rate);
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)
unsigned int get_shutter_speed();
void set_shutter_speed(unsigned int 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
};
#endif
};
#endif