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

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 2011-04-30 08:29:28 +00:00
parent 3ef57bf3bf
commit 8d1f9c9fd9
8 changed files with 1214 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AP_OpticalFlow/AP_OpticalFlow.cpp Normal file
View File

@ -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)
{
}

48
libraries/AP_OpticalFlow/AP_OpticalFlow.h Normal file
View File

@ -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

420
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp Normal file
View File

@ -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();
}

113
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h Normal file
View File

@ -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

180
libraries/AP_OpticalFlow/examples/ADNS3080ImageGrabber/ADNS3080ImageGrabber.py Normal file
View File

@ -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")

11
libraries/AP_OpticalFlow/examples/ADNS3080ImageGrabber/README.txt Normal file
View File

@ -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

383
libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde Normal file
View File

@ -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;
}
}

24
libraries/AP_OpticalFlow/keywords.txt Normal file
View File

@ -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