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

OptFlow: minor commenting and format changes

Browse Source
This commit is contained in:
Randy Mackay 2013-05-28 22:56:11 +09:00
parent c3d21a457f
commit 4ce4b7aef9
3 changed files with 48 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
libraries/AP_OpticalFlow/AP_OpticalFlow.h
View File

@ -22,64 +22,57 @@
#include <AP_Math.h>
// timer process runs at 1khz. 100 iterations = 10hz
#define AP_OPTICALFLOW_NUM_CALLS_FOR_10HZ 100
// timer process runs at 1khz. 50 iterations = 20hz
#define AP_OPTICALFLOW_NUM_CALLS_FOR_20HZ 50
// timer process runs at 1khz. 20 iterations = 50hz
#define AP_OPTICALFLOW_NUM_CALLS_FOR_50HZ 20
#define AP_OPTICALFLOW_NUM_CALLS_FOR_10HZ 100 // timer process runs at 1khz. 100 iterations = 10hz
#define AP_OPTICALFLOW_NUM_CALLS_FOR_20HZ 50 // timer process runs at 1khz. 50 iterations = 20hz
#define AP_OPTICALFLOW_NUM_CALLS_FOR_50HZ 20 // timer process runs at 1khz. 20 iterations = 50hz
class AP_OpticalFlow
{
public:
// raw sensor change in x and y position (i.e. unrotated)
int raw_dx, raw_dy;
// image quality (below 15 you really can't trust the x,y values returned)
uint8_t surface_quality;
// total x,y position
int x,y;
// rotated change in x and y position
int dx,dy;
// position as offsets from original position
float vlon, vlat;
// millis() time of last update
uint32_t last_update;
// field of view in Radians
float field_of_view;
// number returned from sensor when moved one pixel
float scaler;
// number of pixels of resolution in the sensor
int num_pixels;
// temp - delete me!
float exp_change_x, exp_change_y;
float change_x, change_y;
float x_cm, y_cm;
// constructor
AP_OpticalFlow() {
_sensor = this;
};
~AP_OpticalFlow() {
_sensor = NULL;
};
virtual bool init();
// 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 uint8_t read_register(uint8_t address);
virtual void write_register(uint8_t address, uint8_t value);
// Rotation vector to transform sensor readings to the body frame.
virtual void set_orientation(enum Rotation rotation);
// sets field of view of sensor
virtual void set_field_of_view(const float fov) {
field_of_view = fov; update_conversion_factors();
};
virtual void set_field_of_view(const float fov) { field_of_view = fov; update_conversion_factors(); };
// called by timer process to read sensor data from all attached sensors
static void read(uint32_t now);
// read latest values from sensor and fill in x,y and totals.
virtual void update(uint32_t now);
// updates internal lon and lat with estimation based on optical flow
virtual void update_position(float roll,
float pitch, float sin_yaw, float cos_yaw, float altitude);
virtual void update_position(float roll, float pitch, float sin_yaw, float cos_yaw, float altitude);
// public variables
int16_t raw_dx; // raw sensor change in x and y position (i.e. unrotated)
int16_t raw_dy; // raw sensor change in x and y position (i.e. unrotated)
uint8_t surface_quality; // image quality (below 15 you really can't trust the x,y values returned)
int16_t x,y; // total x,y position
int16_t dx,dy; // rotated change in x and y position
float vlon, vlat; // position as offsets from original position
uint32_t 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
int16_t num_pixels; // number of pixels of resolution in the sensor
// public variables for reporting purposes
float exp_change_x, exp_change_y; // expected change in x, y coordinates
float change_x, change_y; // actual change in x, y coordinates
float x_cm, y_cm; // x,y position in cm
protected:
// pointer to the last instantiated optical flow sensor. Will be turned

13
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp
View File

@ -46,17 +46,21 @@ AP_OpticalFlow_ADNS3080::init()
// suspend timer while we set-up SPI communication
hal.scheduler->suspend_timer_procs();
if( _reset_pin != 0)
// reset the device if the reset pin has been defined
if(_reset_pin != 0) {
hal.gpio->pinMode(_reset_pin, GPIO_OUTPUT);
}
// reset the device
reset();
// check 3 times for the sensor on standard SPI bus
// get pointer to the spi bus
_spi = hal.spi->device(AP_HAL::SPIDevice_ADNS3080_SPI0);
if (_spi == NULL) {
retvalue = false; goto finish;
}
// check 3 times for the sensor on standard SPI bus
while( retvalue == false && retry < 3 ) {
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) {
retvalue = true;
@ -65,11 +69,13 @@ AP_OpticalFlow_ADNS3080::init()
retry++;
}
// if not found, check 3 times on SPI3
// if not found, get pointer to the SPI3 bus
_spi = hal.spi->device(AP_HAL::SPIDevice_ADNS3080_SPI3);
if (_spi == NULL) {
retvalue = false; goto finish;
}
// check 3 times on SPI3
retry = 0;
while( retvalue == false && retry < 3 ) {
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) {
@ -77,6 +83,7 @@ AP_OpticalFlow_ADNS3080::init()
}
retry++;
}
// If we fail to find on SPI3, no connection available.
retvalue = false;
_spi = NULL;

9
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h
View File

@ -79,14 +79,21 @@
class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow
{
public:
// constructor
AP_OpticalFlow_ADNS3080(uint8_t reset_pin = 0);
// initialise the sensor
bool init();
uint8_t read_register(uint8_t address);
void write_register(uint8_t address, uint8_t value);
// reset sensor by holding a pin high (or is it low?) for 10us.
void reset();
// read latest values from sensor and fill in x,y and totals,
// return true on successful read
// returns true on successful read
void update(uint32_t now);
// ADNS3080 specific features