From 5d61e9289cfbf97d0823eae85adc9fa8f7f06cfb Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 16 Sep 2011 19:24:57 -0700 Subject: [PATCH] added sin and cos yaw from DCM, renamed "get" function because it doesn't return a value. --- libraries/AP_OpticalFlow/AP_OpticalFlow.cpp | 86 ++++++------ libraries/AP_OpticalFlow/AP_OpticalFlow.h | 22 ++-- .../AP_OpticalFlow_ADNS3080.cpp | 122 +++++++++--------- .../AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h | 33 ++--- 4 files changed, 140 insertions(+), 123 deletions(-) diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index 67d2821ac0..2613acae3e 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp @@ -12,15 +12,11 @@ #define FORTYFIVE_DEGREES 0.78539816 -AP_OpticalFlow::AP_OpticalFlow() : raw_dx(0),raw_dy(0),x(0),y(0),surface_quality(0),dx(0),dy(0),last_update(0),field_of_view(1),scaler(0),num_pixels(0) -{ -} - // 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) -bool +bool AP_OpticalFlow::init(bool initCommAPI) { - _orientation_matrix = Matrix3f(1,0,0,0,1,0,0,0,1); + _orientation_matrix = Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1); update_conversion_factors(); return true; // just return true by default } @@ -38,28 +34,28 @@ int AP_OpticalFlow::read() } // reads a value from the sensor (will be sensor specific) -byte +byte AP_OpticalFlow::read_register(byte address) { } // writes a value to one of the sensor's register (will be sensor specific) -void +void AP_OpticalFlow::write_register(byte address, byte value) { } // rotate raw values to arrive at final x,y,dx and dy values -void +void AP_OpticalFlow::apply_orientation_matrix() { Vector3f rot_vector; - + // next rotate dx and dy - rot_vector = _orientation_matrix * Vector3f(raw_dx,raw_dy,0); + rot_vector = _orientation_matrix * Vector3f(raw_dx, raw_dy, 0); dx = rot_vector.x; dy = rot_vector.y; - + // add rotated values to totals (perhaps this is pointless as we need to take into account yaw, roll, pitch) x += dx; y += dy; @@ -69,30 +65,26 @@ AP_OpticalFlow::apply_orientation_matrix() 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) - radians_to_pixels = (num_pixels*scaler) / field_of_view; + 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) + // 0.00615 + radians_to_pixels = (num_pixels * scaler) / field_of_view; + // 162.99 } // updates internal lon and lat with estimation based on optical flow void -AP_OpticalFlow::get_position(float roll, float pitch, float yaw, float altitude) +AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float sin_yaw_y, float altitude) { - float diff_roll = roll - _last_roll; - float diff_pitch = pitch - _last_pitch; - float avg_altitude = (altitude + _last_altitude)/2; - //float exp_change_x, exp_change_y; - //float change_x, change_y; - //float x_cm, y_cm; - float cos_yaw = cos(yaw); - float sin_yaw = sin(yaw); - int i; - + float diff_roll = roll - _last_roll; + float diff_pitch = pitch - _last_pitch; + // only update position if surface quality is good and angle is not over 45 degrees if( surface_quality >= 10 && fabs(roll) <= FORTYFIVE_DEGREES && fabs(pitch) <= FORTYFIVE_DEGREES ) { + altitude = max(altitude, 0); // calculate expected x,y diff due to roll and pitch change exp_change_x = diff_roll * radians_to_pixels; exp_change_y = -diff_pitch * radians_to_pixels; - + // real estimated raw change from mouse change_x = dx - exp_change_x; change_y = dy - exp_change_y; @@ -100,14 +92,36 @@ AP_OpticalFlow::get_position(float roll, float pitch, float yaw, float altitude) // convert raw change to horizontal movement in cm x_cm = -change_x * avg_altitude * conv_factor; // perhaps this altitude should actually be the distance to the ground? i.e. if we are very rolled over it should be longer? y_cm = -change_y * avg_altitude * conv_factor; // for example if you are leaned over at 45 deg the ground will appear farther away and motion from opt flow sensor will be less - - // use yaw to convert x and y into lon and lat - lat += y_cm * cos_yaw - x_cm * sin_yaw; - lng += x_cm * cos_yaw + y_cm * sin_yaw; + + + vlon = x_cm * sin_yaw_y - y_cm * cos_yaw_x; + vlat = y_cm * sin_yaw_y - x_cm * cos_yaw_x; } - - // capture roll and pitch for next iteration - _last_roll = roll; - _last_pitch = pitch; - _last_altitude = altitude; -} \ No newline at end of file +} + + +/* +{ + // only update position if surface quality is good and angle is not over 45 degrees + if( surface_quality >= 10 && fabs(_dcm->roll) <= FORTYFIVE_DEGREES && fabs(_dcm->pitch) <= FORTYFIVE_DEGREES ) { + altitude = max(altitude, 0); + Vector3f omega = _dcm->get_gyro(); + + // calculate expected x,y diff due to roll and pitch change + float exp_change_x = omega.x * radians_to_pixels; + float exp_change_y = -omega.y * radians_to_pixels; + + // real estimated raw change from mouse + float change_x = dx - exp_change_x; + float change_y = dy - exp_change_y; + + // convert raw change to horizontal movement in cm + float x_cm = -change_x * altitude * conv_factor; // perhaps this altitude should actually be the distance to the ground? i.e. if we are very rolled over it should be longer? + float y_cm = -change_y * altitude * conv_factor; // for example if you are leaned over at 45 deg the ground will appear farther away and motion from opt flow sensor will be less + + vlon = (float)x_cm * sin_yaw_y - (float)y_cm * cos_yaw_x; + vlat = (float)y_cm * sin_yaw_y - (float)x_cm * cos_yaw_x; + } +} + +*/ \ No newline at end of file diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h index dec1d0bc28..79f1bb215f 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.h @@ -15,11 +15,11 @@ read : reads latest value from OpticalFlow and stores values in x,y, surface_quality parameter read_register() : reads a value from the sensor (will be sensor specific) write_register() : writes a value to one of the sensor's register (will be sensor specific) - */ -#include "WProgram.h" #include +#include +#include // return value definition #define OPTICALFLOW_FAIL 0 @@ -37,12 +37,12 @@ class AP_OpticalFlow { - public: + public: int raw_dx, raw_dy; // raw sensor change in x and y position (i.e. unrotated) int surface_quality; // image quality (below 15 you really can't trust the x,y values returned) int x,y; // total x,y position int dx,dy; // rotated change in x and y position - float lng, lat; // position as offsets from original position + float vlon, vlat; // position as offsets from original position unsigned long 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 @@ -50,17 +50,19 @@ class AP_OpticalFlow // temp variables - delete me! float exp_change_x, exp_change_y; float change_x, change_y; - float x_cm, y_cm; - public: - AP_OpticalFlow(); // Constructor + float x_cm, y_cm; + + //AP_OpticalFlow(AP_DCM *dcm); virtual bool init(bool initCommAPI = 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) virtual byte read_register(byte address); virtual void write_register(byte address, byte value); virtual void set_orientation(const Matrix3f &rotation_matrix); // Rotation vector to transform sensor readings to the body frame. virtual void set_field_of_view(const float fov) { field_of_view = fov; update_conversion_factors(); }; // sets field of view of sensor virtual int read(); // read latest values from sensor and fill in x,y and totals - virtual void get_position(float roll, float pitch, float yaw, float altitude); // updates internal lon and lat with estimation based on optical flow - + virtual void update_position(float roll, float pitch, float cos_yaw_x, float sin_yaw_y, float altitude); // updates internal lon and lat with estimation based on optical flow + + + //protected: Matrix3f _orientation_matrix; float conv_factor; // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude) @@ -70,6 +72,4 @@ class AP_OpticalFlow virtual void update_conversion_factors(); }; -#include "AP_OpticalFlow_ADNS3080.h" - #endif diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp index 15a2c35cb7..f42d1acb9c 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp @@ -33,18 +33,18 @@ union NumericIntType { int intValue; unsigned int uintValue; - byte byteValue[2]; + byte byteValue[2]; }; - -// Constructors //////////////////////////////////////////////////////////////// + // Constructors //////////////////////////////////////////////////////////////// AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080() { - num_pixels = ADNS3080_PIXELS_X; - field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV; + num_pixels = ADNS3080_PIXELS_X; + field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV; scaler = AP_OPTICALFLOW_ADNS3080_SCALER; } + // 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) @@ -52,7 +52,7 @@ bool AP_OpticalFlow_ADNS3080::init(bool initCommAPI) { int retry = 0; - + pinMode(AP_SPI_DATAOUT,OUTPUT); pinMode(AP_SPI_DATAIN,INPUT); pinMode(AP_SPI_CLOCK,OUTPUT); @@ -63,12 +63,12 @@ AP_OpticalFlow_ADNS3080::init(bool initCommAPI) // reset the device reset(); - + // start the SPI library: if( initCommAPI ) { SPI.begin(); } - + // check the sensor is functioning if( retry < 3 ) { if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) @@ -84,16 +84,16 @@ AP_OpticalFlow_ADNS3080::init(bool initCommAPI) // byte AP_OpticalFlow_ADNS3080::backup_spi_settings() -{ +{ // store current spi values orig_spi_settings_spcr = SPCR & (DORD | CPOL | CPHA); orig_spi_settings_spsr = SPSR & SPI2X; - + // set the values that we need SPI.setBitOrder(MSBFIRST); SPI.setDataMode(SPI_MODE3); - SPI.setClockDivider(SPI_CLOCK_DIV8); // sensor running at 2Mhz. this is it's maximum speed - + SPI.setClockDivider(SPI_CLOCK_DIV8); // sensor running at 2Mhz. this is it's maximum speed + return orig_spi_settings_spcr; } @@ -102,7 +102,7 @@ byte AP_OpticalFlow_ADNS3080::restore_spi_settings() { byte temp; - + // restore SPSR temp = SPSR; temp &= ~SPI2X; @@ -114,35 +114,35 @@ AP_OpticalFlow_ADNS3080::restore_spi_settings() temp &= ~(DORD | CPOL | CPHA); // zero out the important bits temp |= orig_spi_settings_spcr; // restore important bits SPCR = temp; - + return temp; } // Read a register from the sensor byte -AP_OpticalFlow_ADNS3080::read_register(byte address) +AP_OpticalFlow_ADNS3080::read_register(byte address) { byte result = 0, junk = 0; backup_spi_settings(); - + // take the chip select low to select the device digitalWrite(ADNS3080_CHIP_SELECT, LOW); - + // send the device the register you want to read: junk = SPI.transfer(address); - + // small delay delayMicroseconds(50); - + // send a value of 0 to read the first byte returned: result = SPI.transfer(0x00); - + // take the chip select high to de-select: digitalWrite(ADNS3080_CHIP_SELECT, HIGH); - + restore_spi_settings(); - + return result; } @@ -152,24 +152,24 @@ AP_OpticalFlow_ADNS3080::write_register(byte address, byte value) { byte junk = 0; - backup_spi_settings(); - + backup_spi_settings(); + // take the chip select low to select the device digitalWrite(ADNS3080_CHIP_SELECT, LOW); - + // send register address junk = SPI.transfer(address | 0x80 ); - + // small delay - delayMicroseconds(50); - + delayMicroseconds(50); + // send data junk = SPI.transfer(value); - + // take the chip select high to de-select: digitalWrite(ADNS3080_CHIP_SELECT, HIGH); - - restore_spi_settings(); + + restore_spi_settings(); } // reset sensor by holding a pin high (or is it low?) for 10us. @@ -187,7 +187,7 @@ AP_OpticalFlow_ADNS3080::read() { surface_quality = (unsigned int)read_register(ADNS3080_SQUAL); delayMicroseconds(50); // small delay - + // check for movement, update x,y values if( (read_register(ADNS3080_MOTION) & 0x80) != 0 ) { raw_dx = ((char)read_register(ADNS3080_DELTA_X)); @@ -198,11 +198,11 @@ AP_OpticalFlow_ADNS3080::read() raw_dx = 0; raw_dy = 0; } - + last_update = millis(); - + apply_orientation_matrix(); - + return OPTICALFLOW_SUCCESS; } @@ -238,13 +238,13 @@ void AP_OpticalFlow_ADNS3080::set_resolution(int resolution) { byte regVal = read_register(ADNS3080_CONFIGURATION_BITS); - + if( resolution == ADNS3080_RESOLUTION_400 ) { regVal &= ~0x10; - }else if( resolution == ADNS3080_RESOLUTION_1200) { + }else if( resolution == ADNS3080_RESOLUTION_1200) { regVal |= 0x10; } - + delayMicroseconds(50); // small delay write_register(ADNS3080_CONFIGURATION_BITS, regVal); } @@ -273,7 +273,7 @@ AP_OpticalFlow_ADNS3080::set_frame_rate_auto(bool auto_frame_rate) delayMicroseconds(50); // small delay write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,0x1A); delayMicroseconds(50); // small delay - + // decide what value to update in extended config regVal = (regVal & ~0x01); }else{ @@ -287,7 +287,7 @@ AP_OpticalFlow_ADNS3080::set_frame_rate_auto(bool auto_frame_rate) unsigned int AP_OpticalFlow_ADNS3080::get_frame_period() { - NumericIntType aNum; + NumericIntType aNum; aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER); delayMicroseconds(50); // small delay aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER); @@ -300,11 +300,11 @@ AP_OpticalFlow_ADNS3080::set_frame_period(unsigned int period) { NumericIntType aNum; aNum.uintValue = period; - + // set frame rate to manual set_frame_rate_auto(false); delayMicroseconds(50); // small delay - + // set specific frame period write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]); delayMicroseconds(50); // small delay @@ -325,7 +325,7 @@ AP_OpticalFlow_ADNS3080::set_frame_rate(unsigned int rate) { unsigned long clockSpeed = ADNS3080_CLOCK_SPEED; unsigned int period = (unsigned int)(clockSpeed / (unsigned long)rate); - + set_frame_period(period); } @@ -349,11 +349,11 @@ AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool auto_shutter_speed) delayMicroseconds(50); // small delay if( auto_shutter_speed ) { // return shutter speed max to default - write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,0x8c); - delayMicroseconds(50); // small delay + write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,0x8c); + delayMicroseconds(50); // small delay write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,0x20); - delayMicroseconds(50); // small delay - + delayMicroseconds(50); // small delay + // determine value to put into extended config regVal = regVal & ~0x02; }else{ @@ -361,41 +361,41 @@ AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool auto_shutter_speed) regVal = regVal & ~0x02 | 0x02; } write_register(ADNS3080_EXTENDED_CONFIG, regVal); - delayMicroseconds(50); // small delay + delayMicroseconds(50); // small delay } // get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual unsigned int AP_OpticalFlow_ADNS3080::get_shutter_speed() { - NumericIntType aNum; + NumericIntType aNum; aNum.byteValue[1] = read_register(ADNS3080_SHUTTER_UPPER); delayMicroseconds(50); // small delay aNum.byteValue[0] = read_register(ADNS3080_SHUTTER_LOWER); return aNum.uintValue; } - - + + // set_shutter_speed_auto - set shutter speed to auto (true), or manual (false) unsigned int AP_OpticalFlow_ADNS3080::set_shutter_speed(unsigned int shutter_speed) { NumericIntType aNum; aNum.uintValue = shutter_speed; - + // set shutter speed to manual set_shutter_speed_auto(false); delayMicroseconds(50); // small delay - + // set specific shutter speed - write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]); - delayMicroseconds(50); // small delay + write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]); + delayMicroseconds(50); // small delay write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,aNum.byteValue[1]); delayMicroseconds(50); // small delay - + // larger delay delay(50); - + // need to update frame period to cause shutter value to take effect aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER); delayMicroseconds(50); // small delay @@ -427,13 +427,13 @@ AP_OpticalFlow_ADNS3080::print_pixel_data(Stream *serPort) bool isFirstPixel = true; byte regValue; byte 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 delayMicroseconds(1510); // min frame speed is 2000 frames/second so 1 frame = 500 nano seconds. so 500 x 3 + 10 = 1510 - + // display the pixel data for( i=0; iprintln(); } - + // hardware reset to restore sensor to normal operation reset(); } diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h index 41d233ebae..e5c296f186 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h @@ -1,8 +1,10 @@ #ifndef AP_OPTICALFLOW_ADNS3080_H #define AP_OPTICALFLOW_ADNS3080_H -#include -#include +//#include +//#include +//#include +//#include "AP_OpticalFlow.h" #include "AP_OpticalFlow.h" // orientations for ADNS3080 sensor @@ -29,10 +31,10 @@ #define ADNS3080_CHIP_SELECT 34 // PC3 #define ADNS3080_RESET 40 // PG1 was 35 / PC2 #else // normal arduino SPI pins...these need to be checked - #define AP_SPI_DATAIN 12 //MISO + #define AP_SPI_DATAIN 12 //MISO #define AP_SPI_DATAOUT 11 //MOSI #define AP_SPI_CLOCK 13 //SCK - #define ADNS3080_CHIP_SELECT 10 //SS + #define ADNS3080_CHIP_SELECT 10 //SS #define ADNS3080_RESET 9 //RESET #endif @@ -91,39 +93,40 @@ class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow // bytes to store SPI settings byte orig_spi_settings_spcr; byte orig_spi_settings_spsr; - + // save and restore SPI settings byte backup_spi_settings(); byte restore_spi_settings(); - + bool _motion; // true if there has been motion - + public: - AP_OpticalFlow_ADNS3080(); // Constructor + AP_OpticalFlow_ADNS3080(); + //AP_OpticalFlow_ADNS3080(); // Constructor bool init(bool initCommAPI = 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) byte read_register(byte address); void write_register(byte address, byte value); void reset(); // reset sensor by holding a pin high (or is it low?) for 10us. int read(); // read latest values from sensor and fill in x,y and totals, return OPTICALFLOW_SUCCESS on successful read - + // ADNS3080 specific features bool motion() { if( _motion ) { _motion = false; return true; }else{ return false; } } // return true if there has been motion since the last time this was called - + bool get_led_always_on(); // returns true if LED is always on, false if only on when required void set_led_always_on( bool alwaysOn ); // set parameter to true if you want LED always on, otherwise false for only when required - + int get_resolution(); // returns resolution (either 400 or 1200 counts per inch) void set_resolution(int resolution); // set parameter to 400 or 1200 counts per inch - + bool get_frame_rate_auto(); // get_frame_rate_auto - return true if frame rate is set to "auto", false if manual void set_frame_rate_auto(bool auto_frame_rate); // set_frame_rate_auto(bool) - set frame rate to auto (true), or manual (false) - unsigned int get_frame_period(); // get_frame_period - + unsigned int get_frame_period(); // get_frame_period - void set_frame_period(unsigned int period); unsigned int get_frame_rate(); void set_frame_rate(unsigned int rate); - + bool get_shutter_speed_auto(); // get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual void set_shutter_speed_auto(bool auto_shutter_speed); // set_shutter_speed_auto - set shutter speed to auto (true), or manual (false) @@ -131,7 +134,7 @@ class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow unsigned int set_shutter_speed(unsigned int shutter_speed); void clear_motion(); // will cause the x,y, dx, dy, and the sensor's motion registers to be cleared - + int print_pixel_data(Stream *serPort); // dumps a 30x30 image to the Serial port };