diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp index 157ac750c0..231d2a4f7f 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp @@ -7,24 +7,11 @@ * 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" +#include "SPI3.h" #if defined(ARDUINO) && ARDUINO >= 100 #include "Arduino.h" #else @@ -35,42 +22,45 @@ // We use Serial Port 2 in SPI Mode #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - #define AP_SPI_DATAIN 50 // MISO // PB3 - #define AP_SPI_DATAOUT 51 // MOSI // PB2 - #define AP_SPI_CLOCK 52 // SCK // PB1 + #define ADNS3080_SPI_MISO 50 // PB3 + #define ADNS3080_SPI_MOSI 51 // PB2 + #define ADNS3080_SPI_SCK 52 // PB1 #else // normal arduino SPI pins...these need to be checked - #define AP_SPI_DATAIN 12 //MISO - #define AP_SPI_DATAOUT 11 //MOSI - #define AP_SPI_CLOCK 13 //SCK + #define ADNS3080_SPI_MISO 12 // MISO + #define ADNS3080_SPI_MOSI 11 // MOSI + #define ADNS3080_SPI_SCK 13 // SCK #endif union NumericIntType { - int intValue; - unsigned int uintValue; - byte byteValue[2]; + int16_t intValue; + uint16_t uintValue; + uint8_t byteValue[2]; }; // Constructors //////////////////////////////////////////////////////////////// -AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080(int cs_pin, int reset_pin) : _cs_pin(cs_pin), _reset_pin(reset_pin) +AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080(int16_t cs_pin, int16_t reset_pin) : + _cs_pin(cs_pin), + _reset_pin(reset_pin), + _spi_bus(ADNS3080_SPI_UNKNOWN) { num_pixels = ADNS3080_PIXELS_X; field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV; scaler = AP_OPTICALFLOW_ADNS3080_SCALER; } - // 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) +// assumes SPI bus has been initialised but will attempt to initialise nonstandard SPI3 bus if required bool -AP_OpticalFlow_ADNS3080::init(bool initCommAPI) +AP_OpticalFlow_ADNS3080::init(bool initCommAPI, AP_PeriodicProcess *scheduler) { - int retry = 0; + int8_t retry = 0; + bool retvalue = false; + + // suspend timer while we set-up SPI communication + scheduler->suspend_timer(); - 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); @@ -82,77 +72,127 @@ AP_OpticalFlow_ADNS3080::init(bool initCommAPI) // start the SPI library: if( initCommAPI ) { + pinMode(ADNS3080_SPI_MOSI,OUTPUT); + pinMode(ADNS3080_SPI_MISO,INPUT); + pinMode(ADNS3080_SPI_SCK,OUTPUT); SPI.begin(); + SPI.setClockDivider(SPI_CLOCK_DIV8); // 2MHZ SPI rate } - // check the sensor is functioning - while( retry < 3 ) { - if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) - return true; + // check 3 times for the sensor on standard SPI bus + _spi_bus = ADNS3080_SPIBUS_1; + while( retvalue == false && retry < 3 ) { + if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) { + retvalue = true; + } retry++; } - return false; + // if not found, check 3 times on SPI3 + if( !retvalue ) { + + // start the SPI3 library: + if( initCommAPI ) { + SPI3.begin(); + SPI3.setDataMode(SPI3_MODE3); // Mode3 + SPI3.setSpeed(SPI3_SPEED_2MHZ); // 2MHZ SPI rate + } + + _spi_bus = ADNS3080_SPIBUS_3; + retry = 0; + while( retvalue == false && retry < 3 ) { + if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) { + retvalue = true; + } + retry++; + } + } + + // resume timer + scheduler->resume_timer(); + + // if device is working register the global static read function to be called at 1khz + if( retvalue ) { + scheduler->register_process( AP_OpticalFlow_ADNS3080::read ); + }else{ + _spi_bus = ADNS3080_SPI_UNKNOWN; + } + + return retvalue; } // // backup_spi_settings - checks current SPI settings (clock speed, etc), sets values to what we need // -byte +void AP_OpticalFlow_ADNS3080::backup_spi_settings() { - // store current spi values - orig_spi_settings_spcr = SPCR & (DORD | CPOL | CPHA); - orig_spi_settings_spsr = SPSR & SPI2X; + if( _spi_bus == ADNS3080_SPIBUS_1 ) { + // store current spi mode + orig_spi_settings_spcr = SPCR & (CPOL | CPHA); - // 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 + // set to our required values + SPI.setDataMode(SPI_MODE3); + // we do not set speed to 2Mhz because we assume it is that already no more than 2Mhz - return orig_spi_settings_spcr; + }else if( _spi_bus == ADNS3080_SPIBUS_3 ) { + /* Wait for empty transmit buffer */ + while ( !( UCSR3A & (1< 0 ) { return false; }else{ @@ -382,7 +421,7 @@ AP_OpticalFlow_ADNS3080::get_shutter_speed_auto() void AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool auto_shutter_speed) { - byte regVal = read_register(ADNS3080_EXTENDED_CONFIG); + uint8_t regVal = read_register(ADNS3080_EXTENDED_CONFIG); delayMicroseconds(50); // small delay if( auto_shutter_speed ) { // return shutter speed max to default @@ -402,7 +441,7 @@ AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool auto_shutter_speed) } // get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual -unsigned int +uint16_t AP_OpticalFlow_ADNS3080::get_shutter_speed() { NumericIntType aNum; @@ -460,10 +499,10 @@ AP_OpticalFlow_ADNS3080::clear_motion() void AP_OpticalFlow_ADNS3080::print_pixel_data(Stream *serPort) { - int i,j; + int16_t i,j; bool isFirstPixel = true; - byte regValue; - byte pixelValue; + uint8_t regValue; + uint8_t pixelValue; // write to frame capture register to force capture of frame write_register(ADNS3080_FRAME_CAPTURE,0x83); diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h index a122dd6027..681825f2e1 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h @@ -74,62 +74,70 @@ #define ADNS3080_FRAME_RATE_MAX 6469 #define ADNS3080_FRAME_RATE_MIN 2000 +// SPI bus definitions +#define ADNS3080_SPI_UNKNOWN 0 +#define ADNS3080_SPIBUS_1 1 // standard SPI bus +#define ADNS3080_SPIBUS_3 3 // SPI3 + 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(); - 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(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); + AP_OpticalFlow_ADNS3080(int16_t cs_pin = ADNS3080_CHIP_SELECT, int16_t reset_pin = ADNS3080_RESET); + bool init(bool initCommAPI, AP_PeriodicProcess *scheduler); // 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) + uint8_t read_register(uint8_t address); + void write_register(uint8_t address, uint8_t 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 + void update(uint32_t now); // 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 + + // return true if there has been motion since the last time this was called + bool motion() { if( _motion ) { _motion = false; return true; }else{ return false; } } + + bool overflow() { return _overflow; } // true if there has been an overflow 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 + int16_t get_resolution(); // returns resolution (either 400 or 1600 counts per inch) + void set_resolution(uint16_t 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); + uint16_t get_frame_period(); // get_frame_period + void set_frame_period(uint16_t period); - unsigned int get_frame_rate(); - void set_frame_rate(unsigned int rate); + uint16_t get_frame_rate(); + void set_frame_rate(uint16_t 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); + uint16_t get_shutter_speed(); + void set_shutter_speed(uint16_t 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 + +private: + // bytes to store SPI settings + uint8_t orig_spi_settings_spcr; // spi1's mode + uint8_t orig_spi3_settings_ucsr3c; // spi3's mode + uint8_t orig_spi3_settings_ubrr3; // spi3's speed + + // save and restore SPI settings + void backup_spi_settings(); + void restore_spi_settings(); + + int16_t _cs_pin; // pin used for chip select + int16_t _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 + uint8_t _spi_bus; // 0 = unknown, 1 = using SPI, 3 = using SPI3 }; #endif