Line data    Source code

       1             : /*
       2             :    Simulator for MAVLink Gimbal Protocol v2 peripherals
       3             : 
       4             : ./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter \
       5             :     -A --serial5=sim:avt_cm62_gimbal --speedup=1
       6             : 
       7             : param set MNT1_TYPE 6          # MAVLink
       8             : param set CAM1_TYPE 4          # mount
       9             : param set SERIAL5_PROTOCOL 2   # MAVLink2
      10             : reboot
      11             : 
      12             : long REQUEST_MESSAGE 259        # CAMERA_INFORMATION
      13             : status *CAM*
      14             : */
      15             : 
      16             : #pragma once
      17             : 
      18             : #include "SIM_config.h"
      19             : 
      20             : #if AP_SIM_MAVLINKGIMBALV2_ENABLED
      21             : 
      22             : #include "SIM_Mount.h"
      23             : #include "SIM_Gimbal.h"
      24             : #include <GCS_MAVLink/GCS_MAVLink.h>
      25             : #include <AP_Math/AP_Math.h>
      26             : #include <AP_Common/Location.h>
      27             : 
      28             : namespace SITL {
      29             : 
      30             : /*
      31             :   Base class for Gimbal Protocol v2 simulators.  Handles all MAVLink
      32             :   framing; subclasses supply device-specific identity data via
      33             :   pure-virtual accessors.
      34             : */
      35             : class MAVLinkGimbalv2 : public Mount {
      36             : public:
      37             :     void update(const class Aircraft &aircraft) override;
      38             :     void set_instance(uint8_t instance) override;
      39             : 
      40             : protected:
      41             :     // device identity — every subclass must implement all of these
      42             :     virtual const char *get_vendor_name()      const = 0;
      43             :     virtual const char *get_model_name()       const = 0;
      44             :     virtual uint32_t    get_firmware_version() const = 0;
      45             :     virtual uint16_t    get_cap_flags()        const = 0;
      46             :     virtual float get_pitch_min_rad() const = 0;
      47             :     virtual float get_pitch_max_rad() const = 0;
      48             :     virtual float get_yaw_min_rad()   const = 0;
      49             :     virtual float get_yaw_max_rad()   const = 0;
      50             : 
      51             :     // physics model shared by all MAVLink gimbal simulators
      52             :     Gimbal _gimbal;
      53             : 
      54             :     // available to combined classes (e.g. gimbal+camera) for sending messages
      55             :     // and accessing transport state
      56             :     void send_mavlink_message(const mavlink_message_t &msg);
      57      333183 :     uint8_t vehicle_sysid()  const { return _vehicle_system_id; }
      58             :     uint8_t vehicle_compid() const { return _vehicle_component_id; }
      59         326 :     mavlink_status_t &gimbal_mav_status() { return mav.status; }
      60             : 
      61             :     // override in a combined class to handle non-gimbal commands on the same link
      62             :     virtual void handle_message(const mavlink_message_t &msg);
      63             : 
      64             : private:
      65             :     void update_input();
      66             :     void update_gimbal(const class Aircraft &aircraft);
      67             : 
      68             :     void send_heartbeat();
      69             :     void send_gimbal_device_information();
      70             :     void send_attitude_status();
      71             :     void send_command_ack(uint8_t target_sysid, uint8_t target_compid,
      72             :                           MAV_CMD command, MAV_RESULT result);
      73             : 
      74             :     uint8_t _compid {MAV_COMP_ID_GIMBAL};
      75             : 
      76             :     bool     _seen_autopilot_heartbeat;
      77             :     uint8_t  _vehicle_system_id;
      78             :     uint8_t  _vehicle_component_id;
      79             : 
      80             :     uint32_t _last_heartbeat_ms;
      81             :     uint32_t _last_attitude_status_ms;
      82             : 
      83             :     // current demand from the autopilot, set by GIMBAL_DEVICE_SET_ATTITUDE
      84             :     struct {
      85             :         bool valid;               // true once a command has been received
      86             :         bool is_rate;             // true = rate command, false = angle command
      87             :         bool yaw_is_ef;           // true = rates/attitude expressed in earth frame
      88             :         Vector3f rates_rads;      // demanded body rates (is_rate == true)
      89             :         Quaternion attitude;      // desired attitude (is_rate == false)
      90             :     } _target;
      91             : 
      92             :     struct {
      93             :         mavlink_message_t rxmsg;
      94             :         mavlink_status_t  status;
      95             :     } mav;
      96             : 
      97             :     Matrix3f _vehicle_dcm;
      98             : 
      99             :     // ROI location tracking: set by COMMAND_INT DO_SET_ROI_LOCATION
     100             :     struct {
     101             :         bool valid;
     102             :         Location loc;
     103             :     } _roi;
     104             : };
     105             : 
     106             : }  // namespace SITL
     107             : 
     108             : #endif  // AP_SIM_MAVLINKGIMBALV2_ENABLED