AC_PrecLand: further refactor irlock backend

Author: jschall

libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp

@@ -10,7 +10,9 @@ extern const AP_HAL::HAL& hal;
 // Constructor
 AC_PrecLand_IRLock::AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state)
     : AC_PrecLand_Backend(frontend, state),
-      irlock()
+      irlock(),
+      _have_los_meas(false),
+      _los_meas_time_ms(0)
 {
 }
 
@@ -28,26 +30,33 @@ void AC_PrecLand_IRLock::update()
 
     // get new sensor data
     irlock.update();
+    
+    if (irlock.num_targets() > 0 && irlock.last_update_ms() != _los_meas_time_ms) {
+        irlock.get_unit_vector_body(_los_meas_body);
+        _have_los_meas = true;
+        _los_meas_time_ms = irlock.last_update_ms();
+    }
+    _have_los_meas = _have_los_meas && AP_HAL::millis()-_los_meas_time_ms <= 1000;
 }
 
 // provides a unit vector towards the target in body frame
 //  returns same as have_los_meas()
 bool AC_PrecLand_IRLock::get_los_body(Vector3f& ret) {
     if (have_los_meas()) {
-        irlock.get_unit_vector_body(ret);
+        ret = _los_meas_body;
         return true;
     }
     return false;
 }
 
 // returns system time in milliseconds of last los measurement
 uint32_t AC_PrecLand_IRLock::los_meas_time_ms() {
-    return irlock.last_update_ms();
+    return _los_meas_time_ms;
 }
 
 // return true if there is a valid los measurement available
 bool AC_PrecLand_IRLock::have_los_meas() {
-    return irlock.num_targets() > 0;
+    return _have_los_meas;
 }
 
 #endif // PX4

libraries/AC_PrecLand/AC_PrecLand_IRLock.h

@@ -42,6 +42,9 @@ class AC_PrecLand_IRLock : public AC_PrecLand_Backend
 
 private:
     AP_IRLock_PX4 irlock;
-
+    
+    Vector3f            _los_meas_body;         // unit vector in body frame pointing towards target
+    bool                _have_los_meas;         // true if there is a valid measurement from the camera
+    uint32_t            _los_meas_time_ms;      // system time in milliseconds when los was measured
 };
 #endif