Line data    Source code

       1             : #include <AP_HAL/AP_HAL.h>
       2             : #include "AC_PrecLand_SITL.h"
       3             : 
       4             : #if AC_PRECLAND_SITL_ENABLED
       5             : 
       6             : #include "AP_AHRS/AP_AHRS.h"
       7             : // init - perform initialisation of this backend
       8           9 : void AC_PrecLand_SITL::init()
       9             : {
      10           9 :     _sitl = AP::sitl();
      11           9 : }
      12             : 
      13             : // update - give chance to driver to get updates from sensor
      14      205019 : void AC_PrecLand_SITL::update()
      15             : {
      16      205019 :     _state.healthy = _sitl->precland_sim.healthy();
      17             : 
      18      205019 :     if (_state.healthy && _sitl->precland_sim.last_update_ms() != _los_meas.time_ms) {
      19       23046 :         const Vector3d position = _sitl->precland_sim.get_target_position();
      20       23046 :         const Matrix3d body_to_ned = AP::ahrs().get_rotation_body_to_ned().todouble();
      21       23046 :         _los_meas.vec_unit =  body_to_ned.mul_transpose(-position).tofloat();
      22       23046 :         _distance_to_target = _sitl->precland_sim.option_enabled(SITL::SIM_Precland::Option::ENABLE_TARGET_DISTANCE) ? _los_meas.vec_unit.length() : 0.0f;
      23       23046 :         _los_meas.vec_unit /= _los_meas.vec_unit.length();
      24       23046 :         _los_meas.frame = AC_PrecLand::VectorFrame::BODY_FRD;
      25             : 
      26       23046 :         if (_frontend._orient != Rotation::ROTATION_PITCH_270) {
      27             :             // rotate body frame vector based on orientation
      28             :             // this is done to have homogeneity among backends
      29             :             // frontend rotates it back to get correct body frame vector
      30        7252 :             _los_meas.vec_unit.rotate_inverse(_frontend._orient);
      31        7252 :             _los_meas.vec_unit.rotate_inverse(ROTATION_PITCH_90);
      32             :         }
      33             : 
      34       23046 :         _los_meas.valid = true;
      35       23046 :         _los_meas.time_ms = _sitl->precland_sim.last_update_ms();
      36             :     } else {
      37      181973 :         _los_meas.valid = false;
      38             :     }
      39             : 
      40      205019 :     _los_meas.valid = _los_meas.valid && AP_HAL::millis() - _los_meas.time_ms <= 1000;
      41      205019 : }
      42             : 
      43             : #endif  // AC_PRECLAND_SITL_ENABLED