Line data    Source code

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