From 2ab3d2c080fa2dc4c658dbacb4cfc42600dfd216 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Mar 2012 13:51:28 +1100 Subject: [PATCH] OpticalFlow: fixed line endings this library was a mixture of dos and unix line endings, which makes for very messy editing --- libraries/AP_OpticalFlow/AP_OpticalFlow.cpp | 204 ++-- libraries/AP_OpticalFlow/AP_OpticalFlow.h | 142 +-- .../AP_OpticalFlow_ADNS3080.cpp | 942 +++++++++--------- .../AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h | 286 +++--- 4 files changed, 787 insertions(+), 787 deletions(-) diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index 00fb2eca2c0..1aeed584c0f 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp @@ -1,109 +1,109 @@ -/* - 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" - -#define FORTYFIVE_DEGREES 0.78539816 - -AP_OpticalFlow* AP_OpticalFlow::_sensor = NULL; // pointer to the last instantiated optical flow sensor. Will be turned into a table if we ever add support for more than one sensor - -// 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) -bool -AP_OpticalFlow::init(bool initCommAPI) -{ - _orientation_matrix = Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1); - update_conversion_factors(); - return true; // just return true by default -} - -// set_orientation - Rotation vector to transform sensor readings to the body frame. -void -AP_OpticalFlow::set_orientation(const Matrix3f &rotation_matrix) -{ - _orientation_matrix = rotation_matrix; -} - -// read value from the sensor. Should be overridden by derived class -bool -AP_OpticalFlow::update() -{ +/* + 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" + +#define FORTYFIVE_DEGREES 0.78539816 + +AP_OpticalFlow* AP_OpticalFlow::_sensor = NULL; // pointer to the last instantiated optical flow sensor. Will be turned into a table if we ever add support for more than one sensor + +// 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) +bool +AP_OpticalFlow::init(bool initCommAPI) +{ + _orientation_matrix = Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1); + update_conversion_factors(); + return true; // just return true by default +} + +// set_orientation - Rotation vector to transform sensor readings to the body frame. +void +AP_OpticalFlow::set_orientation(const Matrix3f &rotation_matrix) +{ + _orientation_matrix = rotation_matrix; +} + +// read value from the sensor. Should be overridden by derived class +bool +AP_OpticalFlow::update() +{ return true; -} - -// reads a value from the sensor (will be sensor specific) -byte -AP_OpticalFlow::read_register(byte address) -{ +} + +// reads a value from the sensor (will be sensor specific) +byte +AP_OpticalFlow::read_register(byte address) +{ return 0; -} - -// writes a value to one of the sensor's register (will be sensor specific) -void -AP_OpticalFlow::write_register(byte address, byte value) -{ -} - -// rotate raw values to arrive at final x,y,dx and dy values -void -AP_OpticalFlow::apply_orientation_matrix() -{ - Vector3f rot_vector; - - // next rotate dx and dy - rot_vector = _orientation_matrix * Vector3f(raw_dx, raw_dy, 0); - dx = rot_vector.x; - dy = rot_vector.y; - - // add rotated values to totals (perhaps this is pointless as we need to take into account yaw, roll, pitch) - x += dx; - y += dy; -} - -// updatse conversion factors that are dependent upon field_of_view -void -AP_OpticalFlow::update_conversion_factors() -{ - conv_factor = (1.0 / (float)(num_pixels * scaler)) * 2.0 * tan(field_of_view / 2.0); // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude) - // 0.00615 - radians_to_pixels = (num_pixels * scaler) / field_of_view; - // 162.99 -} - -// updates internal lon and lat with estimation based on optical flow -void -AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float sin_yaw_y, float altitude) -{ - float diff_roll = roll - _last_roll; - float diff_pitch = pitch - _last_pitch; - - // only update position if surface quality is good and angle is not over 45 degrees - if( surface_quality >= 10 && fabs(roll) <= FORTYFIVE_DEGREES && fabs(pitch) <= FORTYFIVE_DEGREES ) { - altitude = max(altitude, 0); - // calculate expected x,y diff due to roll and pitch change - exp_change_x = diff_roll * radians_to_pixels; - exp_change_y = -diff_pitch * radians_to_pixels; - - // real estimated raw change from mouse - change_x = dx - exp_change_x; - change_y = dy - exp_change_y; - +} + +// writes a value to one of the sensor's register (will be sensor specific) +void +AP_OpticalFlow::write_register(byte address, byte value) +{ +} + +// rotate raw values to arrive at final x,y,dx and dy values +void +AP_OpticalFlow::apply_orientation_matrix() +{ + Vector3f rot_vector; + + // next rotate dx and dy + rot_vector = _orientation_matrix * Vector3f(raw_dx, raw_dy, 0); + dx = rot_vector.x; + dy = rot_vector.y; + + // add rotated values to totals (perhaps this is pointless as we need to take into account yaw, roll, pitch) + x += dx; + y += dy; +} + +// updatse conversion factors that are dependent upon field_of_view +void +AP_OpticalFlow::update_conversion_factors() +{ + conv_factor = (1.0 / (float)(num_pixels * scaler)) * 2.0 * tan(field_of_view / 2.0); // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude) + // 0.00615 + radians_to_pixels = (num_pixels * scaler) / field_of_view; + // 162.99 +} + +// updates internal lon and lat with estimation based on optical flow +void +AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float sin_yaw_y, float altitude) +{ + float diff_roll = roll - _last_roll; + float diff_pitch = pitch - _last_pitch; + + // only update position if surface quality is good and angle is not over 45 degrees + if( surface_quality >= 10 && fabs(roll) <= FORTYFIVE_DEGREES && fabs(pitch) <= FORTYFIVE_DEGREES ) { + altitude = max(altitude, 0); + // calculate expected x,y diff due to roll and pitch change + exp_change_x = diff_roll * radians_to_pixels; + exp_change_y = -diff_pitch * radians_to_pixels; + + // real estimated raw change from mouse + change_x = dx - exp_change_x; + change_y = dy - exp_change_y; + float avg_altitude = (altitude + _last_altitude)*0.5; - // convert raw change to horizontal movement in cm - x_cm = -change_x * avg_altitude * conv_factor; // perhaps this altitude should actually be the distance to the ground? i.e. if we are very rolled over it should be longer? - y_cm = -change_y * avg_altitude * conv_factor; // for example if you are leaned over at 45 deg the ground will appear farther away and motion from opt flow sensor will be less - - // convert x/y movements into lon/lat movement - vlon = x_cm * sin_yaw_y + y_cm * cos_yaw_x; - vlat = y_cm * sin_yaw_y - x_cm * cos_yaw_x; - } + // convert raw change to horizontal movement in cm + x_cm = -change_x * avg_altitude * conv_factor; // perhaps this altitude should actually be the distance to the ground? i.e. if we are very rolled over it should be longer? + y_cm = -change_y * avg_altitude * conv_factor; // for example if you are leaned over at 45 deg the ground will appear farther away and motion from opt flow sensor will be less + + // convert x/y movements into lon/lat movement + vlon = x_cm * sin_yaw_y + y_cm * cos_yaw_x; + vlat = y_cm * sin_yaw_y - x_cm * cos_yaw_x; + } _last_altitude = altitude; _last_roll = roll; diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h index 968de9cd49d..5e6657fefe5 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.h @@ -1,74 +1,74 @@ -#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 - -// standard rotation matrices -#define AP_OPTICALFLOW_ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1) -#define AP_OPTICALFLOW_ROTATION_YAW_45 Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1) -#define AP_OPTICALFLOW_ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1) -#define AP_OPTICALFLOW_ROTATION_YAW_135 Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1) -#define AP_OPTICALFLOW_ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1) -#define AP_OPTICALFLOW_ROTATION_YAW_225 Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1) -#define AP_OPTICALFLOW_ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1) -#define AP_OPTICALFLOW_ROTATION_YAW_315 Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1) - -class AP_OpticalFlow -{ - public: - int raw_dx, raw_dy; // raw sensor change in x and y position (i.e. unrotated) - int surface_quality; // image quality (below 15 you really can't trust the x,y values returned) - int x,y; // total x,y position - int dx,dy; // rotated change in x and y position - float vlon, vlat; // position as offsets from original position - unsigned long last_update; // millis() time of last update - float field_of_view; // field of view in Radians - float scaler; // number returned from sensor when moved one pixel - int num_pixels; // number of pixels of resolution in the sensor - // temp variables - delete me! - float exp_change_x, exp_change_y; - float change_x, change_y; - float x_cm, y_cm; - - AP_OpticalFlow() { _sensor = this; }; - ~AP_OpticalFlow() { _sensor = NULL; }; - virtual 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) - virtual byte read_register(byte address); - virtual void write_register(byte address, byte value); - virtual void set_orientation(const Matrix3f &rotation_matrix); // Rotation vector to transform sensor readings to the body frame. - virtual void set_field_of_view(const float fov) { field_of_view = fov; update_conversion_factors(); }; // sets field of view of sensor - static void read(uint32_t ) { if( _sensor != NULL ) _sensor->update(); }; // call to update all attached sensors - virtual bool update(); // read latest values from sensor and fill in x,y and totals. returns true on success - virtual void update_position(float roll, float pitch, float cos_yaw_x, float sin_yaw_y, float altitude); // updates internal lon and lat with estimation based on optical flow - -protected: - static AP_OpticalFlow *_sensor; // pointer to the last instantiated optical flow sensor. Will be turned into a table if we ever add support for more than one sensor - Matrix3f _orientation_matrix; - float conv_factor; // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude) - float radians_to_pixels; +#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 + +// standard rotation matrices +#define AP_OPTICALFLOW_ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1) +#define AP_OPTICALFLOW_ROTATION_YAW_45 Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1) +#define AP_OPTICALFLOW_ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1) +#define AP_OPTICALFLOW_ROTATION_YAW_135 Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1) +#define AP_OPTICALFLOW_ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1) +#define AP_OPTICALFLOW_ROTATION_YAW_225 Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1) +#define AP_OPTICALFLOW_ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1) +#define AP_OPTICALFLOW_ROTATION_YAW_315 Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1) + +class AP_OpticalFlow +{ + public: + int raw_dx, raw_dy; // raw sensor change in x and y position (i.e. unrotated) + int surface_quality; // image quality (below 15 you really can't trust the x,y values returned) + int x,y; // total x,y position + int dx,dy; // rotated change in x and y position + float vlon, vlat; // position as offsets from original position + unsigned long last_update; // millis() time of last update + float field_of_view; // field of view in Radians + float scaler; // number returned from sensor when moved one pixel + int num_pixels; // number of pixels of resolution in the sensor + // temp variables - delete me! + float exp_change_x, exp_change_y; + float change_x, change_y; + float x_cm, y_cm; + + AP_OpticalFlow() { _sensor = this; }; + ~AP_OpticalFlow() { _sensor = NULL; }; + virtual 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) + virtual byte read_register(byte address); + virtual void write_register(byte address, byte value); + virtual void set_orientation(const Matrix3f &rotation_matrix); // Rotation vector to transform sensor readings to the body frame. + virtual void set_field_of_view(const float fov) { field_of_view = fov; update_conversion_factors(); }; // sets field of view of sensor + static void read(uint32_t ) { if( _sensor != NULL ) _sensor->update(); }; // call to update all attached sensors + virtual bool update(); // read latest values from sensor and fill in x,y and totals. returns true on success + virtual void update_position(float roll, float pitch, float cos_yaw_x, float sin_yaw_y, float altitude); // updates internal lon and lat with estimation based on optical flow + +protected: + static AP_OpticalFlow *_sensor; // pointer to the last instantiated optical flow sensor. Will be turned into a table if we ever add support for more than one sensor + Matrix3f _orientation_matrix; + float conv_factor; // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude) + float radians_to_pixels; float _last_roll, _last_pitch, _last_altitude; - virtual void apply_orientation_matrix(); // rotate raw values to arrive at final x,y,dx and dy values - virtual void update_conversion_factors(); -}; - + virtual void apply_orientation_matrix(); // rotate raw values to arrive at final x,y,dx and dy values + virtual void update_conversion_factors(); +}; + #include "AP_OpticalFlow_ADNS3080.h" -#endif +#endif diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp index dc24db13922..fda2472619b 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp @@ -1,482 +1,482 @@ -/* - 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 - -union NumericIntType -{ - int intValue; - unsigned int uintValue; - byte byteValue[2]; -}; - - // Constructors //////////////////////////////////////////////////////////////// -AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080(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; -} - - -// 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::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 ) { - SPI.begin(); - } - - // check the sensor is functioning - if( retry < 3 ) { - if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) - return true; +/* + 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 + +union NumericIntType +{ + int intValue; + unsigned int uintValue; + byte byteValue[2]; +}; + + // Constructors //////////////////////////////////////////////////////////////// +AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080(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; +} + + +// 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::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 ) { + SPI.begin(); + } + + // check the sensor is functioning + if( 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 -// -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(_cs_pin, 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(_cs_pin, 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(_cs_pin, 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(_cs_pin, HIGH); - - restore_spi_settings(); -} - -// reset sensor by holding a pin high (or is it low?) for 10us. -void -AP_OpticalFlow_ADNS3080::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::update() -{ - byte motion_reg; - surface_quality = (unsigned int)read_register(ADNS3080_SQUAL); - delayMicroseconds(50); // 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(50); // 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::disable_serial_pullup() -{ - byte regVal = read_register(ADNS3080_EXTENDED_CONFIG); - regVal = (regVal | ADNS3080_SERIALNPU_OFF); - delayMicroseconds(50); // 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::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( bool alwaysOn ) -{ - byte regVal = read_register(ADNS3080_CONFIGURATION_BITS); +} + +// +// 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(_cs_pin, 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(_cs_pin, 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(_cs_pin, 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(_cs_pin, HIGH); + + restore_spi_settings(); +} + +// reset sensor by holding a pin high (or is it low?) for 10us. +void +AP_OpticalFlow_ADNS3080::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::update() +{ + byte motion_reg; + surface_quality = (unsigned int)read_register(ADNS3080_SQUAL); + delayMicroseconds(50); // 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(50); // 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::disable_serial_pullup() +{ + byte regVal = read_register(ADNS3080_EXTENDED_CONFIG); + regVal = (regVal | ADNS3080_SERIALNPU_OFF); + delayMicroseconds(50); // 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::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( bool 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 1600 counts per inch) -int -AP_OpticalFlow_ADNS3080::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::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(50); // 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::get_frame_rate_auto() -{ - byte regVal = read_register(ADNS3080_EXTENDED_CONFIG); + delayMicroseconds(50); // small delay + write_register(ADNS3080_CONFIGURATION_BITS, regVal); +} + +// returns resolution (either 400 or 1600 counts per inch) +int +AP_OpticalFlow_ADNS3080::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::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(50); // 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::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(bool 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 -bool -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(bool 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 + 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(bool 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 +bool +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(bool 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 &= ~0x02; - }else{ - // determine value to put into extended config + }else{ + // determine value to put into extended config regVal |= 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) + } + 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) void -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 +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::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(50); - } - serPort->println(); - } - - // hardware reset to restore sensor to normal operation - reset(); -} + 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(); +} diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h index cb5eb1bb3df..eee31c5eb81 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h @@ -1,145 +1,145 @@ -#ifndef AP_OPTICALFLOW_ADNS3080_H -#define AP_OPTICALFLOW_ADNS3080_H - -#include "AP_OpticalFlow.h" - -// orientations for ADNS3080 sensor -#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD AP_OPTICALFLOW_ROTATION_YAW_180 -#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_RIGHT AP_OPTICALFLOW_ROTATION_YAW_135 -#define AP_OPTICALFLOW_ADNS3080_PINS_RIGHT AP_OPTICALFLOW_ROTATION_YAW_90 -#define AP_OPTICALFLOW_ADNS3080_PINS_BACK_RIGHT AP_OPTICALFLOW_ROTATION_YAW_45 -#define AP_OPTICALFLOW_ADNS3080_PINS_BACK AP_OPTICALFLOW_ROTATION_NONE -#define AP_OPTICALFLOW_ADNS3080_PINS_BACK_LEFT AP_OPTICALFLOW_ROTATION_YAW_315 -#define AP_OPTICALFLOW_ADNS3080_PINS_LEFT AP_OPTICALFLOW_ROTATION_YAW_270 -#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_LEFT AP_OPTICALFLOW_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 - -// 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_CHIP_SELECT 34 // PC3 - #define ADNS3080_RESET 0 // reset pin is unattached by default -#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_CHIP_SELECT 10 //SS - #define ADNS3080_RESET 9 //RESET -#endif - -// 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 : 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); - 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(); +#ifndef AP_OPTICALFLOW_ADNS3080_H +#define AP_OPTICALFLOW_ADNS3080_H + +#include "AP_OpticalFlow.h" + +// orientations for ADNS3080 sensor +#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD AP_OPTICALFLOW_ROTATION_YAW_180 +#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_RIGHT AP_OPTICALFLOW_ROTATION_YAW_135 +#define AP_OPTICALFLOW_ADNS3080_PINS_RIGHT AP_OPTICALFLOW_ROTATION_YAW_90 +#define AP_OPTICALFLOW_ADNS3080_PINS_BACK_RIGHT AP_OPTICALFLOW_ROTATION_YAW_45 +#define AP_OPTICALFLOW_ADNS3080_PINS_BACK AP_OPTICALFLOW_ROTATION_NONE +#define AP_OPTICALFLOW_ADNS3080_PINS_BACK_LEFT AP_OPTICALFLOW_ROTATION_YAW_315 +#define AP_OPTICALFLOW_ADNS3080_PINS_LEFT AP_OPTICALFLOW_ROTATION_YAW_270 +#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_LEFT AP_OPTICALFLOW_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 + +// 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_CHIP_SELECT 34 // PC3 + #define ADNS3080_RESET 0 // reset pin is unattached by default +#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_CHIP_SELECT 10 //SS + #define ADNS3080_RESET 9 //RESET +#endif + +// 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 : 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); + 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 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 -}; - -#endif +}; + +#endif