diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index 1aeed584c0..06323d6c2f 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp @@ -18,16 +18,16 @@ AP_OpticalFlow* AP_OpticalFlow::_sensor = NULL; // pointer to the last instanti bool AP_OpticalFlow::init(bool initCommAPI) { - _orientation_matrix = Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1); - update_conversion_factors(); - return true; // just return true by default + _orientation = ROTATION_NONE; + update_conversion_factors(); + return true; // just return true by default } // set_orientation - Rotation vector to transform sensor readings to the body frame. void -AP_OpticalFlow::set_orientation(const Matrix3f &rotation_matrix) +AP_OpticalFlow::set_orientation(enum Rotation rotation) { - _orientation_matrix = rotation_matrix; + _orientation = rotation; } // read value from the sensor. Should be overridden by derived class @@ -54,10 +54,12 @@ AP_OpticalFlow::write_register(byte address, byte value) void AP_OpticalFlow::apply_orientation_matrix() { - Vector3f rot_vector; + Vector3f rot_vector; + rot_vector(raw_dx, raw_dy, 0); // next rotate dx and dy - rot_vector = _orientation_matrix * Vector3f(raw_dx, raw_dy, 0); + rot_vector.rotate(_orientation); + dx = rot_vector.x; dy = rot_vector.y; @@ -108,4 +110,4 @@ AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float _last_altitude = altitude; _last_roll = roll; _last_pitch = pitch; -} \ No newline at end of file +} diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h index 5e6657fefe..187499b57e 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.h @@ -21,16 +21,6 @@ #include #include -// standard rotation matrices -#define AP_OPTICALFLOW_ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1) -#define AP_OPTICALFLOW_ROTATION_YAW_45 Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1) -#define AP_OPTICALFLOW_ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1) -#define AP_OPTICALFLOW_ROTATION_YAW_135 Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1) -#define AP_OPTICALFLOW_ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1) -#define AP_OPTICALFLOW_ROTATION_YAW_225 Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1) -#define AP_OPTICALFLOW_ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1) -#define AP_OPTICALFLOW_ROTATION_YAW_315 Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1) - class AP_OpticalFlow { public: @@ -53,7 +43,7 @@ class AP_OpticalFlow 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_orientation(enum Rotation rotation); // 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 static void read(uint32_t ) { if( _sensor != NULL ) _sensor->update(); }; // call to update all attached sensors virtual bool update(); // read latest values from sensor and fill in x,y and totals. returns true on success @@ -61,7 +51,7 @@ class AP_OpticalFlow protected: static AP_OpticalFlow *_sensor; // pointer to the last instantiated optical flow sensor. Will be turned into a table if we ever add support for more than one sensor - Matrix3f _orientation_matrix; + enum Rotation _orientation; 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, _last_pitch, _last_altitude; diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h index eee31c5eb8..fc998e139f 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h @@ -4,14 +4,14 @@ #include "AP_OpticalFlow.h" // orientations for ADNS3080 sensor -#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD AP_OPTICALFLOW_ROTATION_YAW_180 -#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_RIGHT AP_OPTICALFLOW_ROTATION_YAW_135 -#define AP_OPTICALFLOW_ADNS3080_PINS_RIGHT AP_OPTICALFLOW_ROTATION_YAW_90 -#define AP_OPTICALFLOW_ADNS3080_PINS_BACK_RIGHT AP_OPTICALFLOW_ROTATION_YAW_45 -#define AP_OPTICALFLOW_ADNS3080_PINS_BACK AP_OPTICALFLOW_ROTATION_NONE -#define AP_OPTICALFLOW_ADNS3080_PINS_BACK_LEFT AP_OPTICALFLOW_ROTATION_YAW_315 -#define AP_OPTICALFLOW_ADNS3080_PINS_LEFT AP_OPTICALFLOW_ROTATION_YAW_270 -#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_LEFT AP_OPTICALFLOW_ROTATION_YAW_225 +#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.202458 // 11.6 degrees