OptFlow_PX4: check healthy before updating

2014-10-15 16:27:19 +09:00 · 2014-10-15 16:27:19 +09:00 · a476a914b0
parent f504ea7b30
commit a476a914b0
1 changed files with 5 additions and 0 deletions
--- a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp
+++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp
@ -54,6 +54,11 @@ void AP_OpticalFlow_PX4::init(void)
 // update - read latest values from sensor and fill in x,y and totals.
 void AP_OpticalFlow_PX4::update(void)
 {
+    // return immediately if not healthy
+    if (!_flags.healthy) {
+        return;
+    }
+
    struct optical_flow_s report;
    while (::read(_fd, &report, sizeof(optical_flow_s)) == sizeof(optical_flow_s) && report.timestamp != _last_timestamp) {
        _device_id = report.sensor_id;