// 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)
voidAP_OpticalFlow::init(booleaninitCommAPI)
bool
AP_OpticalFlow::init(boolinitCommAPI)
{
_orientation_matrix=Matrix3f(1,0,0,0,1,0,0,0,1);
update_conversion_factors();
returntrue;// just return true by default
}
// set_orientation - Rotation vector to transform sensor readings to the body frame.
// 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)
intraw_dx,raw_dy;// raw sensor change in x and y position (i.e. unrotated)
intsurface_quality;// image quality (below 15 you really can't trust the x,y values returned)
intx,y;// total x,y position
intdx,dy;// rotated change in x and y position
unsignedlonglng,lat;// position
unsignedlonglast_update;// millis() time of last update
floatfield_of_view;// field of view in Radians
floatscaler;// number returned from sensor when moved one pixel
intnum_pixels;// number of pixels of resolution in the sensor
public:
AP_OpticalFlow();// Constructor
virtualvoidinit(booleaninitCommAPI=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)
virtualintread();// read latest values from sensor and fill in x,y and totals
virtualboolinit(boolinitCommAPI=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)
virtualbyteread_register(byteaddress);
virtualvoidwrite_register(byteaddress,bytevalue);
virtualvoidset_orientation(constMatrix3f&rotation_matrix);// Rotation vector to transform sensor readings to the body frame.
virtualvoidset_field_of_view(constfloatfov){field_of_view=fov;update_conversion_factors();};// sets field of view of sensor
virtualintread();// read latest values from sensor and fill in x,y and totals
virtualvoidget_position(floatroll,floatpitch,floatyaw,floataltitude);// updates internal lon and lat with estimation based on optical flow
private:
protected:
Matrix3f_orientation_matrix;
floatconv_factor;// multiply this number by altitude and pixel change to get horizontal move (in same units as altitude)
// 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)
@ -374,7 +400,8 @@ unsigned int AP_OpticalFlow_ADNS3080::set_shutter_speed(unsigned int shutter_spe
@@ -374,7 +400,8 @@ unsigned int AP_OpticalFlow_ADNS3080::set_shutter_speed(unsigned int shutter_spe
}
// clear_motion - will cause the Delta_X, Delta_Y, and internal motion registers to be cleared
voidAP_OpticalFlow_ADNS3080::clear_motion()
void
AP_OpticalFlow_ADNS3080::clear_motion()
{
write_register(ADNS3080_MOTION_CLEAR,0xFF);// writing anything to this register will clear the sensor's motion registers
// scaler - value returned when sensor is moved equivalent of 1 pixel
#define AP_OPTICALFLOW_ADNS3080_SCALER 10.5
// We use Serial Port 2 in SPI Mode
#define AP_SPI_DATAIN 50 // MISO // PB3
@ -71,27 +90,27 @@ class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow
@@ -71,27 +90,27 @@ class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow
bytebackup_spi_settings();
byterestore_spi_settings();
boolean_motion;// true if there has been motion
bool_motion;// true if there has been motion
public:
AP_OpticalFlow_ADNS3080();// Constructor
voidinit(booleaninitCommAPI=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)
boolinit(boolinitCommAPI=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)
byteread_register(byteaddress);
voidwrite_register(byteaddress,bytevalue);
voidreset();// reset sensor by holding a pin high (or is it low?) for 10us.
intread();// read latest values from sensor and fill in x,y and totals, return OPTICALFLOW_SUCCESS on successful read
// ADNS3080 specific features
booleanmotion(){if(_motion){_motion=false;returntrue;}else{returnfalse;}}// return true if there has been motion since the last time this was called
boolmotion(){if(_motion){_motion=false;returntrue;}else{returnfalse;}}// return true if there has been motion since the last time this was called
booleanget_led_always_on();// returns true if LED is always on, false if only on when required
voidset_led_always_on(booleanalwaysOn);// set parameter to true if you want LED always on, otherwise false for only when required
boolget_led_always_on();// returns true if LED is always on, false if only on when required
voidset_led_always_on(boolalwaysOn);// set parameter to true if you want LED always on, otherwise false for only when required
intget_resolution();// returns resolution (either 400 or 1200 counts per inch)
voidset_resolution(intresolution);// set parameter to 400 or 1200 counts per inch
booleanget_frame_rate_auto();// get_frame_rate_auto - return true if frame rate is set to "auto", false if manual
voidset_frame_rate_auto(booleanauto_frame_rate);// set_frame_rate_auto(boolean) - set frame rate to auto (true), or manual (false)
boolget_frame_rate_auto();// get_frame_rate_auto - return true if frame rate is set to "auto", false if manual
voidset_frame_rate_auto(boolauto_frame_rate);// set_frame_rate_auto(bool) - set frame rate to auto (true), or manual (false)
rmackay9@yahoo.com
14 years ago