diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp index dfd6ae870e..85c6251b08 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp @@ -190,11 +190,14 @@ AP_OpticalFlow_ADNS3080::reset() bool AP_OpticalFlow_ADNS3080::update() { + byte motion_reg; 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 ) { + motion_reg = read_register(ADNS3080_MOTION); + _overflow = ((motion_reg & 0x10) != 0); // check if we've had an overflow + if( (motion_reg & 0x80) != 0 ) { raw_dx = ((char)read_register(ADNS3080_DELTA_X)); delayMicroseconds(50); // small delay raw_dy = ((char)read_register(ADNS3080_DELTA_Y)); diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h index bd257c986f..cb5eb1bb3d 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h @@ -100,7 +100,8 @@ class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow public: int _cs_pin; // pin used for chip select int _reset_pin; // pin used for chip reset - bool _motion; // true if there has been motion + bool _motion; // true if there has been motion + bool _overflow; // true if the x or y data buffers overflowed public: AP_OpticalFlow_ADNS3080(int cs_pin = ADNS3080_CHIP_SELECT, int reset_pin = ADNS3080_RESET); diff --git a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde index fa369532ce..aec1a24bda 100644 --- a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde +++ b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde @@ -312,6 +312,10 @@ void display_motion() flowSensor.update(); flowSensor.update_position(0,0,0,1,100); + // check for errors + if( flowSensor._overflow ) + Serial.println("overflow!!"); + // x,y,squal //if( flowSensor.motion() || first_time ) { Serial.print("x/dx: ");