diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_CXOF.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_CXOF.cpp index 6eca69f434..60733796f5 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_CXOF.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_CXOF.cpp @@ -97,6 +97,7 @@ void AP_OpticalFlow_CXOF::update(void) } // record gyro values as long as they are being used + // the sanity check of dt below ensures old gyro values are not used if (gyro_sum_count < 1000) { const Vector3f& gyro = AP::ahrs_navekf().get_gyro(); gyro_sum.x += gyro.x; @@ -171,7 +172,8 @@ void AP_OpticalFlow_CXOF::update(void) float dt = (this_frame_us - last_frame_us) * 1.0e-6; last_frame_us = this_frame_us; - if ((dt > 0.0f) && (dt < CXOF_TIMEOUT_SEC)) { + // sanity check dt + if (is_positive(dt) && (dt < CXOF_TIMEOUT_SEC)) { // calculate flow values const Vector2f flowScaler = _flowScaler(); float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x; diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_CXOF.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_CXOF.h index df4ebeabcd..c34ed5f926 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_CXOF.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_CXOF.h @@ -20,10 +20,10 @@ public: private: - AP_HAL::UARTDriver *uart = nullptr; // uart connected to flow sensor + AP_HAL::UARTDriver *uart; // uart connected to flow sensor uint64_t last_frame_us; // system time of last message from flow sensor uint8_t buf[10]; // buff of characters received from flow sensor - uint8_t buf_len = 0; // number of characters in buffer + uint8_t buf_len; // number of characters in buffer Vector2f gyro_sum; // sum of gyro sensor values since last frame from flow sensor - uint16_t gyro_sum_count = 0; // number of gyro sensor values in sum + uint16_t gyro_sum_count; // number of gyro sensor values in sum };