aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_OpticalFlow : Remove support for ADNS3080 sensor

Browse Source 
It is not compatible with the new interface and data fusion requirements
This commit is contained in:
priseborough 2014-10-31 23:51:16 +11:00 committed by Andrew Tridgell
parent 60527a023a
commit 0d774d301d
3 changed files with 0 additions and 384 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_OpticalFlow/AP_OpticalFlow.h
View File

@ -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"

266
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
* AP_OpticalFlow_ADNS3080.cpp - ADNS3080 OpticalFlow Library for
* Ardupilot Mega
* Code by Randy Mackay. DIYDrones.com
*
*/
#include <AP_HAL.h>
#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; i<ADNS3080_PIXELS_Y; i++) {
for (j=0; j<ADNS3080_PIXELS_X; j++) {
regValue = read_register(ADNS3080_FRAME_CAPTURE);
if (isFirstPixel && (regValue & 0x40) == 0) {
hal.console->println_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
}

117
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h
View File

@ -1,117 +0,0 @@
#ifndef __AP_OPTICALFLOW_ADNS3080_H__
#define __AP_OPTICALFLOW_ADNS3080_H__
#include <AP_HAL.h>
#include <AP_AHRS.h>
#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