From f504ea7b30699c7ddceef9e340d215712dc27da6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 15 Oct 2014 16:27:06 +0900 Subject: [PATCH] OptFlow_ADNS3080: check healthy before updating --- libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp index 66079d1375..2ace54d313 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp @@ -171,6 +171,12 @@ void AP_OpticalFlow_ADNS3080::update(void) { uint8_t motion_reg; int16_t raw_dx, raw_dy; // raw sensor change in x and y position (i.e. unrotated) + + // return immediately if not healthy + if (!_flags.healthy) { + return; + } + _surface_quality = read_register(ADNS3080_SQUAL); hal.scheduler->delay_microseconds(50);