mirror of https://github.com/ArduPilot/ardupilot
AP_OpticalFlow - first draft of optical flow library for use with experimental ADNS3080 sensor
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1933 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
rmackay9@yahoo.com
parent
3ef57bf3bf
commit
8d1f9c9fd9
|
|
@ -0,0 +1,35 @@
|
|||
/*
|
||||
ADC.cpp - Analog Digital Converter Base Class for Ardupilot Mega
|
||||
Code by James Goppert. DIYDrones.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
*/
|
||||
|
||||
#include "AP_OpticalFlow.h"
|
||||
|
||||
AP_OpticalFlow::AP_OpticalFlow() : x(0),y(0),surface_quality(0),dx(0),dy(0)
|
||||
{
|
||||
}
|
||||
|
||||
// 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)
|
||||
void AP_OpticalFlow::init(boolean initCommAPI)
|
||||
{
|
||||
}
|
||||
|
||||
// read latest values from sensor and fill in x,y and totals
|
||||
int AP_OpticalFlow::read()
|
||||
{
|
||||
}
|
||||
|
||||
// reads a value from the sensor (will be sensor specific)
|
||||
byte AP_OpticalFlow::read_register(byte address)
|
||||
{
|
||||
}
|
||||
|
||||
// writes a value to one of the sensor's register (will be sensor specific)
|
||||
void AP_OpticalFlow::write_register(byte address, byte value)
|
||||
{
|
||||
}
|
||||
|
|
@ -0,0 +1,48 @@
|
|||
#ifndef AP_OPTICALFLOW_H
|
||||
#define AP_OPTICALFLOW_H
|
||||
|
||||
/*
|
||||
AP_OpticalFlow.cpp - OpticalFlow Base Class for Ardupilot Mega
|
||||
Code by Randy Mackay. DIYDrones.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
Methods:
|
||||
init() : initializate sensor and library.
|
||||
read : reads latest value from OpticalFlow and stores values in x,y, surface_quality parameter
|
||||
read_register() : reads a value from the sensor (will be sensor specific)
|
||||
write_register() : writes a value to one of the sensor's register (will be sensor specific)
|
||||
|
||||
*/
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <inttypes.h>
|
||||
#include <math.h>
|
||||
#include "WConstants.h"
|
||||
|
||||
// return value definition
|
||||
#define OPTICALFLOW_FAIL 0
|
||||
#define OPTICALFLOW_SUCCESS 1
|
||||
|
||||
class AP_OpticalFlow
|
||||
{
|
||||
public:
|
||||
int x, y; // total x,y position
|
||||
int dx, dy; // change in x and y position
|
||||
int surface_quality; // image quality (below 15 you really can't trust the x,y values returned)
|
||||
public:
|
||||
AP_OpticalFlow(); // Constructor
|
||||
virtual void init(boolean initCommAPI = true); // parameter controls whether I2C/SPI interface is initialised (set to false if other devices are on the I2C/SPI bus and have already initialised the interface)
|
||||
virtual int read(); // read latest values from sensor and fill in x,y and totals
|
||||
virtual byte read_register(byte address);
|
||||
virtual void write_register(byte address, byte value);
|
||||
|
||||
private:
|
||||
};
|
||||
|
||||
#include "AP_OpticalFlow_ADNS3080.h"
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,420 @@
|
|||
/*
|
||||
AP_OpticalFlow_ADNS3080.cpp - ADNS3080 OpticalFlow Library for Ardupilot Mega
|
||||
Code by Randy Mackay. DIYDrones.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
External ADNS3080 OpticalFlow is connected via Serial port 2 (in SPI mode)
|
||||
TXD2 = MOSI = pin PH1
|
||||
RXD2 = MISO = pin PH0
|
||||
XCK2 = SCK = pin PH2
|
||||
Chip Select pin is PC4 (33) [PH6 (9)]
|
||||
We are using the 16 clocks per conversion timming to increase efficiency (fast)
|
||||
The sampling frequency is 400Hz (Timer2 overflow interrupt)
|
||||
So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so
|
||||
we have an 4x oversampling and averaging.
|
||||
|
||||
Methods:
|
||||
Init() : Initialization of interrupts an Timers (Timer2 overflow interrupt)
|
||||
Read() : Read latest values from OpticalFlow and store to x,y, surface_quality parameters
|
||||
|
||||
*/
|
||||
|
||||
#include "AP_OpticalFlow_ADNS3080.h"
|
||||
#include <avr/interrupt.h>
|
||||
#include "../SPI/SPI.h"
|
||||
|
||||
#define AP_SPI_TIMEOUT 1000
|
||||
|
||||
union NumericIntType
|
||||
{
|
||||
int intValue;
|
||||
unsigned int uintValue;
|
||||
byte byteValue[2];
|
||||
};
|
||||
|
||||
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080()
|
||||
{
|
||||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
// init - initialise sensor
|
||||
// initCommAPI parameter controls whether SPI interface is initialised (set to false if other devices are on the SPI bus and have already initialised the interface)
|
||||
void AP_OpticalFlow_ADNS3080::init(boolean initCommAPI)
|
||||
{
|
||||
pinMode(AP_SPI_DATAOUT,OUTPUT);
|
||||
pinMode(AP_SPI_DATAIN,INPUT);
|
||||
pinMode(AP_SPI_CLOCK,OUTPUT);
|
||||
pinMode(ADNS3080_CHIP_SELECT,OUTPUT);
|
||||
pinMode(ADNS3080_RESET,OUTPUT);
|
||||
|
||||
digitalWrite(ADNS3080_CHIP_SELECT,HIGH); // disable device (Chip select is active low)
|
||||
|
||||
// reset the device
|
||||
reset();
|
||||
|
||||
// start the SPI library:
|
||||
if( initCommAPI ) {
|
||||
SPI.begin();
|
||||
//SPI.setBitOrder(MSBFIRST);
|
||||
//SPI.setDataMode(SPI_MODE3);
|
||||
//SPI.setClockDivider(SPI_CLOCK_DIV8); // sensor running at 2Mhz. this is it's maximum speed
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// backup_spi_settings - checks current SPI settings (clock speed, etc), sets values to what we need
|
||||
//
|
||||
byte AP_OpticalFlow_ADNS3080::backup_spi_settings()
|
||||
{
|
||||
// store current spi values
|
||||
orig_spi_settings_spcr = SPCR & (DORD | CPOL | CPHA);
|
||||
orig_spi_settings_spsr = SPSR & SPI2X;
|
||||
|
||||
// set the values that we need
|
||||
SPI.setBitOrder(MSBFIRST);
|
||||
SPI.setDataMode(SPI_MODE3);
|
||||
SPI.setClockDivider(SPI_CLOCK_DIV8); // sensor running at 2Mhz. this is it's maximum speed
|
||||
|
||||
return orig_spi_settings_spcr;
|
||||
}
|
||||
|
||||
// restore_spi_settings - restores SPI settings (clock speed, etc) to what their values were before the sensor used the bus
|
||||
byte AP_OpticalFlow_ADNS3080::restore_spi_settings()
|
||||
{
|
||||
byte temp;
|
||||
|
||||
// restore SPSR
|
||||
temp = SPSR;
|
||||
temp &= ~SPI2X;
|
||||
temp |= orig_spi_settings_spsr;
|
||||
SPSR = temp;
|
||||
|
||||
// restore SPCR
|
||||
temp = SPCR;
|
||||
temp &= ~(DORD | CPOL | CPHA); // zero out the important bits
|
||||
temp |= orig_spi_settings_spcr; // restore important bits
|
||||
SPCR = temp;
|
||||
|
||||
return temp;
|
||||
}
|
||||
|
||||
// Read a register from the sensor
|
||||
byte AP_OpticalFlow_ADNS3080::read_register(byte address)
|
||||
{
|
||||
byte result = 0, junk = 0;
|
||||
|
||||
backup_spi_settings();
|
||||
|
||||
// take the chip select low to select the device
|
||||
digitalWrite(ADNS3080_CHIP_SELECT, LOW);
|
||||
|
||||
// send the device the register you want to read:
|
||||
junk = SPI.transfer(address);
|
||||
|
||||
// small delay
|
||||
delayMicroseconds(50);
|
||||
|
||||
// send a value of 0 to read the first byte returned:
|
||||
result = SPI.transfer(0x00);
|
||||
|
||||
// take the chip select high to de-select:
|
||||
digitalWrite(ADNS3080_CHIP_SELECT, HIGH);
|
||||
|
||||
restore_spi_settings();
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// write a value to one of the sensor's registers
|
||||
void AP_OpticalFlow_ADNS3080::write_register(byte address, byte value)
|
||||
{
|
||||
byte junk = 0;
|
||||
|
||||
backup_spi_settings();
|
||||
|
||||
// take the chip select low to select the device
|
||||
digitalWrite(ADNS3080_CHIP_SELECT, LOW);
|
||||
|
||||
// send register address
|
||||
junk = SPI.transfer(address | 0x80 );
|
||||
|
||||
// small delay
|
||||
delayMicroseconds(50);
|
||||
|
||||
// send data
|
||||
junk = SPI.transfer(value);
|
||||
|
||||
// take the chip select high to de-select:
|
||||
digitalWrite(ADNS3080_CHIP_SELECT, HIGH);
|
||||
|
||||
restore_spi_settings();
|
||||
}
|
||||
|
||||
// reset sensor by holding a pin high (or is it low?) for 10us.
|
||||
void AP_OpticalFlow_ADNS3080::reset()
|
||||
{
|
||||
digitalWrite(ADNS3080_RESET,HIGH); // reset sensor
|
||||
delayMicroseconds(10);
|
||||
digitalWrite(ADNS3080_RESET,LOW); // return sensor to normal
|
||||
}
|
||||
|
||||
// read latest values from sensor and fill in x,y and totals
|
||||
int AP_OpticalFlow_ADNS3080::read()
|
||||
{
|
||||
surface_quality = (unsigned int)read_register(ADNS3080_SQUAL);
|
||||
delayMicroseconds(50); // small delay
|
||||
|
||||
// check for movement, update x,y values
|
||||
if( (read_register(ADNS3080_MOTION) & 0x80) != 0 ) {
|
||||
dx = ((char)read_register(ADNS3080_DELTA_X));
|
||||
delayMicroseconds(50); // small delay
|
||||
dy = ((char)read_register(ADNS3080_DELTA_Y));
|
||||
x+=dx;
|
||||
y+=dy;
|
||||
_motion = true;
|
||||
}else{
|
||||
dx = 0;
|
||||
dy = 0;
|
||||
}
|
||||
|
||||
return OPTICALFLOW_SUCCESS;
|
||||
}
|
||||
|
||||
// get_led_always_on - returns true if LED is always on, false if only on when required
|
||||
boolean AP_OpticalFlow_ADNS3080::get_led_always_on()
|
||||
{
|
||||
return ( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x40) > 0 );
|
||||
}
|
||||
|
||||
// set_led_always_on - set parameter to true if you want LED always on, otherwise false for only when required
|
||||
void AP_OpticalFlow_ADNS3080::set_led_always_on( boolean alwaysOn )
|
||||
{
|
||||
byte regVal = read_register(ADNS3080_CONFIGURATION_BITS);
|
||||
regVal = regVal & 0xBf | (alwaysOn << 6);
|
||||
delayMicroseconds(50); // small delay
|
||||
write_register(ADNS3080_CONFIGURATION_BITS, regVal);
|
||||
}
|
||||
|
||||
// returns resolution (either 400 or 1200 counts per inch)
|
||||
int AP_OpticalFlow_ADNS3080::get_resolution()
|
||||
{
|
||||
if( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x10) == 0 )
|
||||
return 400;
|
||||
else
|
||||
return 1200;
|
||||
}
|
||||
|
||||
// set parameter to 400 or 1200 counts per inch
|
||||
void AP_OpticalFlow_ADNS3080::set_resolution(int resolution)
|
||||
{
|
||||
byte regVal = read_register(ADNS3080_CONFIGURATION_BITS);
|
||||
|
||||
if( resolution == ADNS3080_RESOLUTION_400 ) {
|
||||
regVal &= ~0x10;
|
||||
}else if( resolution == ADNS3080_RESOLUTION_1200) {
|
||||
regVal |= 0x10;
|
||||
}
|
||||
|
||||
delayMicroseconds(50); // small delay
|
||||
write_register(ADNS3080_CONFIGURATION_BITS, regVal);
|
||||
}
|
||||
|
||||
// get_frame_rate_auto - return whether frame rate is set to "auto" or manual
|
||||
boolean AP_OpticalFlow_ADNS3080::get_frame_rate_auto()
|
||||
{
|
||||
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
|
||||
if( regVal & 0x01 > 0 ) {
|
||||
return false;
|
||||
}else{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
// set_frame_rate_auto - set frame rate to auto (true) or manual (false)
|
||||
void AP_OpticalFlow_ADNS3080::set_frame_rate_auto(boolean auto_frame_rate)
|
||||
{
|
||||
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
|
||||
delayMicroseconds(50); // small delay
|
||||
if( auto_frame_rate == true ) {
|
||||
// set specific frame period
|
||||
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,0xE0);
|
||||
delayMicroseconds(50); // small delay
|
||||
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,0x1A);
|
||||
delayMicroseconds(50); // small delay
|
||||
|
||||
// decide what value to update in extended config
|
||||
regVal = (regVal & ~0x01);
|
||||
}else{
|
||||
// decide what value to update in extended config
|
||||
regVal = (regVal & ~0x01) | 0x01;
|
||||
}
|
||||
write_register(ADNS3080_EXTENDED_CONFIG, regVal);
|
||||
}
|
||||
|
||||
// get frame period
|
||||
unsigned int AP_OpticalFlow_ADNS3080::get_frame_period()
|
||||
{
|
||||
NumericIntType aNum;
|
||||
aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
|
||||
delayMicroseconds(50); // small delay
|
||||
aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER);
|
||||
return aNum.uintValue;
|
||||
}
|
||||
|
||||
// set frame period
|
||||
void AP_OpticalFlow_ADNS3080::set_frame_period(unsigned int period)
|
||||
{
|
||||
NumericIntType aNum;
|
||||
aNum.uintValue = period;
|
||||
|
||||
// set frame rate to manual
|
||||
set_frame_rate_auto(false);
|
||||
delayMicroseconds(50); // small delay
|
||||
|
||||
// set specific frame period
|
||||
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]);
|
||||
delayMicroseconds(50); // small delay
|
||||
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]);
|
||||
|
||||
}
|
||||
|
||||
unsigned int AP_OpticalFlow_ADNS3080::get_frame_rate()
|
||||
{
|
||||
unsigned long clockSpeed = ADNS3080_CLOCK_SPEED;
|
||||
unsigned int rate = clockSpeed / get_frame_period();
|
||||
return rate;
|
||||
}
|
||||
|
||||
void AP_OpticalFlow_ADNS3080::set_frame_rate(unsigned int rate)
|
||||
{
|
||||
unsigned long clockSpeed = ADNS3080_CLOCK_SPEED;
|
||||
unsigned int period = (unsigned int)(clockSpeed / (unsigned long)rate);
|
||||
|
||||
set_frame_period(period);
|
||||
}
|
||||
|
||||
// get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual
|
||||
boolean AP_OpticalFlow_ADNS3080::get_shutter_speed_auto()
|
||||
{
|
||||
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
|
||||
if( (regVal & 0x02) > 0 ) {
|
||||
return false;
|
||||
}else{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
// set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
|
||||
void AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(boolean auto_shutter_speed)
|
||||
{
|
||||
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
|
||||
delayMicroseconds(50); // small delay
|
||||
if( auto_shutter_speed ) {
|
||||
// return shutter speed max to default
|
||||
write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,0x8c);
|
||||
delayMicroseconds(50); // small delay
|
||||
write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,0x20);
|
||||
delayMicroseconds(50); // small delay
|
||||
|
||||
// determine value to put into extended config
|
||||
regVal = regVal & ~0x02;
|
||||
}else{
|
||||
// determine value to put into extended config
|
||||
regVal = regVal & ~0x02 | 0x02;
|
||||
}
|
||||
write_register(ADNS3080_EXTENDED_CONFIG, regVal);
|
||||
delayMicroseconds(50); // small delay
|
||||
}
|
||||
|
||||
// get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual
|
||||
unsigned int AP_OpticalFlow_ADNS3080::get_shutter_speed()
|
||||
{
|
||||
NumericIntType aNum;
|
||||
aNum.byteValue[1] = read_register(ADNS3080_SHUTTER_UPPER);
|
||||
delayMicroseconds(50); // small delay
|
||||
aNum.byteValue[0] = read_register(ADNS3080_SHUTTER_LOWER);
|
||||
return aNum.uintValue;
|
||||
}
|
||||
|
||||
|
||||
// set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
|
||||
unsigned int AP_OpticalFlow_ADNS3080::set_shutter_speed(unsigned int shutter_speed)
|
||||
{
|
||||
NumericIntType aNum;
|
||||
aNum.uintValue = shutter_speed;
|
||||
|
||||
// set shutter speed to manual
|
||||
set_shutter_speed_auto(false);
|
||||
delayMicroseconds(50); // small delay
|
||||
|
||||
// set specific shutter speed
|
||||
write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]);
|
||||
delayMicroseconds(50); // small delay
|
||||
write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,aNum.byteValue[1]);
|
||||
delayMicroseconds(50); // small delay
|
||||
|
||||
// larger delay
|
||||
delay(50);
|
||||
|
||||
// need to update frame period to cause shutter value to take effect
|
||||
aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
|
||||
delayMicroseconds(50); // small delay
|
||||
aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER);
|
||||
delayMicroseconds(50); // small delay
|
||||
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]);
|
||||
delayMicroseconds(50); // small delay
|
||||
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]);
|
||||
delayMicroseconds(50); // small delay
|
||||
}
|
||||
|
||||
// clear_motion - will cause the Delta_X, Delta_Y, and internal motion registers to be cleared
|
||||
void AP_OpticalFlow_ADNS3080::clear_motion()
|
||||
{
|
||||
write_register(ADNS3080_MOTION_CLEAR,0xFF); // writing anything to this register will clear the sensor's motion registers
|
||||
x = 0;
|
||||
y = 0;
|
||||
dx = 0;
|
||||
dy = 0;
|
||||
_motion = false;
|
||||
}
|
||||
|
||||
// get_pixel_data - captures an image from the sensor and stores it to the pixe_data array
|
||||
int AP_OpticalFlow_ADNS3080::print_pixel_data(HardwareSerial *serPort)
|
||||
{
|
||||
int i,j;
|
||||
boolean isFirstPixel = true;
|
||||
byte regValue;
|
||||
byte pixelValue;
|
||||
|
||||
// write to frame capture register to force capture of frame
|
||||
write_register(ADNS3080_FRAME_CAPTURE,0x83);
|
||||
|
||||
// wait 3 frame periods + 10 nanoseconds for frame to be captured
|
||||
delayMicroseconds(1510); // min frame speed is 2000 frames/second so 1 frame = 500 nano seconds. so 500 x 3 + 10 = 1510
|
||||
|
||||
// display the pixel data
|
||||
for( i=0; i<ADNS3080_PIXELS_Y; i++ ) {
|
||||
for( j=0; j<ADNS3080_PIXELS_X; j++ ) {
|
||||
regValue = read_register(ADNS3080_FRAME_CAPTURE);
|
||||
if( isFirstPixel && (regValue & 0x40 == 0) ) {
|
||||
serPort->println("failed to find first pixel");
|
||||
}
|
||||
isFirstPixel = false;
|
||||
pixelValue = ( regValue << 2);
|
||||
serPort->print(pixelValue,DEC);
|
||||
if( j!= ADNS3080_PIXELS_X-1 )
|
||||
serPort->print(",");
|
||||
delayMicroseconds(50);
|
||||
}
|
||||
serPort->println();
|
||||
}
|
||||
|
||||
// hardware reset to restore sensor to normal operation
|
||||
reset();
|
||||
}
|
||||
|
|
@ -0,0 +1,113 @@
|
|||
#ifndef AP_OPTICALFLOW_ADNS3080_H
|
||||
#define AP_OPTICALFLOW_ADNS3080_H
|
||||
|
||||
#include "AP_OpticalFlow.h"
|
||||
#include "HardwareSerial.h"
|
||||
|
||||
// We use Serial Port 2 in SPI Mode
|
||||
#define AP_SPI_DATAIN 50 // MISO // PB3
|
||||
#define AP_SPI_DATAOUT 51 // MOSI // PB2
|
||||
#define AP_SPI_CLOCK 52 // SCK // PB1
|
||||
#define ADNS3080_CHIP_SELECT 32 // PC5
|
||||
#define ADNS3080_RESET 33 // PC6
|
||||
|
||||
// ADNS3080 hardware config
|
||||
#define ADNS3080_PIXELS_X 30
|
||||
#define ADNS3080_PIXELS_Y 30
|
||||
#define ADNS3080_CLOCK_SPEED 24000000
|
||||
|
||||
// Register Map for the ADNS3080 Optical OpticalFlow Sensor
|
||||
#define ADNS3080_PRODUCT_ID 0x00
|
||||
#define ADNS3080_REVISION_ID 0x01
|
||||
#define ADNS3080_MOTION 0x02
|
||||
#define ADNS3080_DELTA_X 0x03
|
||||
#define ADNS3080_DELTA_Y 0x04
|
||||
#define ADNS3080_SQUAL 0x05
|
||||
#define ADNS3080_PIXEL_SUM 0x06
|
||||
#define ADNS3080_MAXIMUM_PIXEL 0x07
|
||||
#define ADNS3080_CONFIGURATION_BITS 0x0a
|
||||
#define ADNS3080_EXTENDED_CONFIG 0x0b
|
||||
#define ADNS3080_DATA_OUT_LOWER 0x0c
|
||||
#define ADNS3080_DATA_OUT_UPPER 0x0d
|
||||
#define ADNS3080_SHUTTER_LOWER 0x0e
|
||||
#define ADNS3080_SHUTTER_UPPER 0x0f
|
||||
#define ADNS3080_FRAME_PERIOD_LOWER 0x10
|
||||
#define ADNS3080_FRAME_PERIOD_UPPER 0x11
|
||||
#define ADNS3080_MOTION_CLEAR 0x12
|
||||
#define ADNS3080_FRAME_CAPTURE 0x13
|
||||
#define ADNS3080_SROM_ENABLE 0x14
|
||||
#define ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER 0x19
|
||||
#define ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER 0x1a
|
||||
#define ADNS3080_FRAME_PERIOD_MIN_BOUND_LOWER 0x1b
|
||||
#define ADNS3080_FRAME_PERIOD_MIN_BOUND_UPPER 0x1c
|
||||
#define ADNS3080_SHUTTER_MAX_BOUND_LOWER 0x1e
|
||||
#define ADNS3080_SHUTTER_MAX_BOUND_UPPER 0x1e
|
||||
#define ADNS3080_SROM_ID 0x1f
|
||||
#define ADNS3080_OBSERVATION 0x3d
|
||||
#define ADNS3080_INVERSE_PRODUCT_ID 0x3f
|
||||
#define ADNS3080_PIXEL_BURST 0x40
|
||||
#define ADNS3080_MOTION_BURST 0x50
|
||||
#define ADNS3080_SROM_LOAD 0x60
|
||||
|
||||
#define ADNS3080_LED_MODE_ALWAYS_ON 0x00
|
||||
#define ADNS3080_LED_MODE_WHEN_REQUIRED 0x01
|
||||
|
||||
#define ADNS3080_RESOLUTION_400 400
|
||||
#define ADNS3080_RESOLUTION_1200 1200
|
||||
|
||||
#define ADNS3080_FRAME_RATE_MAX 6469
|
||||
#define ADNS3080_FRAME_RATE_MIN 2000
|
||||
|
||||
|
||||
|
||||
class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow
|
||||
{
|
||||
private:
|
||||
// bytes to store SPI settings
|
||||
byte orig_spi_settings_spcr;
|
||||
byte orig_spi_settings_spsr;
|
||||
|
||||
// save and restore SPI settings
|
||||
byte backup_spi_settings();
|
||||
byte restore_spi_settings();
|
||||
|
||||
boolean _motion; // true if there has been motion
|
||||
|
||||
public:
|
||||
AP_OpticalFlow_ADNS3080(); // Constructor
|
||||
void init(boolean initCommAPI = true); // parameter controls whether I2C/SPI interface is initialised (set to false if other devices are on the I2C/SPI bus and have already initialised the interface)
|
||||
byte read_register(byte address);
|
||||
void write_register(byte address, byte value);
|
||||
void reset(); // reset sensor by holding a pin high (or is it low?) for 10us.
|
||||
int read(); // read latest values from sensor and fill in x,y and totals, return OPTICALFLOW_SUCCESS on successful read
|
||||
|
||||
// ADNS3080 specific features
|
||||
boolean motion() { if( _motion ) { _motion = false; return true; }else{ return false; } } // return true if there has been motion since the last time this was called
|
||||
|
||||
boolean get_led_always_on(); // returns true if LED is always on, false if only on when required
|
||||
void set_led_always_on( boolean alwaysOn ); // set parameter to true if you want LED always on, otherwise false for only when required
|
||||
|
||||
int get_resolution(); // returns resolution (either 400 or 1200 counts per inch)
|
||||
void set_resolution(int resolution); // set parameter to 400 or 1200 counts per inch
|
||||
|
||||
boolean 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(boolean auto_frame_rate); // set_frame_rate_auto(boolean) - set frame rate to auto (true), or manual (false)
|
||||
|
||||
unsigned int get_frame_period(); // get_frame_period -
|
||||
void set_frame_period(unsigned int period);
|
||||
|
||||
unsigned int get_frame_rate();
|
||||
void set_frame_rate(unsigned int rate);
|
||||
|
||||
boolean get_shutter_speed_auto(); // get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual
|
||||
void set_shutter_speed_auto(boolean auto_shutter_speed); // set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
|
||||
|
||||
unsigned int get_shutter_speed();
|
||||
unsigned int set_shutter_speed(unsigned int shutter_speed);
|
||||
|
||||
void clear_motion(); // will cause the x,y, dx, dy, and the sensor's motion registers to be cleared
|
||||
|
||||
int print_pixel_data(HardwareSerial *serPort); // dumps a 30x30 image to the Serial port
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,180 @@
|
|||
# File: ADNS3080ImageGrabber.py
|
||||
|
||||
import serial
|
||||
import string
|
||||
import math
|
||||
import time
|
||||
from Tkinter import *
|
||||
from threading import Timer
|
||||
|
||||
comPort = 'COM8' #default com port
|
||||
comPortBaud = 115200
|
||||
|
||||
class App:
|
||||
grid_size = 15
|
||||
num_pixels = 30
|
||||
image_started = FALSE
|
||||
image_current_row = 0;
|
||||
ser = serial.Serial()
|
||||
pixel_dictionary = {}
|
||||
|
||||
def __init__(self, master):
|
||||
|
||||
# set main window's title
|
||||
master.title("ADNS3080ImageGrabber")
|
||||
|
||||
frame = Frame(master)
|
||||
frame.grid(row=0,column=0)
|
||||
|
||||
self.comPortStr = StringVar()
|
||||
self.comPort = Entry(frame,textvariable=self.comPortStr)
|
||||
self.comPort.grid(row=0,column=0)
|
||||
self.comPort.delete(0, END)
|
||||
self.comPort.insert(0,comPort)
|
||||
|
||||
self.button = Button(frame, text="Open", fg="red", command=self.open_serial)
|
||||
self.button.grid(row=0,column=1)
|
||||
|
||||
self.entryStr = StringVar()
|
||||
self.entry = Entry(frame,textvariable=self.entryStr)
|
||||
self.entry.grid(row=0,column=2)
|
||||
self.entry.delete(0, END)
|
||||
self.entry.insert(0,"I")
|
||||
|
||||
self.send_button = Button(frame, text="Send", command=self.send_to_serial)
|
||||
self.send_button.grid(row=0,column=3)
|
||||
|
||||
self.canvas = Canvas(master, width=self.grid_size*self.num_pixels, height=self.grid_size*self.num_pixels)
|
||||
self.canvas.grid(row=1)
|
||||
|
||||
## start attempts to read from serial port
|
||||
self.read_loop()
|
||||
|
||||
def __del__(self):
|
||||
self.stop_read_loop()
|
||||
|
||||
def open_serial(self):
|
||||
# close the serial port
|
||||
if( self.ser.isOpen() ):
|
||||
try:
|
||||
self.ser.close()
|
||||
except:
|
||||
i=i # do nothing
|
||||
# open the serial port
|
||||
try:
|
||||
self.ser = serial.Serial(port=self.comPortStr.get(),baudrate=comPortBaud, timeout=1)
|
||||
print("serial port '" + self.comPortStr.get() + "' opened!")
|
||||
except:
|
||||
print("failed to open serial port '" + self.comPortStr.get() + "'")
|
||||
|
||||
def send_to_serial(self):
|
||||
if self.ser.isOpen():
|
||||
self.ser.write(self.entryStr.get())
|
||||
print "sent '" + self.entryStr.get() + "' to " + self.ser.portstr
|
||||
else:
|
||||
print "Serial port not open!"
|
||||
|
||||
def read_loop(self):
|
||||
try:
|
||||
self.t.cancel()
|
||||
except:
|
||||
aVar = 1 # do nothing
|
||||
#print("reading")
|
||||
if( self.ser.isOpen() ) :
|
||||
self.read_from_serial();
|
||||
|
||||
self.t = Timer(0.0,self.read_loop)
|
||||
self.t.start()
|
||||
|
||||
def stop_read_loop(self):
|
||||
try:
|
||||
self.t.cancel()
|
||||
except:
|
||||
print("failed to cancel timer")
|
||||
# do nothing
|
||||
|
||||
def read_from_serial(self):
|
||||
if( self.ser.isOpen() ):
|
||||
while( self.ser.inWaiting() > 0 ):
|
||||
|
||||
self.line_processed = FALSE
|
||||
line = self.ser.readline()
|
||||
|
||||
# process the line read
|
||||
|
||||
if( line.find("-------------------------") == 0 ):
|
||||
self.line_processed = TRUE
|
||||
self.image_started = FALSE
|
||||
self.image_current_row = 0
|
||||
|
||||
if( self.image_started == TRUE ):
|
||||
if( self.image_current_row >= self.num_pixels ):
|
||||
self.image_started == FALSE
|
||||
else:
|
||||
words = string.split(line,",")
|
||||
if len(words) >= 30:
|
||||
self.line_processed = TRUE
|
||||
x = 0
|
||||
for v in words:
|
||||
try:
|
||||
colour = int(v)
|
||||
except:
|
||||
colour = 0;
|
||||
#self.display_pixel(x,self.image_current_row,colour)
|
||||
self.display_pixel(self.num_pixels-1-self.image_current_row,self.num_pixels-1-x,colour)
|
||||
x += 1
|
||||
self.image_current_row += 1
|
||||
else:
|
||||
print("line " + str(self.image_current_row) + "incomplete (" + str(len(words)) + " of " + str(self.num_pixels) + "), ignoring")
|
||||
#print("bad line: " + line);
|
||||
|
||||
if( line.find("image data") >= 0 ):
|
||||
self.line_processed = TRUE
|
||||
self.image_started = TRUE
|
||||
self.image_current_row = 0
|
||||
# clear canvas
|
||||
#self.canvas.delete(ALL) # remove all items
|
||||
|
||||
#display the line if we couldn't understand it
|
||||
if( self.line_processed == FALSE ):
|
||||
print( line )
|
||||
|
||||
def display_default_image(self):
|
||||
# display the grid
|
||||
for x in range(0, self.num_pixels-1):
|
||||
for y in range(0, self.num_pixels-1):
|
||||
colour = x * y / 3.53
|
||||
self.display_pixel(x,y,colour)
|
||||
|
||||
def display_pixel(self, x, y, colour):
|
||||
if( x >= 0 and x < self.num_pixels and y >= 0 and y < self.num_pixels ) :
|
||||
|
||||
#find the old pixel if it exists and delete it
|
||||
if self.pixel_dictionary.has_key(x+y*self.num_pixels) :
|
||||
self.old_pixel = self.pixel_dictionary[x+y*self.num_pixels]
|
||||
self.canvas.delete(self.old_pixel)
|
||||
del(self.old_pixel)
|
||||
|
||||
fillColour = "#%02x%02x%02x" % (colour, colour, colour)
|
||||
#draw a new pixel and add to pixel_array
|
||||
self.new_pixel = self.canvas.create_rectangle(x*self.grid_size, y*self.grid_size, (x+1)*self.grid_size, (y+1)*self.grid_size, fill=fillColour)
|
||||
self.pixel_dictionary[x+y*self.num_pixels] = self.new_pixel
|
||||
|
||||
|
||||
## main loop ##
|
||||
|
||||
root = Tk()
|
||||
#root.withdraw()
|
||||
#serPort = SerialHandler(comPort,comPortBaud)
|
||||
|
||||
# create main display
|
||||
app = App(root)
|
||||
app.display_default_image()
|
||||
|
||||
print("entering main loop!")
|
||||
|
||||
root.mainloop()
|
||||
|
||||
app.stop_read_loop()
|
||||
|
||||
print("exiting")
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
Instructions for running the ADNS3080ImageGrabber.py
|
||||
|
||||
1. Start Arduino and upload the AP_OpticalFlow_test.pde to the APM
|
||||
2. Install Python 2.7 from http://www.python.org/getit/
|
||||
3. Start the Python IDLE editor
|
||||
4. File, Open, .../arduino-0022/libraries/AP_OpticalFlow/examples/ADNS3080ImageGrabber/ADNS3080ImageGrabber.py
|
||||
5. Run, Run Module - the Python Shell and then ADNS3080ImageGrabber applications should appear
|
||||
6. on the ADNS3080ImageGrabber screen, change the default com port, "COM8" to the correct port (sorry, doesn't auto detect)
|
||||
7. wait a few moments, then push the Send button to start/stop grabbing images from the sensor (a new image should appear every 2 seconds)
|
||||
|
||||
Note: you should see the the AP_OpticalFlow_ADNS3080's menu and any errors appear in the Python Shell
|
||||
|
|
@ -0,0 +1,383 @@
|
|||
/*
|
||||
Example of AP_OpticalFlow library.
|
||||
Code by Randy Mackay. DIYDrones.com
|
||||
*/
|
||||
|
||||
#include <avr/interrupt.h>
|
||||
#include <SPI.h> // Arduino SPI library
|
||||
#include "AP_OpticalFlow.h" // ArduCopter OpticalFlow Library
|
||||
|
||||
AP_OpticalFlow_ADNS3080 flowSensor;
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(115200);
|
||||
Serial.println("ArduPilot Mega OpticalFlow library test ver 1.5");
|
||||
|
||||
delay(1000);
|
||||
|
||||
flowSensor.init(); // flowSensor initialization
|
||||
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
//
|
||||
// display menu
|
||||
//
|
||||
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();
|
||||
}
|
||||
|
||||
//
|
||||
// display config
|
||||
//
|
||||
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();
|
||||
|
||||
// product id
|
||||
Serial.print("\tproduct id: ");
|
||||
Serial.print(flowSensor.read_register(ADNS3080_PRODUCT_ID),HEX);
|
||||
delayMicroseconds(50);
|
||||
Serial.print(" (hex)");
|
||||
Serial.println();
|
||||
|
||||
// frame rate
|
||||
Serial.print("\tframe rate: ");
|
||||
Serial.print(flowSensor.get_frame_rate());
|
||||
if( flowSensor.get_frame_rate_auto() == true ) {
|
||||
Serial.print(" (auto)");
|
||||
}else{
|
||||
Serial.print(" (manual)");
|
||||
}
|
||||
Serial.println();
|
||||
|
||||
// resolution
|
||||
Serial.print("\tresolution: ");
|
||||
Serial.print(flowSensor.get_resolution());
|
||||
Serial.println();
|
||||
|
||||
// shutter speed
|
||||
Serial.print("\tshutter speed: ");
|
||||
Serial.print(flowSensor.get_shutter_speed());
|
||||
if( flowSensor.get_shutter_speed_auto() ) {
|
||||
Serial.print(" (auto)");
|
||||
}else{
|
||||
Serial.print(" (manual)");
|
||||
}
|
||||
Serial.println();
|
||||
}
|
||||
|
||||
//
|
||||
// set frame rate
|
||||
//
|
||||
void set_frame_rate()
|
||||
{
|
||||
int value;
|
||||
byte extConfig;
|
||||
|
||||
// frame rate
|
||||
Serial.print("frame rate: ");
|
||||
Serial.print(flowSensor.get_frame_rate());
|
||||
if( flowSensor.get_frame_rate_auto() == true ) {
|
||||
Serial.print(" (auto)");
|
||||
}else{
|
||||
Serial.print(" (manual)");
|
||||
}
|
||||
Serial.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)");
|
||||
|
||||
// get user input
|
||||
Serial.flush();
|
||||
while( !Serial.available() ) {
|
||||
delay(20);
|
||||
}
|
||||
value = Serial.read();
|
||||
|
||||
if( value == 'a' || value == 'A')
|
||||
flowSensor.set_frame_rate_auto(true);
|
||||
if( value == '2' )
|
||||
flowSensor.set_frame_rate(2000);
|
||||
if( value == '3' )
|
||||
flowSensor.set_frame_rate(3000);
|
||||
if( value == '4' )
|
||||
flowSensor.set_frame_rate(4000);
|
||||
if( value == '5' )
|
||||
flowSensor.set_frame_rate(5000);
|
||||
if( value == '6' )
|
||||
flowSensor.set_frame_rate(6469);
|
||||
|
||||
// display new frame rate
|
||||
Serial.print("frame rate: ");
|
||||
Serial.print(flowSensor.get_frame_rate());
|
||||
if( flowSensor.get_frame_rate_auto() == true ) {
|
||||
Serial.print(" (auto)");
|
||||
}else{
|
||||
Serial.print(" (manual)");
|
||||
}
|
||||
Serial.println();
|
||||
}
|
||||
|
||||
// display_image - captures and displays image from flowSensor flowSensor
|
||||
void display_image()
|
||||
{
|
||||
Serial.println("image data --------------");
|
||||
flowSensor.print_pixel_data(&Serial);
|
||||
Serial.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");
|
||||
|
||||
Serial.flush();
|
||||
|
||||
while( !Serial.available() ) {
|
||||
display_image();
|
||||
i=0;
|
||||
while( i<20 && !Serial.available() ) {
|
||||
delay(100); // give the viewer a bit of time to catchup
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
Serial.flush();
|
||||
}
|
||||
|
||||
//
|
||||
// set resolutiojn
|
||||
//
|
||||
void set_resolution()
|
||||
{
|
||||
int value;
|
||||
byte reg;
|
||||
int resolution = flowSensor.get_resolution();
|
||||
Serial.print("resolution: ");
|
||||
Serial.println(resolution);
|
||||
Serial.println("Choose new value:");
|
||||
Serial.println(" 1) 1200");
|
||||
Serial.println(" 4) 400");
|
||||
Serial.println(" x) exit");
|
||||
Serial.println();
|
||||
|
||||
// get user input
|
||||
Serial.flush();
|
||||
while( !Serial.available() ) {
|
||||
delay(20);
|
||||
}
|
||||
value = Serial.read();
|
||||
|
||||
// update resolution
|
||||
if( value == '1' ) {
|
||||
flowSensor.set_resolution(ADNS3080_RESOLUTION_1200);
|
||||
}
|
||||
if( value == '4' ) {
|
||||
Serial.println("you want 1200!");
|
||||
flowSensor.set_resolution(ADNS3080_RESOLUTION_400);
|
||||
}
|
||||
|
||||
Serial.print("new resolution: ");
|
||||
Serial.println(flowSensor.get_resolution());
|
||||
}
|
||||
|
||||
//
|
||||
// set shutter speed
|
||||
//
|
||||
void set_shutter_speed()
|
||||
{
|
||||
int value;
|
||||
byte extConfig;
|
||||
|
||||
// shutter speed
|
||||
Serial.print("shutter speed: ");
|
||||
Serial.print(flowSensor.get_shutter_speed());
|
||||
if( flowSensor.get_shutter_speed_auto() == true ) {
|
||||
Serial.print(" (auto)");
|
||||
}else{
|
||||
Serial.print(" (manual)");
|
||||
}
|
||||
Serial.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)");
|
||||
|
||||
// get user input
|
||||
Serial.flush();
|
||||
while( !Serial.available() ) {
|
||||
delay(20);
|
||||
}
|
||||
value = Serial.read();
|
||||
|
||||
if( value == 'a' || value == 'A')
|
||||
flowSensor.set_shutter_speed_auto(true);
|
||||
if( value == '1' )
|
||||
flowSensor.set_shutter_speed(1000);
|
||||
if( value == '2' )
|
||||
flowSensor.set_shutter_speed(2000);
|
||||
if( value == '3' )
|
||||
flowSensor.set_shutter_speed(3000);
|
||||
if( value == '4' )
|
||||
flowSensor.set_shutter_speed(4000);
|
||||
if( value == '5' )
|
||||
flowSensor.set_shutter_speed(5000);
|
||||
if( value == '6' )
|
||||
flowSensor.set_shutter_speed(6000);
|
||||
if( value == '7' )
|
||||
flowSensor.set_shutter_speed(7000);
|
||||
if( value == '8' )
|
||||
flowSensor.set_shutter_speed(8000);
|
||||
if( value == '9' )
|
||||
flowSensor.set_shutter_speed(9000);
|
||||
|
||||
// display new shutter speed
|
||||
Serial.print("shutter speed: ");
|
||||
Serial.print(flowSensor.get_shutter_speed());
|
||||
if( flowSensor.get_shutter_speed_auto() == true ) {
|
||||
Serial.print(" (auto)");
|
||||
}else{
|
||||
Serial.print(" (manual)");
|
||||
}
|
||||
Serial.println();
|
||||
}
|
||||
|
||||
//
|
||||
// display motion - show x,y and squal values constantly until user presses a key
|
||||
//
|
||||
void display_motion()
|
||||
{
|
||||
int value;
|
||||
boolean first_time = true;
|
||||
Serial.flush();
|
||||
|
||||
// display instructions on how to exit
|
||||
Serial.println("press x to return to menu..");
|
||||
delay(1000);
|
||||
|
||||
while( !Serial.available() ) {
|
||||
flowSensor.read();
|
||||
|
||||
// x,y,squal
|
||||
//if( flowSensor.motion() || first_time ) {
|
||||
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();
|
||||
first_time = false;
|
||||
//}
|
||||
|
||||
// short delay
|
||||
delay(100);
|
||||
}
|
||||
|
||||
// flush the serial
|
||||
Serial.flush();
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
int value;
|
||||
|
||||
// display menu to user
|
||||
display_menu();
|
||||
|
||||
// wait for user to enter something
|
||||
while( !Serial.available() ) {
|
||||
delay(20);
|
||||
}
|
||||
|
||||
// get character from user
|
||||
value = Serial.read();
|
||||
|
||||
switch( value ) {
|
||||
|
||||
case 'c' :
|
||||
// display all config
|
||||
display_config();
|
||||
break;
|
||||
|
||||
case 'f' :
|
||||
// set frame rate
|
||||
set_frame_rate();
|
||||
break;
|
||||
|
||||
case 'i' :
|
||||
// display image
|
||||
display_image();
|
||||
break;
|
||||
|
||||
case 'I' :
|
||||
// display image continuously
|
||||
display_image_continuously();
|
||||
break;
|
||||
|
||||
case 'm' :
|
||||
// display motion
|
||||
display_motion();
|
||||
break;
|
||||
|
||||
case 'r' :
|
||||
// set resolution
|
||||
set_resolution();
|
||||
break;
|
||||
|
||||
case 's' :
|
||||
// set shutter speed
|
||||
set_shutter_speed();
|
||||
break;
|
||||
|
||||
case 'z' :
|
||||
// clear and reset everything
|
||||
flowSensor.clear_motion();
|
||||
break;
|
||||
|
||||
default:
|
||||
Serial.println("unrecognised command");
|
||||
Serial.println();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
AP_OpticalFlow KEYWORD1
|
||||
AP_OpticalFlow_ADNS3080 KEYWORD1
|
||||
init KEYWORD2
|
||||
read KEYWORD2
|
||||
read_register KEYWORD2
|
||||
write_register KEYWORD2
|
||||
reset KEYWORD2
|
||||
motion KEYWORD2
|
||||
get_led_always_on KEYWORD2
|
||||
set_led_always_on KEYWORD2
|
||||
get_resolution KEYWORD2
|
||||
set_resolution KEYWORD2
|
||||
get_frame_rate_auto KEYWORD2
|
||||
set_frame_rate_auto KEYWORD2
|
||||
get_frame_period KEYWORD2
|
||||
set_frame_period KEYWORD2
|
||||
get_frame_rate KEYWORD2
|
||||
set_frame_rate KEYWORD2
|
||||
get_shutter_speed_auto KEYWORD2
|
||||
set_shutter_speed_auto KEYWORD2
|
||||
get_shutter_speed KEYWORD2
|
||||
set_shutter_speed KEYWORD2
|
||||
clear_motion KEYWORD2
|
||||
print_pixel_data KEYWORD2
|
||||
Loading…
Reference in New Issue