Line data    Source code

       1             : #include "AC_PrecLand_config.h"
       2             : 
       3             : #if AC_PRECLAND_IRLOCK_ENABLED
       4             : 
       5             : #include "AC_PrecLand_IRLock.h"
       6             : #include <AP_HAL/AP_HAL.h>
       7             : 
       8             : // Constructor
       9           2 : AC_PrecLand_IRLock::AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state)
      10             :     : AC_PrecLand_Backend(frontend, state),
      11           2 :       irlock()
      12             : {
      13           2 : }
      14             : 
      15             : // init - perform initialisation of this backend
      16           2 : void AC_PrecLand_IRLock::init()
      17             : {
      18           2 :     irlock.init(get_bus());
      19           2 : }
      20             : 
      21             : // update - give chance to driver to get updates from sensor
      22       36450 : void AC_PrecLand_IRLock::update()
      23             : {
      24             :     // update health
      25       36450 :     _state.healthy = irlock.healthy();
      26             :     
      27             :     // get new sensor data
      28       36450 :     irlock.update();
      29             :     
      30       36450 :     if (irlock.num_targets() > 0 && irlock.last_update_ms() != _los_meas.time_ms) {
      31        3172 :         irlock.get_unit_vector_body(_los_meas.vec_unit);
      32        3172 :         _los_meas.frame = AC_PrecLand::VectorFrame::BODY_FRD;
      33        3172 :         _los_meas.valid = true;
      34        3172 :         _los_meas.time_ms = irlock.last_update_ms();
      35             :     }
      36       36450 :     _los_meas.valid = _los_meas.valid && AP_HAL::millis() - _los_meas.time_ms <= 1000;
      37       36450 : }
      38             : 
      39             : #endif // AC_PRECLAND_IRLOCK_ENABLED