From cb584f81fb95c56c8cdeb39eb9cb78aa65ff6089 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Fri, 14 Sep 2012 21:02:53 +0900 Subject: [PATCH] AP_OpticalFlow: remove APM2 version of library now that AP_OpticalFlow_ADNS3080 class can autodetect which SPI bus the sensor is on --- .../AP_OpticalFlow_ADNS3080_APM2.cpp | 536 ------------------ .../AP_OpticalFlow_ADNS3080_APM2.h | 143 ----- 2 files changed, 679 deletions(-) delete mode 100644 libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080_APM2.cpp delete mode 100644 libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080_APM2.h diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080_APM2.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080_APM2.cpp deleted file mode 100644 index 6cc21b09ab..0000000000 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080_APM2.cpp +++ /dev/null @@ -1,536 +0,0 @@ -/* - * 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< 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; iprintln("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 deleted file mode 100644 index 830f7f52ba..0000000000 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080_APM2.h +++ /dev/null @@ -1,143 +0,0 @@ -#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