From e9fd7c955df1dd40cc958c0be7914a0d90e49d84 Mon Sep 17 00:00:00 2001 From: rmackay9 <rmackay9@yahoo.com> Date: Sat, 21 Apr 2012 20:10:35 +0900 Subject: [PATCH] AP_OpticalFlow - added support for optical flow for APM2 --- libraries/AP_OpticalFlow/AP_OpticalFlow.h | 1 + .../AP_OpticalFlow_ADNS3080_APM2.cpp | 536 ++++++++++++++++++ .../AP_OpticalFlow_ADNS3080_APM2.h | 141 +++++ 3 files changed, 678 insertions(+) create mode 100644 libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080_APM2.cpp create mode 100644 libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080_APM2.h diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h index 187499b57..970b57299 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.h @@ -60,5 +60,6 @@ protected: }; #include "AP_OpticalFlow_ADNS3080.h" +#include "AP_OpticalFlow_ADNS3080_APM2.h" #endif diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080_APM2.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080_APM2.cpp new file mode 100644 index 000000000..cb5b5c8be --- /dev/null +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080_APM2.cpp @@ -0,0 +1,536 @@ +/* + 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 "SPI.h" +#if defined(ARDUINO) && ARDUINO >= 100 + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +#define AP_SPI_TIMEOUT 1000 + +// We use Serial Port 2 in SPI Mode +#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + #define AP_SPI_DATAIN 15 // MISO + #define AP_SPI_DATAOUT 14 // MOSI + #define AP_SPI_CLOCK PJ2 // SCK +#else // normal arduino SPI pins...these need to be checked + # error Please check the Tools/Board menu to ensure you have selected Arduino Mega as your target. +#endif + +// mask for saving bit order and data mode to avoid interference with other users of the bus +#define UCSR3C_MASK 0x07 + +// SPI3 setting for UCSR3C +#define SPI3_MODE_SPI 0xC0 // UMSEL31 = 1, UMSEL30 = 1 + +// settings for phase and polarity bits of UCSR3C +#define SPI3_MODE_MASK 0x03 +#define SPI3_MODE0 0x00 +#define SPI3_MODE1 0x01 +#define SPI3_MODE2 0x02 +#define SPI3_MODE3 0x03 +#define SPI3_MODE SPI3_MODE3 + +// settings for phase and polarity bits of UCSR3C +#define SPI3_ORDER_MASK 0x04 +#define SPI3_MSBFIRST 0x00 +#define SPI3_LSBFIRST 0x04 + +#define SPI3_SPEED 0x04 // 2 megahertz? + +#define SPI3_DELAY 20 // delay in microseconds after sending data + + +union NumericIntType +{ + int intValue; + unsigned int uintValue; + byte byteValue[2]; +}; + + // Constructors //////////////////////////////////////////////////////////////// +AP_OpticalFlow_ADNS3080_APM2::AP_OpticalFlow_ADNS3080_APM2(int cs_pin, int reset_pin) : _cs_pin(cs_pin), _reset_pin(reset_pin) +{ + num_pixels = ADNS3080_PIXELS_X; + field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV; + scaler = AP_OPTICALFLOW_ADNS3080_SCALER; +} + +// SPI Methods +// *** INTERNAL FUNCTIONS *** +unsigned char AP_OpticalFlow_ADNS3080_APM2::SPI_transfer(uint8_t data) +{ + + /* Wait for empty transmit buffer */ + while ( !( UCSR3A & (1<<UDRE3)) ); + + /* Put data into buffer, sends the data */ + UDR3 = data; + + /* Wait for data to be received */ + while ( !(UCSR3A & (1<<RXC3)) ); + + /* Get and return received data from buffer */ + return UDR3; +} + +// 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) +bool +AP_OpticalFlow_ADNS3080_APM2::init(bool initCommAPI) +{ + int retry = 0; + + pinMode(AP_SPI_DATAOUT,OUTPUT); + pinMode(AP_SPI_DATAIN,INPUT); + pinMode(AP_SPI_CLOCK,OUTPUT); + pinMode(_cs_pin,OUTPUT); + if( _reset_pin != 0) + pinMode(ADNS3080_RESET,OUTPUT); + + digitalWrite(_cs_pin,HIGH); // disable device (Chip select is active low) + + // reset the device + reset(); + + // start the SPI library: + if( initCommAPI ) { + // Setup Serial Port3 in SPI mode (MSPI), Mode 0, Clock: 8Mhz + UBRR3 = 0; + DDRJ |= (1<<PJ2); // SPI clock XCK3 (PJ2) as output. This enable SPI Master mode + // put UART3 into SPI master mode + UCSR3C = SPI3_MODE_SPI | SPI3_MODE; + // Enable receiver and transmitter. + UCSR3B = (1<<RXEN3)|(1<<TXEN3); + // Set Baud rate + UBRR3 = SPI3_SPEED; // SPI running at 8Mhz + } + + delay(10); + + // check the sensor is functioning + while( retry < 3 ) { + if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) { + return true; + } + retry++; + } + + return false; +} + +// +// backup_spi_settings - checks current SPI settings (clock speed, etc), sets values to what we need +// +void AP_OpticalFlow_ADNS3080_APM2::backup_spi_settings() +{ + + uint8_t temp; + + /* Wait for empty transmit buffer */ + while ( !( UCSR3A & (1<<UDRE3)) ); + + // store current spi values + orig_spi_settings_ucsr3c = UCSR3C; + orig_spi_settings_ubrr3 = UBRR3; + + // decide new value for UCSR3C + temp = (orig_spi_settings_ucsr3c & ~UCSR3C_MASK) | SPI3_MODE | SPI3_MSBFIRST; + UCSR3C = temp; + UBRR3 = SPI3_SPEED; // SPI running at 1Mhz +} + +// restore_spi_settings - restores SPI settings (clock speed, etc) to what their values were before the sensor used the bus +void AP_OpticalFlow_ADNS3080_APM2::restore_spi_settings() +{ + /* Wait for empty transmit buffer */ + while ( !( UCSR3A & (1<<UDRE3)) ); + + // restore UCSRC3C and UBRR3 + UCSR3C = orig_spi_settings_ucsr3c; + UBRR3 = orig_spi_settings_ubrr3; +} + +// Read a register from the sensor +byte +AP_OpticalFlow_ADNS3080_APM2::read_register(byte address) +{ + byte result = 0, junk = 0; + + backup_spi_settings(); + + // take the chip select low to select the device + digitalWrite(_cs_pin, LOW); + + // send the device the register you want to read: + junk = SPI_transfer(address); + + // small delay + delayMicroseconds(SPI3_DELAY); + + // send a value of 0 to read the first byte returned: + result = SPI_transfer(0x00); + + // take the chip select high to de-select: + digitalWrite(_cs_pin, HIGH); + + restore_spi_settings(); + + return result; +} + +// write a value to one of the sensor's registers +void +AP_OpticalFlow_ADNS3080_APM2::write_register(byte address, byte value) +{ + byte junk = 0; + + backup_spi_settings(); + + // take the chip select low to select the device + digitalWrite(_cs_pin, LOW); + + // send register address + junk = SPI_transfer(address | 0x80 ); + + // small delay + delayMicroseconds(SPI3_DELAY); + + // send data + junk = SPI_transfer(value); + + // take the chip select high to de-select: + digitalWrite(_cs_pin, HIGH); + + restore_spi_settings(); +} + +// reset sensor by holding a pin high (or is it low?) for 10us. +void +AP_OpticalFlow_ADNS3080_APM2::reset() +{ + // return immediately if the reset pin is not defined + if( _reset_pin == 0) + return; + + digitalWrite(_reset_pin,HIGH); // reset sensor + delayMicroseconds(10); + digitalWrite(_reset_pin,LOW); // return sensor to normal +} + +// read latest values from sensor and fill in x,y and totals +bool +AP_OpticalFlow_ADNS3080_APM2::update() +{ + byte motion_reg; + surface_quality = (unsigned int)read_register(ADNS3080_SQUAL); + delayMicroseconds(SPI3_DELAY); // small delay + + // check for movement, update x,y values + motion_reg = read_register(ADNS3080_MOTION); + _overflow = ((motion_reg & 0x10) != 0); // check if we've had an overflow + if( (motion_reg & 0x80) != 0 ) { + raw_dx = ((char)read_register(ADNS3080_DELTA_X)); + delayMicroseconds(SPI3_DELAY); // small delay + raw_dy = ((char)read_register(ADNS3080_DELTA_Y)); + _motion = true; + }else{ + raw_dx = 0; + raw_dy = 0; + } + + last_update = millis(); + + apply_orientation_matrix(); + + return true; +} + +void +AP_OpticalFlow_ADNS3080_APM2::disable_serial_pullup() +{ + byte regVal = read_register(ADNS3080_EXTENDED_CONFIG); + regVal = (regVal | ADNS3080_SERIALNPU_OFF); + delayMicroseconds(SPI3_DELAY); // small delay + write_register(ADNS3080_EXTENDED_CONFIG, regVal); +} + +// get_led_always_on - returns true if LED is always on, false if only on when required +bool +AP_OpticalFlow_ADNS3080_APM2::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_APM2::set_led_always_on( bool alwaysOn ) +{ + byte regVal = read_register(ADNS3080_CONFIGURATION_BITS); + regVal = (regVal & 0xbf) | (alwaysOn << 6); + delayMicroseconds(SPI3_DELAY); // small delay + write_register(ADNS3080_CONFIGURATION_BITS, regVal); +} + +// returns resolution (either 400 or 1600 counts per inch) +int +AP_OpticalFlow_ADNS3080_APM2::get_resolution() +{ + if( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x10) == 0 ) + return 400; + else + return 1600; +} + +// set parameter to 400 or 1600 counts per inch +void +AP_OpticalFlow_ADNS3080_APM2::set_resolution(int resolution) +{ + byte regVal = read_register(ADNS3080_CONFIGURATION_BITS); + + if( resolution == ADNS3080_RESOLUTION_400 ) { + regVal &= ~0x10; + scaler = AP_OPTICALFLOW_ADNS3080_SCALER; + }else if( resolution == ADNS3080_RESOLUTION_1600) { + regVal |= 0x10; + scaler = AP_OPTICALFLOW_ADNS3080_SCALER * 4; + } + + delayMicroseconds(SPI3_DELAY); // small delay + write_register(ADNS3080_CONFIGURATION_BITS, regVal); + + // this will affect conversion factors so update them + update_conversion_factors(); +} + +// get_frame_rate_auto - return whether frame rate is set to "auto" or manual +bool +AP_OpticalFlow_ADNS3080_APM2::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_APM2::set_frame_rate_auto(bool auto_frame_rate) +{ + byte regVal = read_register(ADNS3080_EXTENDED_CONFIG); + delayMicroseconds(SPI3_DELAY); // small delay + if( auto_frame_rate == true ) { + // set specific frame period + write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,0xE0); + delayMicroseconds(SPI3_DELAY); // small delay + write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,0x1A); + delayMicroseconds(SPI3_DELAY); // 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_APM2::get_frame_period() +{ + NumericIntType aNum; + aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER); + delayMicroseconds(SPI3_DELAY); // small delay + aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER); + return aNum.uintValue; +} + +// set frame period +void +AP_OpticalFlow_ADNS3080_APM2::set_frame_period(unsigned int period) +{ + NumericIntType aNum; + aNum.uintValue = period; + + // set frame rate to manual + set_frame_rate_auto(false); + delayMicroseconds(SPI3_DELAY); // small delay + + // set specific frame period + write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]); + delayMicroseconds(SPI3_DELAY); // small delay + write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]); + +} + +unsigned int +AP_OpticalFlow_ADNS3080_APM2::get_frame_rate() +{ + unsigned long clockSpeed = ADNS3080_CLOCK_SPEED; + unsigned int rate = clockSpeed / get_frame_period(); + return rate; +} + +void +AP_OpticalFlow_ADNS3080_APM2::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 +bool +AP_OpticalFlow_ADNS3080_APM2::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_APM2::set_shutter_speed_auto(bool auto_shutter_speed) +{ + byte regVal = read_register(ADNS3080_EXTENDED_CONFIG); + delayMicroseconds(SPI3_DELAY); // small delay + if( auto_shutter_speed ) { + // return shutter speed max to default + write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,0x8c); + delayMicroseconds(SPI3_DELAY); // small delay + write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,0x20); + delayMicroseconds(SPI3_DELAY); // small delay + + // determine value to put into extended config + regVal &= ~0x02; + }else{ + // determine value to put into extended config + regVal |= 0x02; + } + write_register(ADNS3080_EXTENDED_CONFIG, regVal); + delayMicroseconds(SPI3_DELAY); // small delay +} + +// get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual +unsigned int +AP_OpticalFlow_ADNS3080_APM2::get_shutter_speed() +{ + NumericIntType aNum; + aNum.byteValue[1] = read_register(ADNS3080_SHUTTER_UPPER); + delayMicroseconds(SPI3_DELAY); // 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) +void +AP_OpticalFlow_ADNS3080_APM2::set_shutter_speed(unsigned int shutter_speed) +{ + NumericIntType aNum; + aNum.uintValue = shutter_speed; + + // set shutter speed to manual + set_shutter_speed_auto(false); + delayMicroseconds(SPI3_DELAY); // small delay + + // set specific shutter speed + write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]); + delayMicroseconds(SPI3_DELAY); // small delay + write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,aNum.byteValue[1]); + delayMicroseconds(SPI3_DELAY); // 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(SPI3_DELAY); // small delay + aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER); + delayMicroseconds(SPI3_DELAY); // small delay + write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]); + delayMicroseconds(SPI3_DELAY); // small delay + write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]); + delayMicroseconds(SPI3_DELAY); // small delay +} + +// clear_motion - will cause the Delta_X, Delta_Y, and internal motion registers to be cleared +void +AP_OpticalFlow_ADNS3080_APM2::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 +void +AP_OpticalFlow_ADNS3080_APM2::print_pixel_data(Stream *serPort) +{ + int i,j; + bool 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(SPI3_DELAY); + } + serPort->println(); + } + + // hardware reset to restore sensor to normal operation + reset(); +} diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080_APM2.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080_APM2.h new file mode 100644 index 000000000..cca404d5b --- /dev/null +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080_APM2.h @@ -0,0 +1,141 @@ +#ifndef AP_OPTICALFLOW_ADNS3080_APM2_H +#define AP_OPTICALFLOW_ADNS3080_APM2_H + +#include "AP_OpticalFlow.h" + +// default pin settings +#define ADNS3080_CHIP_SELECT 34 // PC3 +#define ADNS3080_RESET 0 // reset pin is unattached by default + +// orientations for ADNS3080 sensor +#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD ROTATION_YAW_180 +#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_RIGHT ROTATION_YAW_135 +#define AP_OPTICALFLOW_ADNS3080_PINS_RIGHT ROTATION_YAW_90 +#define AP_OPTICALFLOW_ADNS3080_PINS_BACK_RIGHT ROTATION_YAW_45 +#define AP_OPTICALFLOW_ADNS3080_PINS_BACK ROTATION_NONE +#define AP_OPTICALFLOW_ADNS3080_PINS_BACK_LEFT ROTATION_YAW_315 +#define AP_OPTICALFLOW_ADNS3080_PINS_LEFT ROTATION_YAW_270 +#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_LEFT ROTATION_YAW_225 + +// field of view of ADNS3080 sensor lenses +#define AP_OPTICALFLOW_ADNS3080_08_FOV 0.202458 // 11.6 degrees + +// scaler - value returned when sensor is moved equivalent of 1 pixel +#define AP_OPTICALFLOW_ADNS3080_SCALER 1.1 + +// 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 + +// Configuration Bits +#define ADNS3080_LED_MODE_ALWAYS_ON 0x00 +#define ADNS3080_LED_MODE_WHEN_REQUIRED 0x01 + +#define ADNS3080_RESOLUTION_400 400 +#define ADNS3080_RESOLUTION_1600 1600 + +// Extended Configuration bits +#define ADNS3080_SERIALNPU_OFF 0x02 + +#define ADNS3080_FRAME_RATE_MAX 6469 +#define ADNS3080_FRAME_RATE_MIN 2000 + + +class AP_OpticalFlow_ADNS3080_APM2 : public AP_OpticalFlow +{ + private: + // bytes to store SPI settings + byte orig_spi_settings_ucsr3c; + byte orig_spi_settings_ubrr3; + + // save and restore SPI settings + void backup_spi_settings(); + void restore_spi_settings(); + + public: + int _cs_pin; // pin used for chip select + int _reset_pin; // pin used for chip reset + bool _motion; // true if there has been motion + bool _overflow; // true if the x or y data buffers overflowed + + public: + AP_OpticalFlow_ADNS3080_APM2(int cs_pin = ADNS3080_CHIP_SELECT, int reset_pin = ADNS3080_RESET); + bool init(bool 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. + bool update(); // read latest values from sensor and fill in x,y and totals, return true on successful read + + // ADNS3080 specific features + bool motion() { if( _motion ) { _motion = false; return true; }else{ return false; } } // return true if there has been motion since the last time this was called + + void disable_serial_pullup(); + + bool get_led_always_on(); // returns true if LED is always on, false if only on when required + void set_led_always_on( bool alwaysOn ); // set parameter to true if you want LED always on, otherwise false for only when required + + int get_resolution(); // returns resolution (either 400 or 1600 counts per inch) + void set_resolution(int resolution); // set parameter to 400 or 1600 counts per inch + + bool get_frame_rate_auto(); // get_frame_rate_auto - return true if frame rate is set to "auto", false if manual + void set_frame_rate_auto(bool auto_frame_rate); // set_frame_rate_auto(bool) - set frame rate to auto (true), or manual (false) + + 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); + + bool get_shutter_speed_auto(); // get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual + void set_shutter_speed_auto(bool auto_shutter_speed); // set_shutter_speed_auto - set shutter speed to auto (true), or manual (false) + + unsigned int get_shutter_speed(); + void 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 + + void print_pixel_data(Stream *serPort); // dumps a 30x30 image to the Serial port + + // SPI functions - we use UAT3 which is not supported by Arduino + unsigned char SPI_transfer(unsigned char data); + void CS_inactive(); + void CS_active(); + void PageErase (uint16_t PageAdr); + void ChipErase (); +}; + +#endif -- GitLab