aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_OpticalFlow: ported to AP_HAL

Browse Source 
* 78 wide
* remove SPI.h, SPI3.h, and FastSerial.h dependencies (though not objects)
* gotta totally refactor HAL SPI driver to fix the rest
This commit is contained in:
Pat Hickey 2012-11-28 10:32:38 -08:00 committed by Andrew Tridgell
parent a38ce8962e
commit c393374d4c
7 changed files with 462 additions and 541 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
libraries/AP_OpticalFlow/AP_OpticalFlow.cpp
View File

@ -12,29 +12,31 @@
#define FORTYFIVE_DEGREES 0.78539816 #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 // pointer to the last instantiated optical flow sensor. Will be turned into
uint8_t AP_OpticalFlow::_num_calls; // number of times we have been called by 1khz timer process. We use this to throttle read down to 20hz // a table if we ever add support for more than one sensor
AP_OpticalFlow* AP_OpticalFlow::_sensor = NULL;
// number of times we have been called by 1khz timer process.
// We use this to throttle read down to 20hz
uint8_t AP_OpticalFlow::_num_calls;
// 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
AP_OpticalFlow::init(bool initCommAPI, AP_PeriodicProcess *scheduler, AP_Semaphore* spi_semaphore, AP_Semaphore* spi3_semaphore)
{ {
_orientation = ROTATION_NONE; _orientation = ROTATION_NONE;
update_conversion_factors(); update_conversion_factors();
return true; // just return true by default return true; // just return true by default
} }
// set_orientation - Rotation vector to transform sensor readings to the body frame. // set_orientation - Rotation vector to transform sensor readings to the body
void // frame.
AP_OpticalFlow::set_orientation(enum Rotation rotation) void AP_OpticalFlow::set_orientation(enum Rotation rotation)
{ {
_orientation = rotation; _orientation = rotation;
} }
// parent method called at 1khz by periodic process // 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) // this is slowed down to 20hz and each instance's update function is called
void // (only one instance is supported at the moment)
AP_OpticalFlow::read(uint32_t now) void AP_OpticalFlow::read(uint32_t now)
{ {
_num_calls++; _num_calls++;
@ -48,27 +50,16 @@ AP_OpticalFlow::read(uint32_t now)
}; };
// read value from the sensor. Should be overridden by derived class // read value from the sensor. Should be overridden by derived class
void void AP_OpticalFlow::update(uint32_t now){ }
AP_OpticalFlow::update(uint32_t now)
{
}
// reads a value from the sensor (will be sensor specific) // reads a value from the sensor (will be sensor specific)
byte uint8_t AP_OpticalFlow::read_register(uint8_t address){ return 0; }
AP_OpticalFlow::read_register(byte address)
{
return 0;
}
// writes a value to one of the sensor's register (will be sensor specific) // writes a value to one of the sensor's register (will be sensor specific)
void void AP_OpticalFlow::write_register(uint8_t address, uint8_t value) {}
AP_OpticalFlow::write_register(byte address, byte value)
{
}
// rotate raw values to arrive at final x,y,dx and dy values // rotate raw values to arrive at final x,y,dx and dy values
void void AP_OpticalFlow::apply_orientation_matrix()
AP_OpticalFlow::apply_orientation_matrix()
{ {
Vector3f rot_vector; Vector3f rot_vector;
rot_vector(raw_dx, raw_dy, 0); rot_vector(raw_dx, raw_dy, 0);
@ -79,31 +70,36 @@ AP_OpticalFlow::apply_orientation_matrix()
dx = rot_vector.x; dx = rot_vector.x;
dy = rot_vector.y; dy = rot_vector.y;
// add rotated values to totals (perhaps this is pointless as we need to take into account yaw, roll, pitch) // add rotated values to totals (perhaps this is pointless as we need
// to take into account yaw, roll, pitch)
x += dx; x += dx;
y += dy; y += dy;
} }
// updatse conversion factors that are dependent upon field_of_view // updates conversion factors that are dependent upon field_of_view
void void AP_OpticalFlow::update_conversion_factors()
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) // multiply this number by altitude and pixel change to get horizontal
// move (in same units as altitude)
conv_factor = ((1.0 / (float)(num_pixels * scaler))
* 2.0 * tan(field_of_view / 2.0));
// 0.00615 // 0.00615
radians_to_pixels = (num_pixels * scaler) / field_of_view; radians_to_pixels = (num_pixels * scaler) / field_of_view;
// 162.99 // 162.99
} }
// updates internal lon and lat with estimation based on optical flow // updates internal lon and lat with estimation based on optical flow
void void AP_OpticalFlow::update_position(float roll, float pitch,
AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float sin_yaw_y, float altitude) float cos_yaw_x, float sin_yaw_y, float altitude)
{ {
float diff_roll = roll - _last_roll; float diff_roll = roll - _last_roll;
float diff_pitch = pitch - _last_pitch; float diff_pitch = pitch - _last_pitch;
// only update position if surface quality is good and angle is not over 45 degrees // only update position if surface quality is good and angle is not
if( surface_quality >= 10 && fabs(roll) <= FORTYFIVE_DEGREES && fabs(pitch) <= FORTYFIVE_DEGREES ) { // over 45 degrees
altitude = max(altitude, 0); if( surface_quality >= 10 && fabs(roll) <= FORTYFIVE_DEGREES
&& fabs(pitch) <= FORTYFIVE_DEGREES ) {
altitude = (altitude > 0 ? altitude : 0);
// calculate expected x,y diff due to roll and pitch change // calculate expected x,y diff due to roll and pitch change
exp_change_x = diff_roll * radians_to_pixels; exp_change_x = diff_roll * radians_to_pixels;
exp_change_y = -diff_pitch * radians_to_pixels; exp_change_y = -diff_pitch * radians_to_pixels;
@ -115,8 +111,12 @@ AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float
float avg_altitude = (altitude + _last_altitude)*0.5; float avg_altitude = (altitude + _last_altitude)*0.5;
// convert raw change to horizontal movement in cm // 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? // perhaps this altitude should actually be the distance to the
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 // ground? i.e. if we are very rolled over it should be longer?
x_cm = -change_x * 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
y_cm = -change_y * avg_altitude * conv_factor;
// convert x/y movements into lon/lat movement // convert x/y movements into lon/lat movement
vlon = x_cm * sin_yaw_y + y_cm * cos_yaw_x; vlon = x_cm * sin_yaw_y + y_cm * cos_yaw_x;

115
libraries/AP_OpticalFlow/AP_OpticalFlow.h
View File

@ -1,5 +1,5 @@
#ifndef AP_OPTICALFLOW_H #ifndef __AP_OPTICALFLOW_H__
#define AP_OPTICALFLOW_H #define __AP_OPTICALFLOW_H__
/* /*
* AP_OpticalFlow.cpp - OpticalFlow Base Class for Ardupilot Mega * AP_OpticalFlow.cpp - OpticalFlow Base Class for Ardupilot Mega
@ -11,38 +11,49 @@
* version 2.1 of the License, or (at your option) any later version. * version 2.1 of the License, or (at your option) any later version.
* *
* Methods: * Methods:
* init() : initializate sensor and library. * init() : initializate sensor and library.
* read : reads latest value from OpticalFlow and stores values in x,y, surface_quality parameter * read : reads latest value from OpticalFlow and
* read_register() : reads a value from the sensor (will be sensor specific) * stores values in x,y, surface_quality parameter
* write_register() : writes a value to one of the sensor's register (will be sensor specific) * 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_Math.h>
#include <AP_Common.h>
#include <AP_PeriodicProcess.h>
#include <AP_Semaphore.h>
#define AP_OPTICALFLOW_NUM_CALLS_FOR_10HZ 100 // timer process runs at 1khz. 100 iterations = 10hz // 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_10HZ 100
#define AP_OPTICALFLOW_NUM_CALLS_FOR_50HZ 20 // timer process runs at 1khz. 20 iterations = 50hz // timer process runs at 1khz. 50 iterations = 20hz
#define AP_OPTICALFLOW_NUM_CALLS_FOR_20HZ 50
// timer process runs at 1khz. 20 iterations = 50hz
#define AP_OPTICALFLOW_NUM_CALLS_FOR_50HZ 20
class AP_OpticalFlow class AP_OpticalFlow
{ {
public: public:
int raw_dx, raw_dy; // raw sensor change in x and y position (i.e. unrotated) // 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 raw_dx, raw_dy;
int x,y; // total x,y position // image quality (below 15 you really can't trust the x,y values returned)
int dx,dy; // rotated change in x and y position int surface_quality;
float vlon, vlat; // position as offsets from original position // total x,y position
unsigned long last_update; // millis() time of last update int x,y;
float field_of_view; // field of view in Radians // rotated change in x and y position
float scaler; // number returned from sensor when moved one pixel int dx,dy;
int num_pixels; // number of pixels of resolution in the sensor // position as offsets from original position
// temp variables - delete me! float vlon, vlat;
float exp_change_x, exp_change_y; // millis() time of last update
float change_x, change_y; uint32_t last_update;
float x_cm, y_cm; // field of view in Radians
float field_of_view;
// number returned from sensor when moved one pixel
float scaler;
// number of pixels of resolution in the sensor
int num_pixels;
// temp - delete me!
float exp_change_x, exp_change_y;
float change_x, change_y;
float x_cm, y_cm;
AP_OpticalFlow() { AP_OpticalFlow() {
_sensor = this; _sensor = this;
@ -50,26 +61,46 @@ public:
~AP_OpticalFlow() { ~AP_OpticalFlow() {
_sensor = NULL; _sensor = NULL;
}; };
virtual bool init(bool initCommAPI, AP_PeriodicProcess *scheduler, AP_Semaphore* spi_semaphore = NULL, AP_Semaphore* spi3_semaphore = NULL); // 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 bool init();
virtual byte read_register(byte address); // parameter controls whether I2C/SPI interface is initialised
virtual void write_register(byte address, byte value); // (set to false if other devices are on the I2C/SPI bus and have already
virtual void set_orientation(enum Rotation rotation); // Rotation vector to transform sensor readings to the body frame. // initialised the interface)
virtual void set_field_of_view(const float fov) { field_of_view = fov; update_conversion_factors(); }; // sets field of view of sensor virtual uint8_t read_register(uint8_t address);
static void read(uint32_t now); // called by timer process to read sensor data from all attached sensors virtual void write_register(uint8_t address, uint8_t value);
virtual void update(uint32_t now); // read latest values from sensor and fill in x,y and totals. // Rotation vector to transform sensor readings to the body frame.
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 virtual 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
static void read(uint32_t now);
// read latest values from sensor and fill in x,y and totals.
virtual void update(uint32_t now);
// updates internal lon and lat with estimation based on optical flow
virtual void update_position(float roll,
float pitch, float cos_yaw_x, float sin_yaw_y, float altitude);
protected: 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 // pointer to the last instantiated optical flow sensor. Will be turned
enum Rotation _orientation; // into a table if we ever add support for more than one sensor
float conv_factor; // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude) static AP_OpticalFlow * _sensor;
float radians_to_pixels; enum Rotation _orientation;
float _last_roll, _last_pitch, _last_altitude; // multiply this number by altitude and pixel change to get horizontal
virtual void apply_orientation_matrix(); // rotate raw values to arrive at final x,y,dx and dy values // move (in same units as altitude)
virtual void update_conversion_factors(); float conv_factor;
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: private:
static uint8_t _num_calls; // number of times we have been called by 1khz timer process. We use this to throttle read down to 20hz // number of times we have been called by 1khz timer process.
// We use this to throttle read down to 20hz
static uint8_t _num_calls;
}; };
#include "AP_OpticalFlow_ADNS3080.h" #include "AP_OpticalFlow_ADNS3080.h"

392
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp
View File

@ -1,5 +1,6 @@
/* /*
* AP_OpticalFlow_ADNS3080.cpp - ADNS3080 OpticalFlow Library for Ardupilot Mega * AP_OpticalFlow_ADNS3080.cpp - ADNS3080 OpticalFlow Library for
* Ardupilot Mega
* Code by Randy Mackay. DIYDrones.com * Code by Randy Mackay. DIYDrones.com
* *
* This library is free software; you can redistribute it and/or * This library is free software; you can redistribute it and/or
@ -9,30 +10,13 @@
* *
*/ */
#include <AP_HAL.h>
#include "AP_OpticalFlow_ADNS3080.h" #include "AP_OpticalFlow_ADNS3080.h"
#include "SPI.h"
#include "SPI3.h"
#include "AP_Semaphore.h"
#if defined(ARDUINO) && ARDUINO >= 100 extern const AP_HAL::HAL& hal;
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#define AP_SPI_TIMEOUT 1000 #define AP_SPI_TIMEOUT 1000
// We use Serial Port 2 in SPI Mode
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
#define ADNS3080_SPI_MISO 50 // PB3
#define ADNS3080_SPI_MOSI 51 // PB2
#define ADNS3080_SPI_SCK 52 // PB1
#else // normal arduino SPI pins...these need to be checked
#define ADNS3080_SPI_MISO 12 // MISO
#define ADNS3080_SPI_MOSI 11 // MOSI
#define ADNS3080_SPI_SCK 13 // SCK
#endif
union NumericIntType union NumericIntType
{ {
int16_t intValue; int16_t intValue;
@ -41,11 +25,8 @@ union NumericIntType
}; };
// Constructors //////////////////////////////////////////////////////////////// // Constructors ////////////////////////////////////////////////////////////////
AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080(int16_t cs_pin, int16_t reset_pin) : AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080(uint8_t reset_pin) :
_cs_pin(cs_pin), _reset_pin(reset_pin)
_reset_pin(reset_pin),
_spi_bus(ADNS3080_SPI_UNKNOWN),
_spi_semaphore(NULL)
{ {
num_pixels = ADNS3080_PIXELS_X; num_pixels = ADNS3080_PIXELS_X;
field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV; field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV;
@ -54,206 +35,124 @@ AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080(int16_t cs_pin, int16_t reset_p
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
// init - initialise sensor // init - initialise sensor
// assumes SPI bus has been initialised but will attempt to initialise nonstandard SPI3 bus if required // assumes SPI bus has been initialised but will attempt to initialise
// nonstandard SPI3 bus if required
bool bool
AP_OpticalFlow_ADNS3080::init(bool initCommAPI, AP_PeriodicProcess *scheduler, AP_Semaphore* spi_semaphore, AP_Semaphore* spi3_semaphore) AP_OpticalFlow_ADNS3080::init()
{ {
int8_t retry = 0; int8_t retry = 0;
bool retvalue = false; bool retvalue = false;
// suspend timer while we set-up SPI communication // suspend timer while we set-up SPI communication
scheduler->suspend_timer(); hal.scheduler->suspend_timer_procs();
pinMode(_cs_pin,OUTPUT);
if( _reset_pin != 0) if( _reset_pin != 0)
pinMode(ADNS3080_RESET,OUTPUT); hal.gpio->pinMode(_reset_pin, GPIO_OUTPUT);
digitalWrite(_cs_pin,HIGH); // disable device (Chip select is active low)
// reset the device // reset the device
reset(); reset();
// start the SPI library: // check 3 times for the sensor on standard SPI bus
if( initCommAPI ) { _spi = hal.spi->device(AP_HAL::SPIDevice_ADNS3080_SPI0);
pinMode(ADNS3080_SPI_MOSI,OUTPUT); if (_spi == NULL) {
pinMode(ADNS3080_SPI_MISO,INPUT); retvalue = false; goto finish;
pinMode(ADNS3080_SPI_SCK,OUTPUT);
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV8); // 2MHZ SPI rate
} }
// check 3 times for the sensor on standard SPI bus while( retvalue == false && retry < 3 ) {
_spi_bus = ADNS3080_SPIBUS_1; if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) {
_spi_semaphore = spi_semaphore; retvalue = true;
goto finish;
}
retry++;
}
// if not found, check 3 times on SPI3
_spi = hal.spi->device(AP_HAL::SPIDevice_ADNS3080_SPI3);
if (_spi == NULL) {
retvalue = false; goto finish;
}
retry = 0;
while( retvalue == false && retry < 3 ) { while( retvalue == false && retry < 3 ) {
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) { if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) {
retvalue = true; retvalue = true;
} }
retry++; retry++;
} }
// If we fail to find on SPI3, no connection available.
retvalue = false;
_spi = NULL;
// if not found, check 3 times on SPI3 finish:
if( !retvalue ) {
// start the SPI3 library:
if( initCommAPI ) {
SPI3.begin();
SPI3.setDataMode(SPI3_MODE3); // Mode3
SPI3.setSpeed(SPI3_SPEED_2MHZ); // 2MHZ SPI rate
}
_spi_bus = ADNS3080_SPIBUS_3;
_spi_semaphore = spi3_semaphore;
retry = 0;
while( retvalue == false && retry < 3 ) {
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) {
retvalue = true;
}
retry++;
}
}
// resume timer // resume timer
scheduler->resume_timer(); hal.scheduler->resume_timer_procs();
// if device is working register the global static read function to be called at 1khz // if device is working register the global static read function to
// be called at 1khz
if( retvalue ) { if( retvalue ) {
scheduler->register_process( AP_OpticalFlow_ADNS3080::read ); hal.scheduler->register_timer_process( AP_OpticalFlow_ADNS3080::read );
}else{
_spi_bus = ADNS3080_SPI_UNKNOWN;
} }
return retvalue; return retvalue;
} }
//
// backup_spi_settings - checks current SPI settings (clock speed, etc), sets values to what we need
//
void
AP_OpticalFlow_ADNS3080::backup_spi_settings()
{
if( _spi_bus == ADNS3080_SPIBUS_1 ) {
// store current spi mode and data rate
orig_spi_settings_spcr = SPCR & (CPOL | CPHA | SPR1 | SPR0);
// set to our required values
SPI.setDataMode(SPI_MODE3);
SPI.setClockDivider(SPI_CLOCK_DIV8); // 2MHZ SPI rate
}else if( _spi_bus == ADNS3080_SPIBUS_3 ) {
/* Wait for empty transmit buffer */
while ( !( UCSR3A & (1<<UDRE3)) ) ;
// store current spi values
orig_spi3_settings_ucsr3c = UCSR3C;
orig_spi3_settings_ubrr3 = UBRR3;
// set to our required values
SPI3.setDataMode(SPI3_MODE3);
SPI3.setSpeed(SPI3_SPEED_2MHZ); // 2MHZ SPI rate
}
}
// restore_spi_settings - restores SPI settings (clock speed, etc) to what their values were before the sensor used the bus
void
AP_OpticalFlow_ADNS3080::restore_spi_settings()
{
byte temp;
if( _spi_bus == ADNS3080_SPIBUS_1 ) {
// split off the two bits we need to write
temp = SPCR & ~(CPOL | CPHA | SPR1 | SPR0);
temp |= orig_spi_settings_spcr;
// write back the bits
SPCR = temp;
}else if( _spi_bus == ADNS3080_SPIBUS_3 ) {
/* Wait for empty transmit buffer */
while ( !( UCSR3A & (1<<UDRE3)) ) ;
// restore UCSRC3C (spi mode) and UBBR3 (speed)
UCSR3C = orig_spi3_settings_ucsr3c;
UBRR3 = orig_spi3_settings_ubrr3;
}
}
// Read a register from the sensor // Read a register from the sensor
byte uint8_t AP_OpticalFlow_ADNS3080::read_register(uint8_t address)
AP_OpticalFlow_ADNS3080::read_register(byte address)
{ {
uint8_t result = 0; if (_spi == NULL) return 0;
// get spi semaphore if required // get spi semaphore if required
if( _spi_semaphore != NULL ) { AP_HAL::Semaphore* sem = _spi->get_semaphore();
if( sem != NULL ) {
// if failed to get semaphore then just quietly fail // if failed to get semaphore then just quietly fail
if( !_spi_semaphore->get(this) ) { if( !sem->get(this) ) {
return 0; return 0;
} }
} }
backup_spi_settings(); _spi->cs_assert();
// send the device the register you want to read:
_spi->transfer(address);
hal.scheduler->delay_microseconds(50);
// send a value of 0 to read the first byte returned:
uint8_t result = _spi->transfer(0x00);
// take the chip select low to select the device _spi->cs_release();
digitalWrite(_cs_pin, LOW);
if( _spi_bus == ADNS3080_SPIBUS_1 ) {
SPI.transfer(address); // send the device the register you want to read:
delayMicroseconds(50); // small delay
result = SPI.transfer(0x00); // send a value of 0 to read the first byte returned:
}else if( _spi_bus == ADNS3080_SPIBUS_3 ) {
SPI3.transfer(address); // send the device the register you want to read:
delayMicroseconds(50); // small delay
result = SPI3.transfer(0x00); // send a value of 0 to read the first byte returned:
}
// take the chip select high to de-select:
digitalWrite(_cs_pin, HIGH);
restore_spi_settings();
// get spi semaphore if required // get spi semaphore if required
if( _spi_semaphore != NULL ) { if( sem != NULL ) {
_spi_semaphore->release(this); sem->release(this);
} }
return result; return result;
} }
// write a value to one of the sensor's registers // write a value to one of the sensor's registers
void void AP_OpticalFlow_ADNS3080::write_register(uint8_t address, uint8_t value)
AP_OpticalFlow_ADNS3080::write_register(byte address, byte value)
{ {
if (_spi == NULL) return;
AP_HAL::Semaphore* sem = _spi->get_semaphore();
// get spi semaphore if required // get spi semaphore if required
if( _spi_semaphore != NULL ) { if( sem != NULL ) {
// if failed to get semaphore then just quietly fail // if failed to get semaphore then just quietly fail
if( !_spi_semaphore->get(this) ) { if( !sem->get(this) ) {
return; return;
} }
} }
backup_spi_settings(); _spi->cs_assert();
// take the chip select low to select the device // send register address
digitalWrite(_cs_pin, LOW); _spi->transfer(address | 0x80 );
hal.scheduler->delay_microseconds(50);
if( _spi_bus == ADNS3080_SPIBUS_1 ) { // send data
SPI.transfer(address | 0x80 ); // send register address _spi->transfer(value);
delayMicroseconds(50); // small delay
SPI.transfer(value); // send data
}else if( _spi_bus == ADNS3080_SPIBUS_3 ) {
SPI3.transfer(address | 0x80 ); // send register address
delayMicroseconds(50); // small delay
SPI3.transfer(value); // send data
}
// take the chip select high to de-select:
digitalWrite(_cs_pin, HIGH);
restore_spi_settings();
_spi->cs_release();
// get spi3 semaphore if required // get spi3 semaphore if required
if( _spi_semaphore != NULL ) { if( sem != NULL ) {
_spi_semaphore->release(this); sem->release(this);
} }
} }
@ -265,25 +164,28 @@ AP_OpticalFlow_ADNS3080::reset()
if( _reset_pin == 0) if( _reset_pin == 0)
return; return;
digitalWrite(_reset_pin,HIGH); // reset sensor // reset sensor
delayMicroseconds(10); hal.gpio->write(_reset_pin, 1);
digitalWrite(_reset_pin,LOW); // return sensor to normal 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 // read latest values from sensor and fill in x,y and totals
void void
AP_OpticalFlow_ADNS3080::update(uint32_t now) AP_OpticalFlow_ADNS3080::update(uint32_t now)
{ {
byte motion_reg; uint8_t motion_reg;
surface_quality = (uint16_t)read_register(ADNS3080_SQUAL); surface_quality = (uint16_t)read_register(ADNS3080_SQUAL);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
// check for movement, update x,y values // check for movement, update x,y values
motion_reg = read_register(ADNS3080_MOTION); motion_reg = read_register(ADNS3080_MOTION);
_overflow = ((motion_reg & 0x10) != 0); // check if we've had an overflow // 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)); raw_dx = ((int8_t)read_register(ADNS3080_DELTA_X));
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
raw_dy = ((int8_t)read_register(ADNS3080_DELTA_Y)); raw_dy = ((int8_t)read_register(ADNS3080_DELTA_Y));
_motion = true; _motion = true;
}else{ }else{
@ -291,7 +193,7 @@ AP_OpticalFlow_ADNS3080::update(uint32_t now)
raw_dy = 0; raw_dy = 0;
} }
last_update = millis(); last_update = hal.scheduler->millis();
apply_orientation_matrix(); apply_orientation_matrix();
} }
@ -299,32 +201,31 @@ AP_OpticalFlow_ADNS3080::update(uint32_t now)
void void
AP_OpticalFlow_ADNS3080::disable_serial_pullup() AP_OpticalFlow_ADNS3080::disable_serial_pullup()
{ {
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG); uint8_t regVal = read_register(ADNS3080_EXTENDED_CONFIG);
regVal = (regVal | ADNS3080_SERIALNPU_OFF); regVal = (regVal | ADNS3080_SERIALNPU_OFF);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
write_register(ADNS3080_EXTENDED_CONFIG, regVal); write_register(ADNS3080_EXTENDED_CONFIG, regVal);
} }
// get_led_always_on - returns true if LED is always on, false if only on when required // get_led_always_on - returns true if LED is always on, false if only on
bool // when required
AP_OpticalFlow_ADNS3080::get_led_always_on() bool AP_OpticalFlow_ADNS3080::get_led_always_on()
{ {
return ( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x40) > 0 ); return ( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x40) > 0 );
} }
// set_led_always_on - set parameter to true if you want LED always on, otherwise false for only when required // set_led_always_on - set parameter to true if you want LED always on,
void // otherwise false for only when required
AP_OpticalFlow_ADNS3080::set_led_always_on( bool alwaysOn ) void AP_OpticalFlow_ADNS3080::set_led_always_on( bool alwaysOn )
{ {
byte regVal = read_register(ADNS3080_CONFIGURATION_BITS); uint8_t regVal = read_register(ADNS3080_CONFIGURATION_BITS);
regVal = (regVal & 0xbf) | (alwaysOn << 6); regVal = (regVal & 0xbf) | (alwaysOn << 6);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
write_register(ADNS3080_CONFIGURATION_BITS, regVal); write_register(ADNS3080_CONFIGURATION_BITS, regVal);
} }
// returns resolution (either 400 or 1600 counts per inch) // returns resolution (either 400 or 1600 counts per inch)
int16_t int16_t AP_OpticalFlow_ADNS3080::get_resolution()
AP_OpticalFlow_ADNS3080::get_resolution()
{ {
if( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x10) == 0 ) if( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x10) == 0 )
return 400; return 400;
@ -333,10 +234,9 @@ AP_OpticalFlow_ADNS3080::get_resolution()
} }
// set parameter to 400 or 1600 counts per inch // set parameter to 400 or 1600 counts per inch
void void AP_OpticalFlow_ADNS3080::set_resolution(uint16_t resolution)
AP_OpticalFlow_ADNS3080::set_resolution(uint16_t resolution)
{ {
byte regVal = read_register(ADNS3080_CONFIGURATION_BITS); uint8_t regVal = read_register(ADNS3080_CONFIGURATION_BITS);
if( resolution == ADNS3080_RESOLUTION_400 ) { if( resolution == ADNS3080_RESOLUTION_400 ) {
regVal &= ~0x10; regVal &= ~0x10;
@ -346,7 +246,7 @@ AP_OpticalFlow_ADNS3080::set_resolution(uint16_t resolution)
scaler = AP_OPTICALFLOW_ADNS3080_SCALER * 4; scaler = AP_OPTICALFLOW_ADNS3080_SCALER * 4;
} }
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
write_register(ADNS3080_CONFIGURATION_BITS, regVal); write_register(ADNS3080_CONFIGURATION_BITS, regVal);
// this will affect conversion factors so update them // this will affect conversion factors so update them
@ -354,10 +254,9 @@ AP_OpticalFlow_ADNS3080::set_resolution(uint16_t resolution)
} }
// get_frame_rate_auto - return whether frame rate is set to "auto" or manual // get_frame_rate_auto - return whether frame rate is set to "auto" or manual
bool bool AP_OpticalFlow_ADNS3080::get_frame_rate_auto()
AP_OpticalFlow_ADNS3080::get_frame_rate_auto()
{ {
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG); uint8_t regVal = read_register(ADNS3080_EXTENDED_CONFIG);
if( (regVal & 0x01) != 0 ) { if( (regVal & 0x01) != 0 ) {
return false; return false;
}else{ }else{
@ -366,17 +265,16 @@ AP_OpticalFlow_ADNS3080::get_frame_rate_auto()
} }
// set_frame_rate_auto - set frame rate to auto (true) or manual (false) // set_frame_rate_auto - set frame rate to auto (true) or manual (false)
void void AP_OpticalFlow_ADNS3080::set_frame_rate_auto(bool auto_frame_rate)
AP_OpticalFlow_ADNS3080::set_frame_rate_auto(bool auto_frame_rate)
{ {
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG); uint8_t regVal = read_register(ADNS3080_EXTENDED_CONFIG);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
if( auto_frame_rate == true ) { if( auto_frame_rate == true ) {
// set specific frame period // set specific frame period
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,0xE0); write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,0xE0);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,0x1A); write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,0x1A);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
// decide what value to update in extended config // decide what value to update in extended config
regVal = (regVal & ~0x01); regVal = (regVal & ~0x01);
@ -388,44 +286,40 @@ AP_OpticalFlow_ADNS3080::set_frame_rate_auto(bool auto_frame_rate)
} }
// get frame period // get frame period
uint16_t uint16_t AP_OpticalFlow_ADNS3080::get_frame_period()
AP_OpticalFlow_ADNS3080::get_frame_period()
{ {
NumericIntType aNum; NumericIntType aNum;
aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER); aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER); aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER);
return aNum.uintValue; return aNum.uintValue;
} }
// set frame period // set frame period
void void AP_OpticalFlow_ADNS3080::set_frame_period(uint16_t period)
AP_OpticalFlow_ADNS3080::set_frame_period(uint16_t period)
{ {
NumericIntType aNum; NumericIntType aNum;
aNum.uintValue = period; aNum.uintValue = period;
// set frame rate to manual // set frame rate to manual
set_frame_rate_auto(false); set_frame_rate_auto(false);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
// set specific frame period // set specific frame period
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]); write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]); write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]);
} }
uint16_t uint16_t AP_OpticalFlow_ADNS3080::get_frame_rate()
AP_OpticalFlow_ADNS3080::get_frame_rate()
{ {
uint32_t clockSpeed = ADNS3080_CLOCK_SPEED; uint32_t clockSpeed = ADNS3080_CLOCK_SPEED;
uint16_t rate = clockSpeed / get_frame_period(); uint16_t rate = clockSpeed / get_frame_period();
return rate; return rate;
} }
void void AP_OpticalFlow_ADNS3080::set_frame_rate(uint16_t rate)
AP_OpticalFlow_ADNS3080::set_frame_rate(uint16_t rate)
{ {
uint32_t clockSpeed = ADNS3080_CLOCK_SPEED; uint32_t clockSpeed = ADNS3080_CLOCK_SPEED;
uint16_t period = (uint16_t)(clockSpeed / (uint32_t)rate); uint16_t period = (uint16_t)(clockSpeed / (uint32_t)rate);
@ -433,9 +327,9 @@ AP_OpticalFlow_ADNS3080::set_frame_rate(uint16_t rate)
set_frame_period(period); set_frame_period(period);
} }
// get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual // get_shutter_speed_auto - returns true if shutter speed is adjusted
bool // automatically, false if manual
AP_OpticalFlow_ADNS3080::get_shutter_speed_auto() bool AP_OpticalFlow_ADNS3080::get_shutter_speed_auto()
{ {
uint8_t regVal = read_register(ADNS3080_EXTENDED_CONFIG); uint8_t regVal = read_register(ADNS3080_EXTENDED_CONFIG);
if( (regVal & 0x02) > 0 ) { if( (regVal & 0x02) > 0 ) {
@ -446,17 +340,16 @@ AP_OpticalFlow_ADNS3080::get_shutter_speed_auto()
} }
// set_shutter_speed_auto - set shutter speed to auto (true), or manual (false) // set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
void void AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool auto_shutter_speed)
AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool auto_shutter_speed)
{ {
uint8_t regVal = read_register(ADNS3080_EXTENDED_CONFIG); uint8_t regVal = read_register(ADNS3080_EXTENDED_CONFIG);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
if( auto_shutter_speed ) { if( auto_shutter_speed ) {
// return shutter speed max to default // return shutter speed max to default
write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,0x8c); write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,0x8c);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,0x20); write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,0x20);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
// determine value to put into extended config // determine value to put into extended config
regVal &= ~0x02; regVal &= ~0x02;
@ -465,16 +358,16 @@ AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool auto_shutter_speed)
regVal |= 0x02; regVal |= 0x02;
} }
write_register(ADNS3080_EXTENDED_CONFIG, regVal); write_register(ADNS3080_EXTENDED_CONFIG, regVal);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
} }
// get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual // get_shutter_speed_auto - returns true if shutter speed is adjusted
uint16_t // automatically, false if manual
AP_OpticalFlow_ADNS3080::get_shutter_speed() uint16_t AP_OpticalFlow_ADNS3080::get_shutter_speed()
{ {
NumericIntType aNum; NumericIntType aNum;
aNum.byteValue[1] = read_register(ADNS3080_SHUTTER_UPPER); aNum.byteValue[1] = read_register(ADNS3080_SHUTTER_UPPER);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
aNum.byteValue[0] = read_register(ADNS3080_SHUTTER_LOWER); aNum.byteValue[0] = read_register(ADNS3080_SHUTTER_LOWER);
return aNum.uintValue; return aNum.uintValue;
} }
@ -489,33 +382,35 @@ AP_OpticalFlow_ADNS3080::set_shutter_speed(uint16_t shutter_speed)
// set shutter speed to manual // set shutter speed to manual
set_shutter_speed_auto(false); set_shutter_speed_auto(false);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
// set specific shutter speed // set specific shutter speed
write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]); write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,aNum.byteValue[1]); write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,aNum.byteValue[1]);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
// larger delay // larger delay
delay(50); hal.scheduler->delay(50);
// need to update frame period to cause shutter value to take effect // need to update frame period to cause shutter value to take effect
aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER); aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER); aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]); write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]); write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]);
delayMicroseconds(50); // small delay hal.scheduler->delay_microseconds(50);
} }
// clear_motion - will cause the Delta_X, Delta_Y, and internal motion registers to be cleared // clear_motion - will cause the Delta_X, Delta_Y, and internal motion
void // registers to be cleared
AP_OpticalFlow_ADNS3080::clear_motion() void AP_OpticalFlow_ADNS3080::clear_motion()
{ {
write_register(ADNS3080_MOTION_CLEAR,0xFF); // writing anything to this register will clear the sensor's motion registers // writing anything to this register will clear the sensor's motion
// registers
write_register(ADNS3080_MOTION_CLEAR,0xFF);
x = 0; x = 0;
y = 0; y = 0;
dx = 0; dx = 0;
@ -523,9 +418,9 @@ AP_OpticalFlow_ADNS3080::clear_motion()
_motion = false; _motion = false;
} }
// get_pixel_data - captures an image from the sensor and stores it to the pixe_data array // get_pixel_data - captures an image from the sensor and stores it to the
void // pixe_data array
AP_OpticalFlow_ADNS3080::print_pixel_data() void AP_OpticalFlow_ADNS3080::print_pixel_data()
{ {
int16_t i,j; int16_t i,j;
bool isFirstPixel = true; bool isFirstPixel = true;
@ -536,23 +431,26 @@ AP_OpticalFlow_ADNS3080::print_pixel_data()
write_register(ADNS3080_FRAME_CAPTURE,0x83); write_register(ADNS3080_FRAME_CAPTURE,0x83);
// wait 3 frame periods + 10 nanoseconds for frame to be captured // wait 3 frame periods + 10 nanoseconds for frame to be captured
delayMicroseconds(1510); // min frame speed is 2000 frames/second so 1 frame = 500 nano seconds. so 500 x 3 + 10 = 1510 // min frame speed is 2000 frames/second so 1 frame = 500 nano seconds.
// so 500 x 3 + 10 = 1510
hal.scheduler->delay_microseconds(1510);
// display the pixel data // display the pixel data
for( i=0; i<ADNS3080_PIXELS_Y; i++ ) { for( i=0; i<ADNS3080_PIXELS_Y; i++ ) {
for( j=0; j<ADNS3080_PIXELS_X; j++ ) { for( j=0; j<ADNS3080_PIXELS_X; j++ ) {
regValue = read_register(ADNS3080_FRAME_CAPTURE); regValue = read_register(ADNS3080_FRAME_CAPTURE);
if( isFirstPixel && (regValue & 0x40) == 0 ) { if( isFirstPixel && (regValue & 0x40) == 0 ) {
Serial.print_P(PSTR("failed to find first pixel")); hal.console->println_P(
PSTR("Optflow: failed to find first pixel"));
} }
isFirstPixel = false; isFirstPixel = false;
pixelValue = ( regValue << 2); pixelValue = ( regValue << 2 );
Serial.print(pixelValue,DEC); hal.console->print(pixelValue,DEC);
if( j!= ADNS3080_PIXELS_X-1 ) if( j!= ADNS3080_PIXELS_X-1 )
Serial.print_P(PSTR(",")); hal.console->print_P(PSTR(","));
delayMicroseconds(50); hal.scheduler->delay_microseconds(50);
} }
Serial.println(); hal.console->println();
} }
// hardware reset to restore sensor to normal operation // hardware reset to restore sensor to normal operation

106
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h
View File

@ -1,13 +1,9 @@
#ifndef __AP_OPTICALFLOW_ADNS3080_H__ #ifndef __AP_OPTICALFLOW_ADNS3080_H__
#define __AP_OPTICALFLOW_ADNS3080_H__ #define __AP_OPTICALFLOW_ADNS3080_H__
#include <AP_Semaphore.h> #include <AP_HAL.h>
#include "AP_OpticalFlow.h" #include "AP_OpticalFlow.h"
// default pin settings
#define ADNS3080_CHIP_SELECT 34 // PC3
#define ADNS3080_RESET 0 // reset pin is unattached by default
// orientations for ADNS3080 sensor // orientations for ADNS3080 sensor
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD ROTATION_YAW_180 #define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD ROTATION_YAW_180
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_RIGHT ROTATION_YAW_135 #define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_RIGHT ROTATION_YAW_135
@ -83,63 +79,85 @@
class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow
{ {
public: public:
AP_OpticalFlow_ADNS3080(int16_t cs_pin = ADNS3080_CHIP_SELECT, int16_t reset_pin = ADNS3080_RESET); AP_OpticalFlow_ADNS3080(uint8_t reset_pin = 0);
bool init(bool initCommAPI, AP_PeriodicProcess *scheduler, AP_Semaphore* spi_semaphore = NULL, AP_Semaphore* spi3_semaphore = NULL); // parameter controls whether SPI interface is initialised (set to false if other devices are on the I2C/SPI bus and have already initialised the interface) bool init();
uint8_t read_register(uint8_t address); uint8_t read_register(uint8_t address);
void write_register(uint8_t address, uint8_t value); void write_register(uint8_t address, uint8_t value);
void reset(); // reset sensor by holding a pin high (or is it low?) for 10us. // reset sensor by holding a pin high (or is it low?) for 10us.
void update(uint32_t now); // read latest values from sensor and fill in x,y and totals, return true on successful read void reset();
// read latest values from sensor and fill in x,y and totals,
// return true on successful read
void update(uint32_t now);
// ADNS3080 specific features // ADNS3080 specific features
// return true if there has been motion since the last time this was called // 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; } } bool motion() {
if( _motion ) {
_motion = false;
return true;
} else {
return false;
}
}
bool overflow() { return _overflow; } // true if there has been an overflow // true if there has been an overflow
bool overflow() { return _overflow; }
void disable_serial_pullup(); void disable_serial_pullup();
bool get_led_always_on(); // returns true if LED is always on, false if only on when required // 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 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 );
int16_t get_resolution(); // returns resolution (either 400 or 1600 counts per inch) // returns resolution (either 400 or 1600 counts per inch)
void set_resolution(uint16_t resolution); // set parameter to 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);
bool get_frame_rate_auto(); // get_frame_rate_auto - return true if frame rate is set to "auto", false if manual // get_frame_rate_auto - return true if frame rate is set to "auto",
void set_frame_rate_auto(bool auto_frame_rate); // set_frame_rate_auto(bool) - set frame rate to auto (true), or manual (false) // 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);
uint16_t get_frame_period(); // get_frame_period // get_frame_period
void set_frame_period(uint16_t period); uint16_t get_frame_period();
void set_frame_period(uint16_t period);
uint16_t get_frame_rate(); uint16_t get_frame_rate();
void set_frame_rate(uint16_t rate); void set_frame_rate(uint16_t rate);
bool get_shutter_speed_auto(); // get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual // get_shutter_speed_auto - returns true if shutter speed is adjusted
void set_shutter_speed_auto(bool auto_shutter_speed); // set_shutter_speed_auto - set shutter speed to auto (true), or manual (false) // 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(); uint16_t get_shutter_speed();
void set_shutter_speed(uint16_t shutter_speed); void set_shutter_speed(uint16_t shutter_speed);
void clear_motion(); // will cause the x,y, dx, dy, and the sensor's motion registers to be cleared // will cause the x,y, dx, dy, and the sensor's motion registers to
// be cleared
void clear_motion();
void print_pixel_data(); // dumps a 30x30 image to the Serial port // dumps a 30x30 image to the Serial port
void print_pixel_data();
private: private:
// bytes to store SPI settings // pin used for chip reset
uint8_t orig_spi_settings_spcr; // spi1's mode uint8_t _reset_pin;
uint8_t orig_spi3_settings_ucsr3c; // spi3's mode // true if there has been motion
uint8_t orig_spi3_settings_ubrr3; // spi3's speed bool _motion;
// true if the x or y data buffers overflowed
bool _overflow;
// save and restore SPI settings // SPI device
void backup_spi_settings(); AP_HAL::SPIDeviceDriver *_spi;
void restore_spi_settings();
int16_t _cs_pin; // pin used for chip select
int16_t _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
uint8_t _spi_bus; // 0 = unknown, 1 = using SPI, 3 = using SPI3
AP_Semaphore* _spi_semaphore;
}; };
#endif #endif

314
libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde
View File

@ -3,62 +3,34 @@
* Code by Randy Mackay. DIYDrones.com * Code by Randy Mackay. DIYDrones.com
*/ */
#include <FastSerial.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Progmem.h>
#include <AP_Param.h> #include <AP_Param.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math.h>
#include <SPI.h> // Arduino SPI library #include <AP_HAL.h>
#include <SPI3.h> // SPI3 library #include <AP_HAL_AVR.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_PeriodicProcess.h>
#include <AP_Semaphore.h> // for removing conflict with dataflash on SPI3 bus
#include "AP_OpticalFlow.h" // ArduCopter OpticalFlow Library
//////////////////////////////////////////////////////////////////////////////// #include <AP_OpticalFlow.h>
// Serial ports
////////////////////////////////////////////////////////////////////////////////
//
// Note that FastSerial port buffers are allocated at ::begin time,
// so there is not much of a penalty to defining ports that we don't
// use.
//
FastSerialPort0(Serial); // FTDI/console
#define MS5611_CS 40 // barometer chip select pin const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
#define CONFIG_MPU6000_CHIP_SELECT_PIN 53 // MPU600 chip select pin
//AP_OpticalFlow_ADNS3080 flowSensor; // for APM1 AP_OpticalFlow_ADNS3080 flowSensor;
AP_OpticalFlow_ADNS3080 flowSensor(A3); // override chip select pin to use A3 if using APM2 or APM2.5
Arduino_Mega_ISR_Registry isr_registry;
AP_TimerProcess scheduler;
void setup() void setup()
{ {
Serial.begin(115200); hal.console->println("ArduPilot Mega OpticalFlow library test ver 1.5");
Serial.println("ArduPilot Mega OpticalFlow library test ver 1.5");
delay(1000); hal.scheduler->delay(1000);
// disable other devices on this bus
pinMode(MS5611_CS,OUTPUT);
pinMode(CONFIG_MPU6000_CHIP_SELECT_PIN,OUTPUT);
digitalWrite(MS5611_CS, HIGH);
digitalWrite(CONFIG_MPU6000_CHIP_SELECT_PIN, HIGH);
// initialise timer
isr_registry.init();
scheduler.init(&isr_registry);
// flowSensor initialization // flowSensor initialization
if( flowSensor.init(true, &scheduler) == false ) { if( flowSensor.init() == false ) {
Serial.print("Failed to initialise ADNS3080 "); hal.console->print("Failed to initialise ADNS3080 ");
} }
flowSensor.set_orientation(AP_OPTICALFLOW_ADNS3080_PINS_FORWARD); flowSensor.set_orientation(AP_OPTICALFLOW_ADNS3080_PINS_FORWARD);
flowSensor.set_field_of_view(AP_OPTICALFLOW_ADNS3080_08_FOV); flowSensor.set_field_of_view(AP_OPTICALFLOW_ADNS3080_08_FOV);
delay(1000); hal.scheduler->delay(1000);
} }
// //
@ -66,18 +38,18 @@ void setup()
// //
void display_menu() void display_menu()
{ {
Serial.println(); hal.console->println();
Serial.println("please choose from the following options:"); hal.console->println("please choose from the following options:");
Serial.println(" c - display all config"); hal.console->println(" c - display all config");
Serial.println(" f - set frame rate"); hal.console->println(" f - set frame rate");
Serial.println(" i - display image"); hal.console->println(" i - display image");
Serial.println(" I - display image continuously"); hal.console->println(" I - display image continuously");
Serial.println(" m - display motion"); hal.console->println(" m - display motion");
Serial.println(" r - set resolution"); hal.console->println(" r - set resolution");
Serial.println(" s - set shutter speed"); hal.console->println(" s - set shutter speed");
Serial.println(" z - clear all motion"); hal.console->println(" z - clear all motion");
Serial.println(" a - frame rate auto/manual"); hal.console->println(" a - frame rate auto/manual");
Serial.println(); hal.console->println();
} }
// //
@ -85,45 +57,45 @@ void display_menu()
// //
void display_config() void display_config()
{ {
Serial.print("Config: "); hal.console->print("Config: ");
Serial.print(flowSensor.read_register(ADNS3080_CONFIGURATION_BITS),BIN); hal.console->print(flowSensor.read_register(ADNS3080_CONFIGURATION_BITS),BIN);
delayMicroseconds(50); hal.scheduler->delay_microseconds(50);
Serial.print(","); hal.console->print(",");
Serial.print(flowSensor.read_register(ADNS3080_EXTENDED_CONFIG),BIN); hal.console->print(flowSensor.read_register(ADNS3080_EXTENDED_CONFIG),BIN);
delayMicroseconds(50); hal.scheduler->delay_microseconds(50);
Serial.println(); hal.console->println();
// product id // product id
Serial.print("\tproduct id: "); hal.console->print("\tproduct id: ");
Serial.print(flowSensor.read_register(ADNS3080_PRODUCT_ID),HEX); hal.console->print(flowSensor.read_register(ADNS3080_PRODUCT_ID),HEX);
delayMicroseconds(50); hal.scheduler->delay_microseconds(50);
Serial.print(" (hex)"); hal.console->print(" (hex)");
Serial.println(); hal.console->println();
// frame rate // frame rate
Serial.print("\tframe rate: "); hal.console->print("\tframe rate: ");
Serial.print(flowSensor.get_frame_rate()); hal.console->print(flowSensor.get_frame_rate());
if( flowSensor.get_frame_rate_auto() == true ) { if( flowSensor.get_frame_rate_auto() == true ) {
Serial.print(" (auto)"); hal.console->print(" (auto)");
}else{ }else{
Serial.print(" (manual)"); hal.console->print(" (manual)");
} }
Serial.println(); hal.console->println();
// resolution // resolution
Serial.print("\tresolution: "); hal.console->print("\tresolution: ");
Serial.print(flowSensor.get_resolution()); hal.console->print(flowSensor.get_resolution());
Serial.println(); hal.console->println();
// shutter speed // shutter speed
Serial.print("\tshutter speed: "); hal.console->print("\tshutter speed: ");
Serial.print(flowSensor.get_shutter_speed()); hal.console->print(flowSensor.get_shutter_speed());
if( flowSensor.get_shutter_speed_auto() ) { if( flowSensor.get_shutter_speed_auto() ) {
Serial.print(" (auto)"); hal.console->print(" (auto)");
}else{ }else{
Serial.print(" (manual)"); hal.console->print(" (manual)");
} }
Serial.println(); hal.console->println();
} }
// //
@ -134,30 +106,30 @@ void set_frame_rate()
int value; int value;
// frame rate // frame rate
Serial.print("frame rate: "); hal.console->print("frame rate: ");
Serial.print(flowSensor.get_frame_rate()); hal.console->print(flowSensor.get_frame_rate());
if( flowSensor.get_frame_rate_auto() == true ) { if( flowSensor.get_frame_rate_auto() == true ) {
Serial.print(" (auto)"); hal.console->print(" (auto)");
}else{ }else{
Serial.print(" (manual)"); hal.console->print(" (manual)");
} }
Serial.println(); hal.console->println();
Serial.println("Choose new frame rate:"); hal.console->println("Choose new frame rate:");
Serial.println("\ta) auto"); hal.console->println("\ta) auto");
Serial.println("\t2) 2000 f/s"); hal.console->println("\t2) 2000 f/s");
Serial.println("\t3) 3000 f/s"); hal.console->println("\t3) 3000 f/s");
Serial.println("\t4) 4000 f/s"); hal.console->println("\t4) 4000 f/s");
Serial.println("\t5) 5000 f/s"); hal.console->println("\t5) 5000 f/s");
Serial.println("\t6) 6400 f/s"); hal.console->println("\t6) 6400 f/s");
Serial.println("\tx) exit (leave unchanged)"); hal.console->println("\tx) exit (leave unchanged)");
// get user input // get user input
Serial.flush(); //hal.console->flush();
while( !Serial.available() ) { while( !hal.console->available() ) {
delay(20); hal.scheduler->delay(20);
} }
value = Serial.read(); value = hal.console->read();
if( value == 'a' || value == 'A') if( value == 'a' || value == 'A')
flowSensor.set_frame_rate_auto(true); flowSensor.set_frame_rate_auto(true);
@ -173,42 +145,42 @@ void set_frame_rate()
flowSensor.set_frame_rate(6469); flowSensor.set_frame_rate(6469);
// display new frame rate // display new frame rate
Serial.print("frame rate: "); hal.console->print("frame rate: ");
Serial.print(flowSensor.get_frame_rate()); hal.console->print(flowSensor.get_frame_rate());
if( flowSensor.get_frame_rate_auto() == true ) { if( flowSensor.get_frame_rate_auto() == true ) {
Serial.print(" (auto)"); hal.console->print(" (auto)");
}else{ }else{
Serial.print(" (manual)"); hal.console->print(" (manual)");
} }
Serial.println(); hal.console->println();
} }
// display_image - captures and displays image from flowSensor flowSensor // display_image - captures and displays image from flowSensor flowSensor
void display_image() void display_image()
{ {
Serial.println("image data --------------"); hal.console->println("image data --------------");
flowSensor.print_pixel_data(); flowSensor.print_pixel_data();
Serial.println("-------------------------"); hal.console->println("-------------------------");
} }
// display_image - captures and displays image from flowSensor flowSensor // display_image - captures and displays image from flowSensor flowSensor
void display_image_continuously() void display_image_continuously()
{ {
int i; int i;
Serial.println("press any key to return to menu"); hal.console->println("press any key to return to menu");
Serial.flush(); //hal.console->flush();
while( !Serial.available() ) { while( !hal.console->available() ) {
display_image(); display_image();
i=0; i=0;
while( i<20 && !Serial.available() ) { while( i<20 && !hal.console->available() ) {
delay(100); // give the viewer a bit of time to catchup hal.scheduler->delay(100); // give the viewer a bit of time to catchup
i++; i++;
} }
} }
Serial.flush(); //hal.console->flush();
} }
// //
@ -218,20 +190,20 @@ void set_resolution()
{ {
int value; int value;
int resolution = flowSensor.get_resolution(); int resolution = flowSensor.get_resolution();
Serial.print("resolution: "); hal.console->print("resolution: ");
Serial.println(resolution); hal.console->println(resolution);
Serial.println("Choose new value:"); hal.console->println("Choose new value:");
Serial.println(" 1) 1600"); hal.console->println(" 1) 1600");
Serial.println(" 4) 400"); hal.console->println(" 4) 400");
Serial.println(" x) exit"); hal.console->println(" x) exit");
Serial.println(); hal.console->println();
// get user input // get user input
Serial.flush(); //hal.console->flush();
while( !Serial.available() ) { while( !hal.console->available() ) {
delay(20); hal.scheduler->delay(20);
} }
value = Serial.read(); value = hal.console->read();
// update resolution // update resolution
if( value == '1' ) { if( value == '1' ) {
@ -241,8 +213,8 @@ void set_resolution()
flowSensor.set_resolution(ADNS3080_RESOLUTION_400); flowSensor.set_resolution(ADNS3080_RESOLUTION_400);
} }
Serial.print("new resolution: "); hal.console->print("new resolution: ");
Serial.println(flowSensor.get_resolution()); hal.console->println(flowSensor.get_resolution());
} }
// //
@ -253,34 +225,34 @@ void set_shutter_speed()
int value; int value;
// shutter speed // shutter speed
Serial.print("shutter speed: "); hal.console->print("shutter speed: ");
Serial.print(flowSensor.get_shutter_speed()); hal.console->print(flowSensor.get_shutter_speed());
if( flowSensor.get_shutter_speed_auto() == true ) { if( flowSensor.get_shutter_speed_auto() == true ) {
Serial.print(" (auto)"); hal.console->print(" (auto)");
}else{ }else{
Serial.print(" (manual)"); hal.console->print(" (manual)");
} }
Serial.println(); hal.console->println();
Serial.println("Choose new shutter speed:"); hal.console->println("Choose new shutter speed:");
Serial.println("\ta) auto"); hal.console->println("\ta) auto");
Serial.println("\t1) 1000 clock cycles"); hal.console->println("\t1) 1000 clock cycles");
Serial.println("\t2) 2000 clock cycles"); hal.console->println("\t2) 2000 clock cycles");
Serial.println("\t3) 3000 clock cycles"); hal.console->println("\t3) 3000 clock cycles");
Serial.println("\t4) 4000 clock cycles"); hal.console->println("\t4) 4000 clock cycles");
Serial.println("\t5) 5000 clock cycles"); hal.console->println("\t5) 5000 clock cycles");
Serial.println("\t6) 6000 clock cycles"); hal.console->println("\t6) 6000 clock cycles");
Serial.println("\t7) 7000 clock cycles"); hal.console->println("\t7) 7000 clock cycles");
Serial.println("\t8) 8000 clock cycles"); hal.console->println("\t8) 8000 clock cycles");
Serial.println("\t9) 9000 clock cycles"); hal.console->println("\t9) 9000 clock cycles");
Serial.println("\tx) exit (leave unchanged)"); hal.console->println("\tx) exit (leave unchanged)");
// get user input // get user input
Serial.flush(); //hal.console->flush();
while( !Serial.available() ) { while( !hal.console->available() ) {
delay(20); hal.scheduler->delay(20);
} }
value = Serial.read(); value = hal.console->read();
if( value == 'a' || value == 'A') if( value == 'a' || value == 'A')
flowSensor.set_shutter_speed_auto(true); flowSensor.set_shutter_speed_auto(true);
@ -304,14 +276,14 @@ void set_shutter_speed()
flowSensor.set_shutter_speed(9000); flowSensor.set_shutter_speed(9000);
// display new shutter speed // display new shutter speed
Serial.print("shutter speed: "); hal.console->print("shutter speed: ");
Serial.print(flowSensor.get_shutter_speed()); hal.console->print(flowSensor.get_shutter_speed());
if( flowSensor.get_shutter_speed_auto() == true ) { if( flowSensor.get_shutter_speed_auto() == true ) {
Serial.print(" (auto)"); hal.console->print(" (auto)");
}else{ }else{
Serial.print(" (manual)"); hal.console->print(" (manual)");
} }
Serial.println(); hal.console->println();
} }
// //
@ -319,41 +291,41 @@ void set_shutter_speed()
// //
void display_motion() void display_motion()
{ {
boolean first_time = true; bool first_time = true;
Serial.flush(); //hal.console->flush();
// display instructions on how to exit // display instructions on how to exit
Serial.println("press x to return to menu.."); hal.console->println("press x to return to menu..");
delay(1000); hal.scheduler->delay(1000);
while( !Serial.available() ) { while( !hal.console->available() ) {
//flowSensor.update(); //flowSensor.update();
flowSensor.update_position(0,0,0,1,100); flowSensor.update_position(0,0,0,1,100);
// check for errors // check for errors
if( flowSensor.overflow() ) if( flowSensor.overflow() )
Serial.println("overflow!!"); hal.console->println("overflow!!");
// x,y,squal // x,y,squal
Serial.print("x/dx: "); hal.console->print("x/dx: ");
Serial.print(flowSensor.x,DEC); hal.console->print(flowSensor.x,DEC);
Serial.print("/"); hal.console->print("/");
Serial.print(flowSensor.dx,DEC); hal.console->print(flowSensor.dx,DEC);
Serial.print("\ty/dy: "); hal.console->print("\ty/dy: ");
Serial.print(flowSensor.y,DEC); hal.console->print(flowSensor.y,DEC);
Serial.print("/"); hal.console->print("/");
Serial.print(flowSensor.dy,DEC); hal.console->print(flowSensor.dy,DEC);
Serial.print("\tsqual:"); hal.console->print("\tsqual:");
Serial.print(flowSensor.surface_quality,DEC); hal.console->print(flowSensor.surface_quality,DEC);
Serial.println(); hal.console->println();
first_time = false; first_time = false;
// short delay // short delay
delay(100); hal.scheduler->delay(100);
} }
// flush the serial // flush the serial
Serial.flush(); //hal.console->flush();
} }
void loop() void loop()
@ -364,12 +336,12 @@ void loop()
display_menu(); display_menu();
// wait for user to enter something // wait for user to enter something
while( !Serial.available() ) { while( !hal.console->available() ) {
delay(20); hal.scheduler->delay(20);
} }
// get character from user // get character from user
value = Serial.read(); value = hal.console->read();
switch( value ) { switch( value ) {
@ -414,8 +386,10 @@ void loop()
break; break;
default: default:
Serial.println("unrecognised command"); hal.console->println("unrecognised command");
Serial.println(); hal.console->println();
break; break;
} }
} }
AP_HAL_MAIN();

0
libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/Arduino.h Normal file
View File

0
libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/nocore.inoflag Normal file
View File