Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-31 04:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

OptFlow: reorganise ADNS3080 to simplified interface

Browse Source
This commit is contained in:
Randy Mackay 2014-07-12 11:28:00 +09:00
parent b64f9ed964
commit 3c4be75487
2 changed files with 22 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp
View File

@ -25,12 +25,6 @@
extern const AP_HAL::HAL& hal;
// Constructors ////////////////////////////////////////////////////////////////
AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080()
{
field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV;
}
// Public Methods //////////////////////////////////////////////////////////////
// init - initialise sensor
// assumes SPI bus has been initialised but will attempt to initialise
@ -50,6 +44,7 @@ void AP_OpticalFlow_ADNS3080::init()
while (!_flags.healthy && retry < 3) {
if (read_register(ADNS3080_PRODUCT_ID) == 0x17) {
_flags.healthy = true;
_device_id = ADNS3080_PRODUCT_ID;
}
retry++;
}
@ -176,28 +171,20 @@ 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)
surface_quality = read_register(ADNS3080_SQUAL);
_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_dx = ((int8_t)read_register(ADNS3080_DELTA_X));
_raw.x = ((int8_t)read_register(ADNS3080_DELTA_X));
hal.scheduler->delay_microseconds(50);
raw_dy = ((int8_t)read_register(ADNS3080_DELTA_Y));
_raw.y = ((int8_t)read_register(ADNS3080_DELTA_Y));
}else{
raw_dx = 0;
raw_dy = 0;
_raw.zero();
}
last_update = hal.scheduler->millis();
Vector3f rot_vector(raw_dx, raw_dy, 0);
// rotate dx and dy
rot_vector.rotate(_orientation);
dx = rot_vector.x;
dy = rot_vector.y;
_last_update = hal.scheduler->millis();
}
// parent method called at 1khz by periodic process
@ -219,11 +206,9 @@ void AP_OpticalFlow_ADNS3080::clear_motion()
{
// writing anything to this register will clear the sensor's motion
// registers
write_register(ADNS3080_MOTION_CLEAR,0xFF);
x_cm = 0;
y_cm = 0;
dx = 0;
dy = 0;
write_register(ADNS3080_MOTION_CLEAR,0xFF);
_raw.zero();
_velocity.zero();
}
// get_pixel_data - captures an image from the sensor and stores it to the
@ -268,8 +253,8 @@ 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(field_of_view / 2.0f));
* 2.0f * tanf(AP_OPTICALFLOW_ADNS3080_08_FOV / 2.0f));
// 0.00615
radians_to_pixels = (ADNS3080_PIXELS_X * AP_OPTICALFLOW_ADNS3080_SCALER_1600) / field_of_view;
radians_to_pixels = (ADNS3080_PIXELS_X * AP_OPTICALFLOW_ADNS3080_SCALER_1600) / AP_OPTICALFLOW_ADNS3080_08_FOV;
// 162.99
}

13
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h
View File

@ -2,6 +2,7 @@
#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
@ -65,12 +66,14 @@
// Extended Configuration bits
#define ADNS3080_SERIALNPU_OFF 0x02
class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow
class AP_OpticalFlow_ADNS3080 : public OpticalFlow
{
public:
// constructor
AP_OpticalFlow_ADNS3080();
AP_OpticalFlow_ADNS3080(const AP_AHRS& ahrs) : OpticalFlow(ahrs)
{
}
// initialise the sensor
void init();
@ -103,6 +106,12 @@ private:
// 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