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
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
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
// 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* 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 initCommAPI, AP_PeriodicProcess *scheduler, AP_Semaphore* spi_semaphore, AP_Semaphore* spi3_semaphore)
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)
// set_orientation - Rotation vector to transform sensor readings to the body
// frame.
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(uint32_t now)
// 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(uint32_t now)
{
_num_calls++;
@ -48,27 +50,16 @@ AP_OpticalFlow::read(uint32_t now)
};
// read value from the sensor. Should be overridden by derived class
void
AP_OpticalFlow::update(uint32_t now)
{
}
void AP_OpticalFlow::update(uint32_t now){ }
// reads a value from the sensor (will be sensor specific)
byte
AP_OpticalFlow::read_register(byte address)
{
return 0;
}
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(byte address, byte value)
{
}
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()
void AP_OpticalFlow::apply_orientation_matrix()
{
Vector3f rot_vector;
rot_vector(raw_dx, raw_dy, 0);
@ -79,31 +70,36 @@ AP_OpticalFlow::apply_orientation_matrix()
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)
// 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()
// updates 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)
// 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
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)
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);
// 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 = (altitude > 0 ? 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;
@ -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;
// 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
// perhaps this altitude should actually be the distance to the
// 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
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
#define AP_OPTICALFLOW_H
#ifndef __AP_OPTICALFLOW_H__
#define __AP_OPTICALFLOW_H__
/*
* 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.
*
* 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)
* 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>
#include <AP_PeriodicProcess.h>
#include <AP_Semaphore.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
// timer process runs at 1khz. 100 iterations = 10hz
#define AP_OPTICALFLOW_NUM_CALLS_FOR_10HZ 100
// 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
{
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;
// raw sensor change in x and y position (i.e. unrotated)
int raw_dx, raw_dy;
// image quality (below 15 you really can't trust the x,y values returned)
int surface_quality;
// total x,y position
int x,y;
// rotated change in x and y position
int dx,dy;
// position as offsets from original position
float vlon, vlat;
// millis() time of last update
uint32_t last_update;
// 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() {
_sensor = this;
@ -50,26 +61,46 @@ public:
~AP_OpticalFlow() {
_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 byte read_register(byte address);
virtual void write_register(byte address, byte value);
virtual void set_orientation(enum Rotation rotation); // 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 now); // called by timer process to read sensor data from all attached sensors
virtual void update(uint32_t now); // read latest values from sensor and fill in x,y and totals.
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 bool init();
// 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 uint8_t read_register(uint8_t address);
virtual void write_register(uint8_t address, uint8_t value);
// Rotation vector to transform sensor readings to the body frame.
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:
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
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, _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();
// pointer to the last instantiated optical flow sensor. Will be turned
// into a table if we ever add support for more than one sensor
static 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;
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:
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"

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
*
* 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 "SPI.h"
#include "SPI3.h"
#include "AP_Semaphore.h"
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
extern const AP_HAL::HAL& hal;
#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
{
int16_t intValue;
@ -41,11 +25,8 @@ union NumericIntType
};
// Constructors ////////////////////////////////////////////////////////////////
AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080(int16_t cs_pin, int16_t reset_pin) :
_cs_pin(cs_pin),
_reset_pin(reset_pin),
_spi_bus(ADNS3080_SPI_UNKNOWN),
_spi_semaphore(NULL)
AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080(uint8_t reset_pin) :
_reset_pin(reset_pin)
{
num_pixels = ADNS3080_PIXELS_X;
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 //////////////////////////////////////////////////////////////
// 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
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;
bool retvalue = false;
// suspend timer while we set-up SPI communication
scheduler->suspend_timer();
hal.scheduler->suspend_timer_procs();
pinMode(_cs_pin,OUTPUT);
if( _reset_pin != 0)
pinMode(ADNS3080_RESET,OUTPUT);
digitalWrite(_cs_pin,HIGH); // disable device (Chip select is active low)
hal.gpio->pinMode(_reset_pin, GPIO_OUTPUT);
// reset the device
reset();
// start the SPI library:
if( initCommAPI ) {
pinMode(ADNS3080_SPI_MOSI,OUTPUT);
pinMode(ADNS3080_SPI_MISO,INPUT);
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
_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
_spi_bus = ADNS3080_SPIBUS_1;
_spi_semaphore = spi_semaphore;
while( retvalue == false && retry < 3 ) {
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) {
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 ) {
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) {
retvalue = true;
}
retry++;
}
// If we fail to find on SPI3, no connection available.
retvalue = false;
_spi = NULL;
// if not found, check 3 times on SPI3
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++;
}
}
finish:
// 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 ) {
scheduler->register_process( AP_OpticalFlow_ADNS3080::read );
}else{
_spi_bus = ADNS3080_SPI_UNKNOWN;
hal.scheduler->register_timer_process( AP_OpticalFlow_ADNS3080::read );
}
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
byte
AP_OpticalFlow_ADNS3080::read_register(byte address)
uint8_t AP_OpticalFlow_ADNS3080::read_register(uint8_t address)
{
uint8_t result = 0;
if (_spi == NULL) return 0;
// 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( !_spi_semaphore->get(this) ) {
if( !sem->get(this) ) {
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
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();
_spi->cs_release();
// get spi semaphore if required
if( _spi_semaphore != NULL ) {
_spi_semaphore->release(this);
if( sem != NULL ) {
sem->release(this);
}
return result;
}
// write a value to one of the sensor's registers
void
AP_OpticalFlow_ADNS3080::write_register(byte address, byte value)
void AP_OpticalFlow_ADNS3080::write_register(uint8_t address, uint8_t value)
{
if (_spi == NULL) return;
AP_HAL::Semaphore* sem = _spi->get_semaphore();
// get spi semaphore if required
if( _spi_semaphore != NULL ) {
if( sem != NULL ) {
// if failed to get semaphore then just quietly fail
if( !_spi_semaphore->get(this) ) {
if( !sem->get(this) ) {
return;
}
}
backup_spi_settings();
_spi->cs_assert();
// take the chip select low to select the device
digitalWrite(_cs_pin, LOW);
if( _spi_bus == ADNS3080_SPIBUS_1 ) {
SPI.transfer(address | 0x80 ); // send register address
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();
// send register address
_spi->transfer(address | 0x80 );
hal.scheduler->delay_microseconds(50);
// send data
_spi->transfer(value);
_spi->cs_release();
// get spi3 semaphore if required
if( _spi_semaphore != NULL ) {
_spi_semaphore->release(this);
if( sem != NULL ) {
sem->release(this);
}
}
@ -265,25 +164,28 @@ AP_OpticalFlow_ADNS3080::reset()
if( _reset_pin == 0)
return;
digitalWrite(_reset_pin,HIGH); // reset sensor
delayMicroseconds(10);
digitalWrite(_reset_pin,LOW); // return sensor to normal
// 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(uint32_t now)
{
byte motion_reg;
uint8_t motion_reg;
surface_quality = (uint16_t)read_register(ADNS3080_SQUAL);
delayMicroseconds(50); // small delay
hal.scheduler->delay_microseconds(50);
// 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
// check if we've had an overflow
_overflow = ((motion_reg & 0x10) != 0);
if( (motion_reg & 0x80) != 0 ) {
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));
_motion = true;
}else{
@ -291,7 +193,7 @@ AP_OpticalFlow_ADNS3080::update(uint32_t now)
raw_dy = 0;
}
last_update = millis();
last_update = hal.scheduler->millis();
apply_orientation_matrix();
}
@ -299,32 +201,31 @@ AP_OpticalFlow_ADNS3080::update(uint32_t now)
void
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);
delayMicroseconds(50); // small delay
hal.scheduler->delay_microseconds(50);
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()
// 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 )
// 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);
uint8_t regVal = read_register(ADNS3080_CONFIGURATION_BITS);
regVal = (regVal & 0xbf) | (alwaysOn << 6);
delayMicroseconds(50); // small delay
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()
int16_t AP_OpticalFlow_ADNS3080::get_resolution()
{
if( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x10) == 0 )
return 400;
@ -333,10 +234,9 @@ AP_OpticalFlow_ADNS3080::get_resolution()
}
// set parameter to 400 or 1600 counts per inch
void
AP_OpticalFlow_ADNS3080::set_resolution(uint16_t resolution)
void 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 ) {
regVal &= ~0x10;
@ -346,7 +246,7 @@ AP_OpticalFlow_ADNS3080::set_resolution(uint16_t resolution)
scaler = AP_OPTICALFLOW_ADNS3080_SCALER * 4;
}
delayMicroseconds(50); // small delay
hal.scheduler->delay_microseconds(50);
write_register(ADNS3080_CONFIGURATION_BITS, regVal);
// 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
bool
AP_OpticalFlow_ADNS3080::get_frame_rate_auto()
bool 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 ) {
return false;
}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)
void
AP_OpticalFlow_ADNS3080::set_frame_rate_auto(bool auto_frame_rate)
void AP_OpticalFlow_ADNS3080::set_frame_rate_auto(bool auto_frame_rate)
{
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
delayMicroseconds(50); // small delay
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);
delayMicroseconds(50); // small delay
hal.scheduler->delay_microseconds(50);
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
regVal = (regVal & ~0x01);
@ -388,44 +286,40 @@ AP_OpticalFlow_ADNS3080::set_frame_rate_auto(bool auto_frame_rate)
}
// get frame period
uint16_t
AP_OpticalFlow_ADNS3080::get_frame_period()
uint16_t AP_OpticalFlow_ADNS3080::get_frame_period()
{
NumericIntType aNum;
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);
return aNum.uintValue;
}
// set frame period
void
AP_OpticalFlow_ADNS3080::set_frame_period(uint16_t 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);
delayMicroseconds(50); // small delay
hal.scheduler->delay_microseconds(50);
// set specific frame period
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]);
}
uint16_t
AP_OpticalFlow_ADNS3080::get_frame_rate()
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)
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);
@ -433,9 +327,9 @@ AP_OpticalFlow_ADNS3080::set_frame_rate(uint16_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()
// 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 ) {
@ -446,17 +340,16 @@ AP_OpticalFlow_ADNS3080::get_shutter_speed_auto()
}
// 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)
void AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool auto_shutter_speed)
{
uint8_t regVal = read_register(ADNS3080_EXTENDED_CONFIG);
delayMicroseconds(50); // small delay
hal.scheduler->delay_microseconds(50);
if( auto_shutter_speed ) {
// return shutter speed max to default
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);
delayMicroseconds(50); // small delay
hal.scheduler->delay_microseconds(50);
// determine value to put into extended config
regVal &= ~0x02;
@ -465,16 +358,16 @@ AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool auto_shutter_speed)
regVal |= 0x02;
}
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
uint16_t
AP_OpticalFlow_ADNS3080::get_shutter_speed()
// 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);
delayMicroseconds(50); // small delay
hal.scheduler->delay_microseconds(50);
aNum.byteValue[0] = read_register(ADNS3080_SHUTTER_LOWER);
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_auto(false);
delayMicroseconds(50); // small delay
hal.scheduler->delay_microseconds(50);
// set specific shutter speed
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]);
delayMicroseconds(50); // small delay
hal.scheduler->delay_microseconds(50);
// larger delay
delay(50);
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);
delayMicroseconds(50); // small delay
hal.scheduler->delay_microseconds(50);
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]);
delayMicroseconds(50); // small delay
hal.scheduler->delay_microseconds(50);
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
void
AP_OpticalFlow_ADNS3080::clear_motion()
// 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
// writing anything to this register will clear the sensor's motion
// registers
write_register(ADNS3080_MOTION_CLEAR,0xFF);
x = 0;
y = 0;
dx = 0;
@ -523,9 +418,9 @@ AP_OpticalFlow_ADNS3080::clear_motion()
_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()
// get_pixel_data - captures an image from the sensor and stores it to the
// pixe_data array
void AP_OpticalFlow_ADNS3080::print_pixel_data()
{
int16_t i,j;
bool isFirstPixel = true;
@ -536,23 +431,26 @@ AP_OpticalFlow_ADNS3080::print_pixel_data()
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
// 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
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 ) {
Serial.print_P(PSTR("failed to find first pixel"));
hal.console->println_P(
PSTR("Optflow: failed to find first pixel"));
}
isFirstPixel = false;
pixelValue = ( regValue << 2);
Serial.print(pixelValue,DEC);
pixelValue = ( regValue << 2 );
hal.console->print(pixelValue,DEC);
if( j!= ADNS3080_PIXELS_X-1 )
Serial.print_P(PSTR(","));
delayMicroseconds(50);
hal.console->print_P(PSTR(","));
hal.scheduler->delay_microseconds(50);
}
Serial.println();
hal.console->println();
}
// 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__
#define __AP_OPTICALFLOW_ADNS3080_H__
#include <AP_Semaphore.h>
#include <AP_HAL.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
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD ROTATION_YAW_180
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_RIGHT ROTATION_YAW_135
@ -83,63 +79,85 @@
class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow
{
public:
AP_OpticalFlow_ADNS3080(int16_t cs_pin = ADNS3080_CHIP_SELECT, int16_t reset_pin = ADNS3080_RESET);
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)
uint8_t read_register(uint8_t address);
void write_register(uint8_t address, uint8_t value);
void reset(); // 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
AP_OpticalFlow_ADNS3080(uint8_t reset_pin = 0);
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();
// 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
// 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
void set_led_always_on( bool alwaysOn ); // set parameter to true if you want LED always on, otherwise false for only when required
// 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 );
int16_t get_resolution(); // returns resolution (either 400 or 1600 counts per inch)
void set_resolution(uint16_t resolution); // set parameter to 400 or 1600 counts per inch
// 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);
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)
// 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);
uint16_t get_frame_period(); // get_frame_period
void set_frame_period(uint16_t period);
// 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);
uint16_t get_frame_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
void set_shutter_speed_auto(bool auto_shutter_speed); // set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
// 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);
uint16_t get_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:
// bytes to store SPI settings
uint8_t orig_spi_settings_spcr; // spi1's mode
uint8_t orig_spi3_settings_ucsr3c; // spi3's mode
uint8_t orig_spi3_settings_ubrr3; // spi3's speed
// 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;
// save and restore SPI settings
void backup_spi_settings();
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;
// SPI device
AP_HAL::SPIDeviceDriver *_spi;
};
#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
*/
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Progmem.h>
#include <AP_Param.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <SPI.h> // Arduino SPI library
#include <SPI3.h> // SPI3 library
#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_Math.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.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
#include <AP_OpticalFlow.h>
#define MS5611_CS 40 // barometer chip select pin
#define CONFIG_MPU6000_CHIP_SELECT_PIN 53 // MPU600 chip select pin
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
//AP_OpticalFlow_ADNS3080 flowSensor; // for APM1
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;
AP_OpticalFlow_ADNS3080 flowSensor;
void setup()
{
Serial.begin(115200);
Serial.println("ArduPilot Mega OpticalFlow library test ver 1.5");
hal.console->println("ArduPilot Mega OpticalFlow library test ver 1.5");
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);
hal.scheduler->delay(1000);
// flowSensor initialization
if( flowSensor.init(true, &scheduler) == false ) {
Serial.print("Failed to initialise ADNS3080 ");
if( flowSensor.init() == false ) {
hal.console->print("Failed to initialise ADNS3080 ");
}
flowSensor.set_orientation(AP_OPTICALFLOW_ADNS3080_PINS_FORWARD);
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()
{
Serial.println();
Serial.println("please choose from the following options:");
Serial.println(" c - display all config");
Serial.println(" f - set frame rate");
Serial.println(" i - display image");
Serial.println(" I - display image continuously");
Serial.println(" m - display motion");
Serial.println(" r - set resolution");
Serial.println(" s - set shutter speed");
Serial.println(" z - clear all motion");
Serial.println(" a - frame rate auto/manual");
Serial.println();
hal.console->println();
hal.console->println("please choose from the following options:");
hal.console->println(" c - display all config");
hal.console->println(" f - set frame rate");
hal.console->println(" i - display image");
hal.console->println(" I - display image continuously");
hal.console->println(" m - display motion");
hal.console->println(" r - set resolution");
hal.console->println(" s - set shutter speed");
hal.console->println(" z - clear all motion");
hal.console->println(" a - frame rate auto/manual");
hal.console->println();
}
//
@ -85,45 +57,45 @@ void display_menu()
//
void display_config()
{
Serial.print("Config: ");
Serial.print(flowSensor.read_register(ADNS3080_CONFIGURATION_BITS),BIN);
delayMicroseconds(50);
Serial.print(",");
Serial.print(flowSensor.read_register(ADNS3080_EXTENDED_CONFIG),BIN);
delayMicroseconds(50);
Serial.println();
hal.console->print("Config: ");
hal.console->print(flowSensor.read_register(ADNS3080_CONFIGURATION_BITS),BIN);
hal.scheduler->delay_microseconds(50);
hal.console->print(",");
hal.console->print(flowSensor.read_register(ADNS3080_EXTENDED_CONFIG),BIN);
hal.scheduler->delay_microseconds(50);
hal.console->println();
// product id
Serial.print("\tproduct id: ");
Serial.print(flowSensor.read_register(ADNS3080_PRODUCT_ID),HEX);
delayMicroseconds(50);
Serial.print(" (hex)");
Serial.println();
hal.console->print("\tproduct id: ");
hal.console->print(flowSensor.read_register(ADNS3080_PRODUCT_ID),HEX);
hal.scheduler->delay_microseconds(50);
hal.console->print(" (hex)");
hal.console->println();
// frame rate
Serial.print("\tframe rate: ");
Serial.print(flowSensor.get_frame_rate());
hal.console->print("\tframe rate: ");
hal.console->print(flowSensor.get_frame_rate());
if( flowSensor.get_frame_rate_auto() == true ) {
Serial.print(" (auto)");
hal.console->print(" (auto)");
}else{
Serial.print(" (manual)");
hal.console->print(" (manual)");
}
Serial.println();
hal.console->println();
// resolution
Serial.print("\tresolution: ");
Serial.print(flowSensor.get_resolution());
Serial.println();
hal.console->print("\tresolution: ");
hal.console->print(flowSensor.get_resolution());
hal.console->println();
// shutter speed
Serial.print("\tshutter speed: ");
Serial.print(flowSensor.get_shutter_speed());
hal.console->print("\tshutter speed: ");
hal.console->print(flowSensor.get_shutter_speed());
if( flowSensor.get_shutter_speed_auto() ) {
Serial.print(" (auto)");
hal.console->print(" (auto)");
}else{
Serial.print(" (manual)");
hal.console->print(" (manual)");
}
Serial.println();
hal.console->println();
}
//
@ -134,30 +106,30 @@ void set_frame_rate()
int value;
// frame rate
Serial.print("frame rate: ");
Serial.print(flowSensor.get_frame_rate());
hal.console->print("frame rate: ");
hal.console->print(flowSensor.get_frame_rate());
if( flowSensor.get_frame_rate_auto() == true ) {
Serial.print(" (auto)");
hal.console->print(" (auto)");
}else{
Serial.print(" (manual)");
hal.console->print(" (manual)");
}
Serial.println();
hal.console->println();
Serial.println("Choose new frame rate:");
Serial.println("\ta) auto");
Serial.println("\t2) 2000 f/s");
Serial.println("\t3) 3000 f/s");
Serial.println("\t4) 4000 f/s");
Serial.println("\t5) 5000 f/s");
Serial.println("\t6) 6400 f/s");
Serial.println("\tx) exit (leave unchanged)");
hal.console->println("Choose new frame rate:");
hal.console->println("\ta) auto");
hal.console->println("\t2) 2000 f/s");
hal.console->println("\t3) 3000 f/s");
hal.console->println("\t4) 4000 f/s");
hal.console->println("\t5) 5000 f/s");
hal.console->println("\t6) 6400 f/s");
hal.console->println("\tx) exit (leave unchanged)");
// get user input
Serial.flush();
while( !Serial.available() ) {
delay(20);
//hal.console->flush();
while( !hal.console->available() ) {
hal.scheduler->delay(20);
}
value = Serial.read();
value = hal.console->read();
if( value == 'a' || value == 'A')
flowSensor.set_frame_rate_auto(true);
@ -173,42 +145,42 @@ void set_frame_rate()
flowSensor.set_frame_rate(6469);
// display new frame rate
Serial.print("frame rate: ");
Serial.print(flowSensor.get_frame_rate());
hal.console->print("frame rate: ");
hal.console->print(flowSensor.get_frame_rate());
if( flowSensor.get_frame_rate_auto() == true ) {
Serial.print(" (auto)");
hal.console->print(" (auto)");
}else{
Serial.print(" (manual)");
hal.console->print(" (manual)");
}
Serial.println();
hal.console->println();
}
// display_image - captures and displays image from flowSensor flowSensor
void display_image()
{
Serial.println("image data --------------");
hal.console->println("image data --------------");
flowSensor.print_pixel_data();
Serial.println("-------------------------");
hal.console->println("-------------------------");
}
// display_image - captures and displays image from flowSensor flowSensor
void display_image_continuously()
{
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();
i=0;
while( i<20 && !Serial.available() ) {
delay(100); // give the viewer a bit of time to catchup
while( i<20 && !hal.console->available() ) {
hal.scheduler->delay(100); // give the viewer a bit of time to catchup
i++;
}
}
Serial.flush();
//hal.console->flush();
}
//
@ -218,20 +190,20 @@ void set_resolution()
{
int value;
int resolution = flowSensor.get_resolution();
Serial.print("resolution: ");
Serial.println(resolution);
Serial.println("Choose new value:");
Serial.println(" 1) 1600");
Serial.println(" 4) 400");
Serial.println(" x) exit");
Serial.println();
hal.console->print("resolution: ");
hal.console->println(resolution);
hal.console->println("Choose new value:");
hal.console->println(" 1) 1600");
hal.console->println(" 4) 400");
hal.console->println(" x) exit");
hal.console->println();
// get user input
Serial.flush();
while( !Serial.available() ) {
delay(20);
//hal.console->flush();
while( !hal.console->available() ) {
hal.scheduler->delay(20);
}
value = Serial.read();
value = hal.console->read();
// update resolution
if( value == '1' ) {
@ -241,8 +213,8 @@ void set_resolution()
flowSensor.set_resolution(ADNS3080_RESOLUTION_400);
}
Serial.print("new resolution: ");
Serial.println(flowSensor.get_resolution());
hal.console->print("new resolution: ");
hal.console->println(flowSensor.get_resolution());
}
//
@ -253,34 +225,34 @@ void set_shutter_speed()
int value;
// shutter speed
Serial.print("shutter speed: ");
Serial.print(flowSensor.get_shutter_speed());
hal.console->print("shutter speed: ");
hal.console->print(flowSensor.get_shutter_speed());
if( flowSensor.get_shutter_speed_auto() == true ) {
Serial.print(" (auto)");
hal.console->print(" (auto)");
}else{
Serial.print(" (manual)");
hal.console->print(" (manual)");
}
Serial.println();
hal.console->println();
Serial.println("Choose new shutter speed:");
Serial.println("\ta) auto");
Serial.println("\t1) 1000 clock cycles");
Serial.println("\t2) 2000 clock cycles");
Serial.println("\t3) 3000 clock cycles");
Serial.println("\t4) 4000 clock cycles");
Serial.println("\t5) 5000 clock cycles");
Serial.println("\t6) 6000 clock cycles");
Serial.println("\t7) 7000 clock cycles");
Serial.println("\t8) 8000 clock cycles");
Serial.println("\t9) 9000 clock cycles");
Serial.println("\tx) exit (leave unchanged)");
hal.console->println("Choose new shutter speed:");
hal.console->println("\ta) auto");
hal.console->println("\t1) 1000 clock cycles");
hal.console->println("\t2) 2000 clock cycles");
hal.console->println("\t3) 3000 clock cycles");
hal.console->println("\t4) 4000 clock cycles");
hal.console->println("\t5) 5000 clock cycles");
hal.console->println("\t6) 6000 clock cycles");
hal.console->println("\t7) 7000 clock cycles");
hal.console->println("\t8) 8000 clock cycles");
hal.console->println("\t9) 9000 clock cycles");
hal.console->println("\tx) exit (leave unchanged)");
// get user input
Serial.flush();
while( !Serial.available() ) {
delay(20);
//hal.console->flush();
while( !hal.console->available() ) {
hal.scheduler->delay(20);
}
value = Serial.read();
value = hal.console->read();
if( value == 'a' || value == 'A')
flowSensor.set_shutter_speed_auto(true);
@ -304,14 +276,14 @@ void set_shutter_speed()
flowSensor.set_shutter_speed(9000);
// display new shutter speed
Serial.print("shutter speed: ");
Serial.print(flowSensor.get_shutter_speed());
hal.console->print("shutter speed: ");
hal.console->print(flowSensor.get_shutter_speed());
if( flowSensor.get_shutter_speed_auto() == true ) {
Serial.print(" (auto)");
hal.console->print(" (auto)");
}else{
Serial.print(" (manual)");
hal.console->print(" (manual)");
}
Serial.println();
hal.console->println();
}
//
@ -319,41 +291,41 @@ void set_shutter_speed()
//
void display_motion()
{
boolean first_time = true;
Serial.flush();
bool first_time = true;
//hal.console->flush();
// display instructions on how to exit
Serial.println("press x to return to menu..");
delay(1000);
hal.console->println("press x to return to menu..");
hal.scheduler->delay(1000);
while( !Serial.available() ) {
while( !hal.console->available() ) {
//flowSensor.update();
flowSensor.update_position(0,0,0,1,100);
// check for errors
if( flowSensor.overflow() )
Serial.println("overflow!!");
hal.console->println("overflow!!");
// x,y,squal
Serial.print("x/dx: ");
Serial.print(flowSensor.x,DEC);
Serial.print("/");
Serial.print(flowSensor.dx,DEC);
Serial.print("\ty/dy: ");
Serial.print(flowSensor.y,DEC);
Serial.print("/");
Serial.print(flowSensor.dy,DEC);
Serial.print("\tsqual:");
Serial.print(flowSensor.surface_quality,DEC);
Serial.println();
hal.console->print("x/dx: ");
hal.console->print(flowSensor.x,DEC);
hal.console->print("/");
hal.console->print(flowSensor.dx,DEC);
hal.console->print("\ty/dy: ");
hal.console->print(flowSensor.y,DEC);
hal.console->print("/");
hal.console->print(flowSensor.dy,DEC);
hal.console->print("\tsqual:");
hal.console->print(flowSensor.surface_quality,DEC);
hal.console->println();
first_time = false;
// short delay
delay(100);
hal.scheduler->delay(100);
}
// flush the serial
Serial.flush();
//hal.console->flush();
}
void loop()
@ -364,12 +336,12 @@ void loop()
display_menu();
// wait for user to enter something
while( !Serial.available() ) {
delay(20);
while( !hal.console->available() ) {
hal.scheduler->delay(20);
}
// get character from user
value = Serial.read();
value = hal.console->read();
switch( value ) {
@ -414,8 +386,10 @@ void loop()
break;
default:
Serial.println("unrecognised command");
Serial.println();
hal.console->println("unrecognised command");
hal.console->println();
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