From 8d1f9c9fd9b26dd40d0b45122dd96c1eda661d7b Mon Sep 17 00:00:00 2001 From: "rmackay9@yahoo.com" Date: Sat, 30 Apr 2011 08:29:28 +0000 Subject: [PATCH] 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 --- libraries/AP_OpticalFlow/AP_OpticalFlow.cpp | 35 ++ libraries/AP_OpticalFlow/AP_OpticalFlow.h | 48 ++ .../AP_OpticalFlow_ADNS3080.cpp | 420 ++++++++++++++++++ .../AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h | 113 +++++ .../ADNS3080ImageGrabber.py | 180 ++++++++ .../examples/ADNS3080ImageGrabber/README.txt | 11 + .../AP_OpticalFlow_test.pde | 383 ++++++++++++++++ libraries/AP_OpticalFlow/keywords.txt | 24 + 8 files changed, 1214 insertions(+) create mode 100644 libraries/AP_OpticalFlow/AP_OpticalFlow.cpp create mode 100644 libraries/AP_OpticalFlow/AP_OpticalFlow.h create mode 100644 libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp create mode 100644 libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h create mode 100644 libraries/AP_OpticalFlow/examples/ADNS3080ImageGrabber/ADNS3080ImageGrabber.py create mode 100644 libraries/AP_OpticalFlow/examples/ADNS3080ImageGrabber/README.txt create mode 100644 libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde create mode 100644 libraries/AP_OpticalFlow/keywords.txt diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp new file mode 100644 index 0000000000..875cbdfba2 --- /dev/null +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp @@ -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) +{ +} \ No newline at end of file diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h new file mode 100644 index 0000000000..87a6dabcad --- /dev/null +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.h @@ -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 +#include +#include +#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 diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp new file mode 100644 index 0000000000..326f0c5b4d --- /dev/null +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp @@ -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 +#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; iprintln("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(); +} diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h new file mode 100644 index 0000000000..2d8ec27968 --- /dev/null +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h @@ -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 diff --git a/libraries/AP_OpticalFlow/examples/ADNS3080ImageGrabber/ADNS3080ImageGrabber.py b/libraries/AP_OpticalFlow/examples/ADNS3080ImageGrabber/ADNS3080ImageGrabber.py new file mode 100644 index 0000000000..5cd7fe331e --- /dev/null +++ b/libraries/AP_OpticalFlow/examples/ADNS3080ImageGrabber/ADNS3080ImageGrabber.py @@ -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") diff --git a/libraries/AP_OpticalFlow/examples/ADNS3080ImageGrabber/README.txt b/libraries/AP_OpticalFlow/examples/ADNS3080ImageGrabber/README.txt new file mode 100644 index 0000000000..8992d2b3ee --- /dev/null +++ b/libraries/AP_OpticalFlow/examples/ADNS3080ImageGrabber/README.txt @@ -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 \ No newline at end of file diff --git a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde new file mode 100644 index 0000000000..2b6c674e13 --- /dev/null +++ b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde @@ -0,0 +1,383 @@ +/* + Example of AP_OpticalFlow library. + Code by Randy Mackay. DIYDrones.com +*/ + +#include +#include // 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; + } +} diff --git a/libraries/AP_OpticalFlow/keywords.txt b/libraries/AP_OpticalFlow/keywords.txt new file mode 100644 index 0000000000..d0d4033279 --- /dev/null +++ b/libraries/AP_OpticalFlow/keywords.txt @@ -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 \ No newline at end of file