diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_Onboard.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_Onboard.cpp index b4d8be7bbc..783032f8f6 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_Onboard.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_Onboard.cpp @@ -37,9 +37,7 @@ AP_OpticalFlow_Onboard::AP_OpticalFlow_Onboard(OpticalFlow &_frontend) : void AP_OpticalFlow_Onboard::init(void) { /* register callback to get gyro data */ - hal.opticalflow->init( - FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_Onboard::_get_gyro, - void, float&, float&, float&)); + hal.opticalflow->init(); } void AP_OpticalFlow_Onboard::update() @@ -102,13 +100,4 @@ void AP_OpticalFlow_Onboard::update() #endif } -void AP_OpticalFlow_Onboard::_get_gyro(float &rate_x, float &rate_y, - float &rate_z) -{ - Vector3f rates = get_ahrs().get_gyro(); - rate_x = rates.x; - rate_y = rates.y; - rate_z = rates.z; -} - #endif diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_Onboard.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_Onboard.h index ea6ccee5e2..30487b87d1 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_Onboard.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_Onboard.h @@ -28,6 +28,5 @@ public: void init(void); void update(void); private: - void _get_gyro(float&, float&, float&); uint32_t _last_read_ms; };