Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

OptFlow: shrink lib by removing unused functions 

Saves 46bytes of RAM and 1k of flash
This commit is contained in:
Randy Mackay 2014-01-09 12:26:55 +09:00
parent 1793ee804e
commit 8caa5159f8
4 changed files with 126 additions and 480 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
libraries/AP_OpticalFlow/AP_OpticalFlow.cpp
View File

@ -23,13 +23,6 @@
#define FORTYFIVE_DEGREES 0.78539816f
bool AP_OpticalFlow::init()
{
_orientation = ROTATION_NONE;
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(enum Rotation rotation)
@ -37,64 +30,8 @@ void AP_OpticalFlow::set_orientation(enum Rotation rotation)
_orientation = rotation;
}
// parent method called at 1khz by periodic process
// this is slowed down to 20hz and each instance's update function is called
// (only one instance is supported at the moment)
void AP_OpticalFlow::read(void)
{
_num_calls++;
if( _num_calls >= AP_OPTICALFLOW_NUM_CALLS_FOR_20HZ ) {
_num_calls = 0;
// call to update all attached sensors
if( _sensor != NULL ) {
_sensor->update();
}
}
};
// read value from the sensor. Should be overridden by derived class
void AP_OpticalFlow::update(void){ }
// reads a value from the sensor (will be sensor specific)
uint8_t AP_OpticalFlow::read_register(uint8_t address){ return 0; }
// writes a value to one of the sensor's register (will be sensor specific)
void AP_OpticalFlow::write_register(uint8_t address, uint8_t value) {}
// rotate raw values to arrive at final x,y,dx and dy values
void AP_OpticalFlow::apply_orientation_matrix()
{
Vector3f rot_vector;
rot_vector(raw_dx, raw_dy, 0);
// next rotate dx and dy
rot_vector.rotate(_orientation);
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;
}
// updates conversion factors that are dependent upon field_of_view
void AP_OpticalFlow::update_conversion_factors()
{
// multiply this number by altitude and pixel change to get horizontal
// move (in same units as altitude)
conv_factor = ((1.0f / (float)(num_pixels * scaler))
* 2.0f * tanf(field_of_view / 2.0f));
// 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 sin_yaw, float cos_yaw, float altitude)
void AP_OpticalFlow::update_position(float roll, float pitch, float sin_yaw, float cos_yaw, float altitude)
{
float diff_roll = roll - _last_roll;
float diff_pitch = pitch - _last_pitch;
@ -121,10 +58,6 @@ void AP_OpticalFlow::update_position(float roll, float pitch,
// 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
y_cm = -change_y * avg_altitude * conv_factor;
// convert x/y movements into lon/lat movement
vlon = x_cm * cos_yaw + y_cm * sin_yaw;
vlat = y_cm * cos_yaw - x_cm * sin_yaw;
}
_last_altitude = altitude;

53
libraries/AP_OpticalFlow/AP_OpticalFlow.h
View File

@ -31,52 +31,39 @@
#include <AP_Math.h>
#define AP_OPTICALFLOW_NUM_CALLS_FOR_10HZ 100 // timer process runs at 1khz. 100 iterations = 10hz
#define AP_OPTICALFLOW_NUM_CALLS_FOR_20HZ 50 // timer process runs at 1khz. 50 iterations = 20hz
#define AP_OPTICALFLOW_NUM_CALLS_FOR_50HZ 20 // timer process runs at 1khz. 20 iterations = 50hz
class AP_OpticalFlow
{
public:
// constructor
AP_OpticalFlow() {
_sensor = this;
};
~AP_OpticalFlow() {
_sensor = NULL;
_flags.healthy = false;
};
virtual bool init();
virtual void init();
virtual uint8_t read_register(uint8_t address);
virtual void write_register(uint8_t address, uint8_t value);
// healthy - return true if the sensor is healthy
bool healthy() const { return _flags.healthy; }
// Rotation vector to transform sensor readings to the body frame.
virtual void set_orientation(enum Rotation rotation);
void set_orientation(enum Rotation rotation);
// sets field of view of sensor
virtual void set_field_of_view(const float fov) { field_of_view = fov; update_conversion_factors(); };
// called by timer process to read sensor data from all attached sensors
void read();
void set_field_of_view(const float fov) { field_of_view = fov; };
// read latest values from sensor and fill in x,y and totals.
virtual void update();
virtual void update();
// updates internal lon and lat with estimation based on optical flow
virtual void update_position(float roll, float pitch, float sin_yaw, float cos_yaw, float altitude);
void update_position(float roll, float pitch, float sin_yaw, float cos_yaw, float altitude);
// public variables
int16_t raw_dx; // raw sensor change in x and y position (i.e. unrotated)
int16_t raw_dy; // raw sensor change in x and y position (i.e. unrotated)
uint8_t surface_quality; // image quality (below 15 you really can't trust the x,y values returned)
int16_t x,y; // total x,y position
uint8_t surface_quality; // image quality (below 15 you can't trust the dx,dy values returned)
int16_t dx,dy; // rotated change in x and y position
float vlon, vlat; // position as offsets from original position
uint32_t 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
int16_t num_pixels; // number of pixels of resolution in the sensor
// public variables for reporting purposes
float exp_change_x, exp_change_y; // expected change in x, y coordinates
@ -84,25 +71,17 @@ public:
float x_cm, y_cm; // x,y position in cm
protected:
// pointer to the last instantiated optical flow sensor. Will be turned
// into a table if we ever add support for more than one sensor
AP_OpticalFlow * _sensor;
enum Rotation _orientation;
// multiply this number by altitude and pixel change to get horizontal
// move (in same units as altitude)
float conv_factor;
struct AP_OpticalFlow_Flags {
uint8_t healthy : 1; // true if sensor is healthy
} _flags;
enum Rotation _orientation;
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;
float _last_pitch;
float _last_altitude;
// rotate raw values to arrive at final x,y,dx and dy values
virtual void apply_orientation_matrix();
virtual void update_conversion_factors();
private:
// number of times we have been called by 1khz timer process.
// We use this to throttle read down to 20hz
uint8_t _num_calls;
};
#include "AP_OpticalFlow_ADNS3080.h"

376
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp
View File

@ -25,93 +25,88 @@
extern const AP_HAL::HAL& hal;
#define AP_SPI_TIMEOUT 1000
union NumericIntType
{
int16_t intValue;
uint16_t uintValue;
uint8_t byteValue[2];
};
// Constructors ////////////////////////////////////////////////////////////////
AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080(uint8_t reset_pin) :
_reset_pin(reset_pin)
AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080()
{
num_pixels = ADNS3080_PIXELS_X;
field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV;
scaler = AP_OPTICALFLOW_ADNS3080_SCALER;
scaler = AP_OPTICALFLOW_ADNS3080_SCALER_1600;
}
// Public Methods //////////////////////////////////////////////////////////////
// init - initialise sensor
// assumes SPI bus has been initialised but will attempt to initialise
// nonstandard SPI3 bus if required
bool
AP_OpticalFlow_ADNS3080::init()
void AP_OpticalFlow_ADNS3080::init()
{
int8_t retry = 0;
bool retvalue = false;
_flags.healthy = false;
// suspend timer while we set-up SPI communication
hal.scheduler->suspend_timer_procs();
// reset the device if the reset pin has been defined
if(_reset_pin != 0) {
hal.gpio->pinMode(_reset_pin, GPIO_OUTPUT);
}
// reset the device
reset();
// get pointer to the spi bus
_spi = hal.spi->device(AP_HAL::SPIDevice_ADNS3080_SPI0);
if (_spi == NULL) {
retvalue = false; goto finish;
}
// check 3 times for the sensor on standard SPI bus
while( retvalue == false && retry < 3 ) {
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) {
retvalue = true;
goto finish;
if (_spi != NULL) {
// check 3 times for the sensor on standard SPI bus
while (!_flags.healthy && retry < 3) {
if (read_register(ADNS3080_PRODUCT_ID) == 0x17) {
_flags.healthy = true;
}
retry++;
}
retry++;
}
// if not found, get pointer to the SPI3 bus
_spi = hal.spi->device(AP_HAL::SPIDevice_ADNS3080_SPI3);
if (_spi == NULL) {
retvalue = false; goto finish;
}
// check 3 times on SPI3
retry = 0;
while( retvalue == false && retry < 3 ) {
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) {
retvalue = true;
// if not yet found, get pointer to the SPI3 bus
if (!_flags.healthy) {
_spi = hal.spi->device(AP_HAL::SPIDevice_ADNS3080_SPI3);
if (_spi != NULL) {
// check 3 times on SPI3
retry = 0;
while (!_flags.healthy && retry < 3) {
if (read_register(ADNS3080_PRODUCT_ID) == 0x17) {
_flags.healthy = true;
}
retry++;
}
}
retry++;
}
// If we fail to find on SPI3, no connection available.
retvalue = false;
_spi = NULL;
// configure the sensor
if (_flags.healthy) {
// set frame rate to manual
uint8_t regVal = read_register(ADNS3080_EXTENDED_CONFIG);
hal.scheduler->delay_microseconds(50);
regVal = (regVal & ~0x01) | 0x01;
write_register(ADNS3080_EXTENDED_CONFIG, regVal);
hal.scheduler->delay_microseconds(50);
finish:
// if device is working register the global static read function to
// be called at 1khz
if( retvalue ) {
// set frame period to 12000 (0x2EE0)
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,0xE0);
hal.scheduler->delay_microseconds(50);
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,0x2E);
hal.scheduler->delay_microseconds(50);
// set 1600 resolution bit
regVal = read_register(ADNS3080_CONFIGURATION_BITS);
hal.scheduler->delay_microseconds(50);
regVal |= 0x10;
write_register(ADNS3080_CONFIGURATION_BITS, regVal);
hal.scheduler->delay_microseconds(50);
// update scalers
update_conversion_factors();
// register the global static read function to be called at 1khz
hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_OpticalFlow_ADNS3080::read));
}else{
// no connection available.
_spi = NULL;
}
// resume timer
hal.scheduler->resume_timer_procs();
return retvalue;
}
// Read a register from the sensor
uint8_t AP_OpticalFlow_ADNS3080::read_register(uint8_t address)
{
@ -177,24 +172,8 @@ void AP_OpticalFlow_ADNS3080::write_register(uint8_t address, uint8_t value)
spi_sem->give();
}
// 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;
// reset sensor
hal.gpio->write(_reset_pin, 1);
hal.scheduler->delay_microseconds(10);
// return sensor to normal
hal.gpio->write(_reset_pin, 0);
}
// read latest values from sensor and fill in x,y and totals
void
AP_OpticalFlow_ADNS3080::update(void)
void AP_OpticalFlow_ADNS3080::update(void)
{
uint8_t motion_reg;
surface_quality = read_register(ADNS3080_SQUAL);
@ -202,13 +181,10 @@ AP_OpticalFlow_ADNS3080::update(void)
// check for movement, update x,y values
motion_reg = read_register(ADNS3080_MOTION);
// check if we've had an overflow
_overflow = ((motion_reg & 0x10) != 0);
if( (motion_reg & 0x80) != 0 ) {
if ((motion_reg & 0x80) != 0) {
raw_dx = ((int8_t)read_register(ADNS3080_DELTA_X));
hal.scheduler->delay_microseconds(50);
raw_dy = ((int8_t)read_register(ADNS3080_DELTA_Y));
_motion = true;
}else{
raw_dx = 0;
raw_dy = 0;
@ -216,214 +192,26 @@ AP_OpticalFlow_ADNS3080::update(void)
last_update = hal.scheduler->millis();
apply_orientation_matrix();
Vector3f rot_vector(raw_dx, raw_dy, 0);
// rotate dx and dy
rot_vector.rotate(_orientation);
dx = rot_vector.x;
dy = rot_vector.y;
}
void
AP_OpticalFlow_ADNS3080::disable_serial_pullup()
// parent method called at 1khz by periodic process
// this is slowed down to 20hz and each instance's update function is called
// (only one instance is supported at the moment)
void AP_OpticalFlow_ADNS3080::read(void)
{
uint8_t regVal = read_register(ADNS3080_EXTENDED_CONFIG);
regVal = (regVal | ADNS3080_SERIALNPU_OFF);
hal.scheduler->delay_microseconds(50);
write_register(ADNS3080_EXTENDED_CONFIG, regVal);
}
_num_calls++;
// 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 )
{
uint8_t regVal = read_register(ADNS3080_CONFIGURATION_BITS);
regVal = (regVal & 0xbf) | (alwaysOn << 6);
hal.scheduler->delay_microseconds(50);
write_register(ADNS3080_CONFIGURATION_BITS, regVal);
}
// returns resolution (either 400 or 1600 counts per inch)
int16_t 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(uint16_t resolution)
{
uint8_t regVal = read_register(ADNS3080_CONFIGURATION_BITS);
if( resolution == ADNS3080_RESOLUTION_400 ) {
regVal &= ~0x10;
scaler = AP_OPTICALFLOW_ADNS3080_SCALER;
}else if( resolution == ADNS3080_RESOLUTION_1600) {
regVal |= 0x10;
scaler = AP_OPTICALFLOW_ADNS3080_SCALER * 4;
if (_num_calls >= AP_OPTICALFLOW_ADNS3080_NUM_CALLS_FOR_20HZ) {
_num_calls = 0;
update();
}
hal.scheduler->delay_microseconds(50);
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()
{
uint8_t 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)
{
uint8_t regVal = read_register(ADNS3080_EXTENDED_CONFIG);
hal.scheduler->delay_microseconds(50);
if( auto_frame_rate == true ) {
// set specific frame period
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,0xE0);
hal.scheduler->delay_microseconds(50);
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,0x1A);
hal.scheduler->delay_microseconds(50);
// 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
uint16_t AP_OpticalFlow_ADNS3080::get_frame_period()
{
NumericIntType aNum;
aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
hal.scheduler->delay_microseconds(50);
aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER);
return aNum.uintValue;
}
// set frame period
void AP_OpticalFlow_ADNS3080::set_frame_period(uint16_t period)
{
NumericIntType aNum;
aNum.uintValue = period;
// set frame rate to manual
set_frame_rate_auto(false);
hal.scheduler->delay_microseconds(50);
// set specific frame period
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]);
hal.scheduler->delay_microseconds(50);
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]);
}
uint16_t AP_OpticalFlow_ADNS3080::get_frame_rate()
{
uint32_t clockSpeed = ADNS3080_CLOCK_SPEED;
uint16_t rate = clockSpeed / get_frame_period();
return rate;
}
void AP_OpticalFlow_ADNS3080::set_frame_rate(uint16_t rate)
{
uint32_t clockSpeed = ADNS3080_CLOCK_SPEED;
uint16_t period = (uint16_t)(clockSpeed / (uint32_t)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()
{
uint8_t 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)
{
uint8_t regVal = read_register(ADNS3080_EXTENDED_CONFIG);
hal.scheduler->delay_microseconds(50);
if( auto_shutter_speed ) {
// return shutter speed max to default
write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,0x8c);
hal.scheduler->delay_microseconds(50);
write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,0x20);
hal.scheduler->delay_microseconds(50);
// determine value to put into extended config
regVal &= ~0x02;
}else{
// determine value to put into extended config
regVal |= 0x02;
}
write_register(ADNS3080_EXTENDED_CONFIG, regVal);
hal.scheduler->delay_microseconds(50);
}
// get_shutter_speed_auto - returns true if shutter speed is adjusted
// automatically, false if manual
uint16_t AP_OpticalFlow_ADNS3080::get_shutter_speed()
{
NumericIntType aNum;
aNum.byteValue[1] = read_register(ADNS3080_SHUTTER_UPPER);
hal.scheduler->delay_microseconds(50);
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(uint16_t shutter_speed)
{
NumericIntType aNum;
aNum.uintValue = shutter_speed;
// set shutter speed to manual
set_shutter_speed_auto(false);
hal.scheduler->delay_microseconds(50);
// set specific shutter speed
write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]);
hal.scheduler->delay_microseconds(50);
write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,aNum.byteValue[1]);
hal.scheduler->delay_microseconds(50);
// larger delay
hal.scheduler->delay(50);
// need to update frame period to cause shutter value to take effect
aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
hal.scheduler->delay_microseconds(50);
aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER);
hal.scheduler->delay_microseconds(50);
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]);
hal.scheduler->delay_microseconds(50);
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]);
hal.scheduler->delay_microseconds(50);
}
};
// clear_motion - will cause the Delta_X, Delta_Y, and internal motion
// registers to be cleared
@ -432,11 +220,10 @@ void AP_OpticalFlow_ADNS3080::clear_motion()
// writing anything to this register will clear the sensor's motion
// registers
write_register(ADNS3080_MOTION_CLEAR,0xFF);
x = 0;
y = 0;
x_cm = 0;
y_cm = 0;
dx = 0;
dy = 0;
_motion = false;
}
// get_pixel_data - captures an image from the sensor and stores it to the
@ -457,23 +244,32 @@ void AP_OpticalFlow_ADNS3080::print_pixel_data()
hal.scheduler->delay_microseconds(1510);
// display the pixel data
for( i=0; i<ADNS3080_PIXELS_Y; i++ ) {
for( j=0; j<ADNS3080_PIXELS_X; j++ ) {
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 ) {
if (isFirstPixel && (regValue & 0x40) == 0) {
hal.console->println_P(
PSTR("Optflow: failed to find first pixel"));
}
isFirstPixel = false;
pixelValue = ( regValue << 2 );
hal.console->print(pixelValue,BASE_DEC);
if( j!= ADNS3080_PIXELS_X-1 )
if (j!= ADNS3080_PIXELS_X-1)
hal.console->print_P(PSTR(","));
hal.scheduler->delay_microseconds(50);
}
hal.console->println();
}
// hardware reset to restore sensor to normal operation
reset();
}
// updates conversion factors that are dependent upon field_of_view
void AP_OpticalFlow_ADNS3080::update_conversion_factors()
{
// multiply this number by altitude and pixel change to get horizontal
// move (in same units as altitude)
conv_factor = ((1.0f / (float)(ADNS3080_PIXELS_X * scaler))
* 2.0f * tanf(field_of_view / 2.0f));
// 0.00615
radians_to_pixels = (ADNS3080_PIXELS_X * scaler) / field_of_view;
// 162.99
}

