diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h index cc99e686a9..73a3628f71 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.h @@ -3,5 +3,4 @@ /// @file AP_OpticalFlow.h /// @brief Catch-all header that defines all supported optical flow classes. -#include "AP_OpticalFlow_ADNS3080.h" #include "AP_OpticalFlow_PX4.h" diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp deleted file mode 100644 index 2ace54d313..0000000000 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp +++ /dev/null @@ -1,266 +0,0 @@ -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ - -/* - * AP_OpticalFlow_ADNS3080.cpp - ADNS3080 OpticalFlow Library for - * Ardupilot Mega - * Code by Randy Mackay. DIYDrones.com - * - */ - -#include -#include "AP_OpticalFlow_ADNS3080.h" - -extern const AP_HAL::HAL& hal; - -// Public Methods ////////////////////////////////////////////////////////////// -// init - initialise sensor -// assumes SPI bus has been initialised but will attempt to initialise -// nonstandard SPI3 bus if required -void AP_OpticalFlow_ADNS3080::init() -{ - int8_t retry = 0; - _flags.healthy = false; - - // suspend timer while we set-up SPI communication - hal.scheduler->suspend_timer_procs(); - - // get pointer to the spi bus - _spi = hal.spi->device(AP_HAL::SPIDevice_ADNS3080_SPI0); - if (_spi != NULL) { - // check 3 times for the sensor on standard SPI bus - while (!_flags.healthy && retry < 3) { - if (read_register(ADNS3080_PRODUCT_ID) == 0x17) { - _flags.healthy = true; - _device_id = ADNS3080_PRODUCT_ID; - } - retry++; - } - } - - // if not yet found, get pointer to the SPI3 bus - if (!_flags.healthy) { - _spi = hal.spi->device(AP_HAL::SPIDevice_ADNS3080_SPI3); - if (_spi != NULL) { - // check 3 times on SPI3 - retry = 0; - while (!_flags.healthy && retry < 3) { - if (read_register(ADNS3080_PRODUCT_ID) == 0x17) { - _flags.healthy = true; - } - retry++; - } - } - } - - // configure the sensor - if (_flags.healthy) { - // set frame rate to manual - uint8_t regVal = read_register(ADNS3080_EXTENDED_CONFIG); - hal.scheduler->delay_microseconds(50); - regVal = (regVal & ~0x01) | 0x01; - write_register(ADNS3080_EXTENDED_CONFIG, regVal); - hal.scheduler->delay_microseconds(50); - - // set frame period to 12000 (0x2EE0) - write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,0xE0); - hal.scheduler->delay_microseconds(50); - write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,0x2E); - hal.scheduler->delay_microseconds(50); - - // set 1600 resolution bit - regVal = read_register(ADNS3080_CONFIGURATION_BITS); - hal.scheduler->delay_microseconds(50); - regVal |= 0x10; - write_register(ADNS3080_CONFIGURATION_BITS, regVal); - hal.scheduler->delay_microseconds(50); - - // update scalers - update_conversion_factors(); - - // register the global static read function to be called at 1khz - hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_OpticalFlow_ADNS3080::read)); - }else{ - // no connection available. - _spi = NULL; - } - - // resume timer - hal.scheduler->resume_timer_procs(); -} - -// Read a register from the sensor -uint8_t AP_OpticalFlow_ADNS3080::read_register(uint8_t address) -{ - AP_HAL::Semaphore *spi_sem; - - // check that we have an spi bus - if (_spi == NULL) { - return 0; - } - - // get spi bus semaphore - spi_sem = _spi->get_semaphore(); - - // try to get control of the spi bus - if (spi_sem == NULL || !spi_sem->take_nonblocking()) { - return 0; - } - - _spi->cs_assert(); - // send the device the register you want to read: - _spi->transfer(address); - hal.scheduler->delay_microseconds(50); - // send a value of 0 to read the first byte returned: - uint8_t result = _spi->transfer(0x00); - - _spi->cs_release(); - - // release the spi bus - spi_sem->give(); - - return result; -} - -// write a value to one of the sensor's registers -void AP_OpticalFlow_ADNS3080::write_register(uint8_t address, uint8_t value) -{ - AP_HAL::Semaphore *spi_sem; - - // check that we have an spi bus - if (_spi == NULL) { - return; - } - - // get spi bus semaphore - spi_sem = _spi->get_semaphore(); - - // try to get control of the spi bus - if (spi_sem == NULL || !spi_sem->take_nonblocking()) { - return; - } - - _spi->cs_assert(); - - // send register address - _spi->transfer(address | 0x80 ); - hal.scheduler->delay_microseconds(50); - // send data - _spi->transfer(value); - - _spi->cs_release(); - - // release the spi bus - spi_sem->give(); -} - -// read latest values from sensor and fill in x,y and totals -void AP_OpticalFlow_ADNS3080::update(void) -{ - uint8_t motion_reg; - int16_t raw_dx, raw_dy; // raw sensor change in x and y position (i.e. unrotated) - - // return immediately if not healthy - if (!_flags.healthy) { - return; - } - - _surface_quality = read_register(ADNS3080_SQUAL); - hal.scheduler->delay_microseconds(50); - - // check for movement, update x,y values - motion_reg = read_register(ADNS3080_MOTION); - if ((motion_reg & 0x80) != 0) { - _raw.x = ((int8_t)read_register(ADNS3080_DELTA_X)); - hal.scheduler->delay_microseconds(50); - _raw.y = ((int8_t)read_register(ADNS3080_DELTA_Y)); - }else{ - _raw.zero(); - } - - _last_update = hal.scheduler->millis(); -} - -// parent method called at 1khz by periodic process -// this is slowed down to 20hz and each instance's update function is called -// (only one instance is supported at the moment) -void AP_OpticalFlow_ADNS3080::read(void) -{ - _num_calls++; - - if (_num_calls >= AP_OPTICALFLOW_ADNS3080_NUM_CALLS_FOR_20HZ) { - _num_calls = 0; - update(); - } -}; - -// clear_motion - will cause the Delta_X, Delta_Y, and internal motion -// registers to be cleared -void AP_OpticalFlow_ADNS3080::clear_motion() -{ - // writing anything to this register will clear the sensor's motion - // registers - write_register(ADNS3080_MOTION_CLEAR,0xFF); - _raw.zero(); - _velocity.zero(); -} - -// get_pixel_data - captures an image from the sensor and stores it to the -// pixe_data array -void AP_OpticalFlow_ADNS3080::print_pixel_data() -{ - int16_t i,j; - bool isFirstPixel = true; - uint8_t regValue; - uint8_t 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 - // min frame speed is 2000 frames/second so 1 frame = 500 nano seconds. - // so 500 x 3 + 10 = 1510 - hal.scheduler->delay_microseconds(1510); - - // display the pixel data - for (i=0; iprintln_P( - PSTR("Optflow: failed to find first pixel")); - } - isFirstPixel = false; - pixelValue = ( regValue << 2 ); - hal.console->print(pixelValue,BASE_DEC); - if (j!= ADNS3080_PIXELS_X-1) - hal.console->print_P(PSTR(",")); - hal.scheduler->delay_microseconds(50); - } - hal.console->println(); - } -} - -// updates conversion factors that are dependent upon field_of_view -void AP_OpticalFlow_ADNS3080::update_conversion_factors() -{ - // multiply this number by altitude and pixel change to get horizontal - // move (in same units as altitude) - conv_factor = ((1.0f / (float)(ADNS3080_PIXELS_X * AP_OPTICALFLOW_ADNS3080_SCALER_1600)) - * 2.0f * tanf(AP_OPTICALFLOW_ADNS3080_08_FOV / 2.0f)); - // 0.00615 - radians_to_pixels = (ADNS3080_PIXELS_X * AP_OPTICALFLOW_ADNS3080_SCALER_1600) / AP_OPTICALFLOW_ADNS3080_08_FOV; - // 162.99 -} diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h deleted file mode 100644 index c95c888789..0000000000 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h +++ /dev/null @@ -1,117 +0,0 @@ -#ifndef __AP_OPTICALFLOW_ADNS3080_H__ -#define __AP_OPTICALFLOW_ADNS3080_H__ - -#include -#include -#include "AP_OpticalFlow.h" - -// timer process runs at 1khz. 50 iterations = 20hz -#define AP_OPTICALFLOW_ADNS3080_NUM_CALLS_FOR_20HZ 50 - -// 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.202458f // 11.6 degrees - -// scaler - value returned when sensor is moved equivalent of 1 pixel -#define AP_OPTICALFLOW_ADNS3080_SCALER_400 1.1f // when resolution set to 400 -#define AP_OPTICALFLOW_ADNS3080_SCALER_1600 4.4f // when resolution set to 1600 - -// 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 - -// Extended Configuration bits -#define ADNS3080_SERIALNPU_OFF 0x02 - -class AP_OpticalFlow_ADNS3080 : public OpticalFlow -{ -public: - - // constructor - AP_OpticalFlow_ADNS3080(const AP_AHRS& ahrs) : OpticalFlow(ahrs) - { - } - - // initialise the sensor - void init(); - - // read latest values from sensor and fill in x,y and totals, - // returns true on successful read - void update(void); - - // ADNS3080 specific features - - // called by timer process to read sensor data - void read(); - - uint8_t read_register(uint8_t address); - void write_register(uint8_t address, uint8_t value); - - // will cause the x,y, dx, dy, and the sensor's motion registers to - // be cleared - void clear_motion(); - - // print_pixel_data - dumps a 30x30 image to the Serial port - void print_pixel_data(); - -private: - - // update conversion factors based on field of view - void update_conversion_factors(); - - uint8_t _num_calls; // used to throttle read down to 20hz - - // SPI device - AP_HAL::SPIDeviceDriver *_spi; - - 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; - float _last_pitch; - float _last_altitude; -}; - -#endif