Line data    Source code

       1             : /*
       2             :   MAVLink Gimbal Protocol v2 backend class
       3             :  */
       4             : #pragma once
       5             : 
       6             : #include "AP_Mount_config.h"
       7             : 
       8             : #if HAL_MOUNT_MAVLINK_ENABLED
       9             : 
      10             : #include "AP_Mount_Backend.h"
      11             : 
      12             : #include <AP_Math/AP_Math.h>
      13             : #include <AP_Common/AP_Common.h>
      14             : #include <GCS_MAVLink/GCS_MAVLink.h>
      15             : 
      16             : class AP_Mount_MAVLink : public AP_Mount_Backend
      17             : {
      18             : 
      19             : public:
      20             :     // Constructor
      21             :     using AP_Mount_Backend::AP_Mount_Backend;
      22             : 
      23             :     // update mount position
      24             :     void update() override;
      25             : 
      26             :     // return true if healthy
      27             :     bool healthy() const override;
      28             : 
      29             :     // has_pan_control
      30           0 :     bool has_pan_control() const override { return yaw_range_valid(); }
      31             : 
      32             :     // handle GIMBAL_DEVICE_INFORMATION message
      33             :     void handle_gimbal_device_information(const mavlink_message_t &msg) override;
      34             : 
      35             :     // handle GIMBAL_DEVICE_ATTITUDE_STATUS message
      36             :     void handle_gimbal_device_attitude_status(const mavlink_message_t &msg) override;
      37             : 
      38             : protected:
      39             : 
      40             :     // MVAVLink can send either rates or angles, and can also be
      41             :     // directly commanded to move to a retracted state
      42       11602 :     uint8_t natively_supported_mount_target_types() const override {
      43             : 
      44       11602 :         uint8_t supported_target = (
      45             :             (1U<<unsigned(MountTargetType::ANGLE)) |
      46             :             (1U<<unsigned(MountTargetType::RATE)) |
      47             :             (1U<<unsigned(MountTargetType::RETRACTED))
      48             :         );
      49             : 
      50             :         // remove in ArduPilot-4.9; was a temporary hack until we got
      51             :         // GIMBAL_DEVICE_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL
      52       11602 :         if ((strncmp(vendor_name, "AVTA", 4) == 0) && (strncmp(model_name, "CM41", 4) != 0)){
      53       11602 :             supported_target |= (1U<<unsigned(MountTargetType::LOCATION));
      54             :         }
      55             : 
      56       11602 :         if (has_capability(GIMBAL_DEVICE_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL)) {
      57           0 :             supported_target |= (1U<<unsigned(MountTargetType::LOCATION));
      58             :         }
      59             : 
      60       11602 :         return supported_target;
      61             :     };
      62             : 
      63             :     // get attitude as a quaternion.  returns true on success
      64             :     bool get_attitude_quaternion(Quaternion& att_quat) override;
      65             : 
      66             : private:
      67             : 
      68             :     // returns true if the camera is reporting that it has the
      69             :     // capability indicated by flag
      70       11602 :     bool has_capability(GIMBAL_DEVICE_CAP_FLAGS flag) const {
      71       11602 :         return device_capapability_flags & flag;
      72             :     }
      73             : 
      74             :     // search for gimbal in GCS_MAVLink routing table
      75             :     void find_gimbal();
      76             : 
      77             :     // request GIMBAL_DEVICE_INFORMATION from gimbal (holds vendor and model name, max lean angles)
      78             :     void request_gimbal_device_information() const;
      79             : 
      80             :     // start sending ATTITUDE and AUTOPILOT_STATE_FOR_GIMBAL_DEVICE to gimbal
      81             :     // returns true on success, false on failure to start sending
      82             :     bool start_sending_attitude_to_gimbal();
      83             : 
      84             :     // send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to command gimbal to retract (aka relax)
      85             :     void send_target_retracted() override;
      86             : 
      87             :     // send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to control rate
      88             :     void send_target_rates(const MountRateTarget &rate_rads) override;
      89             : 
      90             :     // send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to control attitude
      91             :     void send_target_angles(const MountAngleTarget &angle_rad) override;
      92             : 
      93             :     // Send MAV_CMD_DO_SET_ROI to gimbal to point at a location
      94             :     void send_target_location(const Location &roi_loc) override;
      95             : 
      96             :     // internal variables
      97             :     bool _got_device_info;          // true once gimbal has provided device info
      98             :     bool _initialised;              // true once the gimbal has provided a GIMBAL_DEVICE_INFORMATION
      99             :     uint32_t _last_devinfo_req_ms;  // system time that GIMBAL_DEVICE_INFORMATION was last requested (used to throttle requests)
     100             :     class GCS_MAVLINK *_link;       // link we have found gimbal on; nullptr if not seen yet
     101             :     uint8_t _sysid;                 // sysid of gimbal
     102             :     uint8_t _compid;                // component id of gimbal
     103             :     mavlink_gimbal_device_attitude_status_t _gimbal_device_attitude_status;  // copy of most recently received gimbal status
     104             :     uint32_t _last_attitude_status_ms;  // system time last attitude status was received (used for health reporting)
     105             :     char vendor_name[MAVLINK_MSG_GIMBAL_DEVICE_INFORMATION_FIELD_VENDOR_NAME_LEN];  // vendor name
     106             :     char model_name[MAVLINK_MSG_GIMBAL_DEVICE_INFORMATION_FIELD_MODEL_NAME_LEN];  // model name
     107             :     uint32_t device_capapability_flags;  // from GIMBAL_DEVICE_INFORMATION
     108             : };
     109             : #endif // HAL_MOUNT_MAVLINK_ENABLED