108
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h
View File

@ -4,6 +4,9 @@
#include <AP_HAL.h>
#include "AP_OpticalFlow.h"
// timer process runs at 1khz. 50 iterations = 20hz
#define AP_OPTICALFLOW_ADNS3080_NUM_CALLS_FOR_20HZ 50
// orientations for ADNS3080 sensor
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD ROTATION_YAW_180
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_RIGHT ROTATION_YAW_135
@ -15,15 +18,16 @@
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_LEFT ROTATION_YAW_225
// field of view of ADNS3080 sensor lenses
#define AP_OPTICALFLOW_ADNS3080_08_FOV 0.202458 // 11.6 degrees
#define AP_OPTICALFLOW_ADNS3080_08_FOV 0.202458f // 11.6 degrees
// scaler - value returned when sensor is moved equivalent of 1 pixel
#define AP_OPTICALFLOW_ADNS3080_SCALER 1.1
#define AP_OPTICALFLOW_ADNS3080_SCALER_400 1.1f // when resolution set to 400
#define AP_OPTICALFLOW_ADNS3080_SCALER_1600 4.4f // when resolution set to 1600
// ADNS3080 hardware config
#define ADNS3080_PIXELS_X 30
#define ADNS3080_PIXELS_Y 30
#define ADNS3080_CLOCK_SPEED 24000000
#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
@ -58,39 +62,18 @@
#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
// SPI bus definitions
#define ADNS3080_SPI_UNKNOWN 0
#define ADNS3080_SPIBUS_1 1 // standard SPI bus
#define ADNS3080_SPIBUS_3 3 // SPI3
class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow
{
public:
// constructor
AP_OpticalFlow_ADNS3080(uint8_t reset_pin = 0);
AP_OpticalFlow_ADNS3080();
// initialise the sensor
bool init();
uint8_t read_register(uint8_t address);
void write_register(uint8_t address, uint8_t value);
// reset sensor by holding a pin high (or is it low?) for 10us.
void reset();
void init();
// read latest values from sensor and fill in x,y and totals,
// returns true on successful read
@ -98,70 +81,25 @@ public:
// ADNS3080 specific features
// return true if there has been motion since the last time this was called
bool motion() {
if( _motion ) {
_motion = false;
return true;
} else {
return false;
}
}
// called by timer process to read sensor data
void read();
// true if there has been an overflow
bool overflow() { return _overflow; }
void disable_serial_pullup();
// returns true if LED is always on, false if only on when required
bool get_led_always_on();
// set parameter to true if you want LED always on, otherwise false
// for only when required
void set_led_always_on( bool alwaysOn );
// returns resolution (either 400 or 1600 counts per inch)
int16_t get_resolution();
// set parameter to 400 or 1600 counts per inch
void set_resolution(uint16_t resolution);
// get_frame_rate_auto - return true if frame rate is set to "auto",
// false if manual
bool get_frame_rate_auto();
// set_frame_rate_auto(bool) - set frame rate to auto (true),
// or manual (false)
void set_frame_rate_auto(bool auto_frame_rate);
// get_frame_period
uint16_t get_frame_period();
void set_frame_period(uint16_t period);
uint16_t get_frame_rate();
void set_frame_rate(uint16_t rate);
// get_shutter_speed_auto - returns true if shutter speed is adjusted
// automatically, false if manual
bool get_shutter_speed_auto();
// set_shutter_speed_auto - set shutter speed to auto (true),
// or manual (false)
void set_shutter_speed_auto(bool auto_shutter_speed);
uint16_t get_shutter_speed();
void set_shutter_speed(uint16_t shutter_speed);
uint8_t read_register(uint8_t address);
void write_register(uint8_t address, uint8_t value);
// will cause the x,y, dx, dy, and the sensor's motion registers to
// be cleared
void clear_motion();
void clear_motion();
// dumps a 30x30 image to the Serial port
void print_pixel_data();
// print_pixel_data - dumps a 30x30 image to the Serial port
void print_pixel_data();
private:
// pin used for chip reset
uint8_t _reset_pin;
// true if there has been motion
bool _motion;
// true if the x or y data buffers overflowed
bool _overflow;
// update conversion factors based on field of view
void update_conversion_factors();
uint8_t _num_calls; // used to throttle read down to 20hz
// SPI device
AP_HAL::SPIDeviceDriver *_spi;