Line data    Source code

       1             : #include "AC_PrecLand_config.h"
       2             : 
       3             : #if AC_PRECLAND_MAVLINK_ENABLED
       4             : 
       5             : #include "AC_PrecLand_MAVLink.h"
       6             : #include <AP_HAL/AP_HAL.h>
       7             : #include <GCS_MAVLink/GCS.h>
       8             : 
       9             : // perform any required initialisation of backend
      10           2 : void AC_PrecLand_MAVLink::init()
      11             : {
      12             :     // set healthy
      13           2 :     _state.healthy = true;
      14           2 : }
      15             : 
      16             : // retrieve updates from sensor
      17       96275 : void AC_PrecLand_MAVLink::update()
      18             : {
      19       96275 :     _los_meas.valid = _los_meas.valid && AP_HAL::millis() - _los_meas.time_ms <= 1000;
      20       96275 : }
      21             : 
      22         671 : void AC_PrecLand_MAVLink::handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms)
      23             : {
      24             :     // check frame is supported
      25         671 :     if (packet.frame != MAV_FRAME_BODY_FRD && packet.frame != MAV_FRAME_LOCAL_FRD) {
      26           0 :         if (!_wrong_frame_msg_sent) {
      27           0 :             _wrong_frame_msg_sent = true;
      28           0 :             GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Plnd: Frame not supported");
      29             :         }
      30           0 :         return;
      31             :     }
      32             : 
      33         671 :     if (packet.position_valid == 1) {
      34           0 :         if (packet.distance > 0) {
      35           0 :             _los_meas.vec_unit = Vector3f{packet.x, packet.y, packet.z};
      36           0 :             _los_meas.vec_unit /= packet.distance;
      37           0 :             _los_meas.frame = (packet.frame == MAV_FRAME_BODY_FRD) ? AC_PrecLand::VectorFrame::BODY_FRD : AC_PrecLand::VectorFrame::LOCAL_FRD;
      38             :         } else {
      39             :             // distance to target must be positive
      40           0 :             return;
      41             :         }
      42             :     } else {
      43             :         // compute unit vector towards target
      44         671 :         _los_meas.vec_unit = Vector3f{-tanf(packet.angle_y), tanf(packet.angle_x), 1.0f};
      45         671 :         _los_meas.vec_unit /= _los_meas.vec_unit.length();
      46         671 :         _los_meas.frame = (packet.frame == MAV_FRAME_BODY_FRD) ? AC_PrecLand::VectorFrame::BODY_FRD : AC_PrecLand::VectorFrame::LOCAL_FRD;
      47             :     }
      48             : 
      49         671 :     _distance_to_target = MAX(0, packet.distance);
      50         671 :     _los_meas.time_ms = timestamp_ms;
      51         671 :     _los_meas.valid = true;
      52             : }
      53             : 
      54             : #endif // AC_PRECLAND_MAVLINK_ENABLED