Line data    Source code

       1             : #include "AP_Mount_config.h"
       2             : 
       3             : #if HAL_MOUNT_STORM32MAVLINK_ENABLED
       4             : 
       5             : #include "AP_Mount_SToRM32.h"
       6             : 
       7             : #include <AP_HAL/AP_HAL.h>
       8             : #include <GCS_MAVLink/GCS.h>
       9             : 
      10             : extern const AP_HAL::HAL& hal;
      11             : 
      12             : #define AP_MOUNT_STORM32_SEARCH_MS  60000   // search for gimbal for 1 minute after startup
      13             : 
      14             : // update mount position - should be called periodically
      15           0 : void AP_Mount_SToRM32::update()
      16             : {
      17           0 :     AP_Mount_Backend::update();
      18             : 
      19             :     // exit immediately if not initialised
      20           0 :     if (!_initialised) {
      21           0 :         find_gimbal();
      22           0 :         return;
      23             :     }
      24             : 
      25           0 :     AP_Mount_Backend::update_mnt_target();
      26             : 
      27             :     // send target angles (which may be derived from other target types)
      28           0 :     AP_Mount_Backend::send_target_to_gimbal();
      29             : }
      30             : 
      31             : // get attitude as a quaternion.  returns true on success
      32           0 : bool AP_Mount_SToRM32::get_attitude_quaternion(Quaternion& att_quat)
      33             : {
      34           0 :     att_quat.from_euler(mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, mnt_target.angle_rad.get_bf_yaw());
      35           0 :     return true;
      36             : }
      37             : 
      38             : // search for gimbal in GCS_MAVLink routing table
      39           0 : void AP_Mount_SToRM32::find_gimbal()
      40             : {
      41             :     // return immediately if initialised
      42           0 :     if (_initialised) {
      43           0 :         return;
      44             :     }
      45             : 
      46             :     // search for gimbal for 60 seconds or until armed
      47           0 :     if ((AP_HAL::millis() > AP_MOUNT_STORM32_SEARCH_MS) && hal.util->get_soft_armed()) {
      48           0 :         return;
      49             :     }
      50             : 
      51             :     // we expect that instance 0 has compid = MAV_COMP_ID_GIMBAL, instance 1 has compid = MAV_COMP_ID_GIMBAL2, etc
      52           0 :     uint8_t compid = (_instance == 0) ? MAV_COMP_ID_GIMBAL : MAV_COMP_ID_GIMBAL2 + (_instance - 1);
      53           0 :     if (GCS_MAVLINK::find_by_mavtype_and_compid(MAV_TYPE_GIMBAL, compid, _sysid, _chan)) {
      54           0 :         _compid = compid;
      55           0 :         _initialised = true;
      56           0 :         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mount: SToRM32");
      57             :     }
      58             : }
      59             : 
      60             : // send_target_angles - send a COMMAND_LONG containing a do_mount_control message
      61           0 : void AP_Mount_SToRM32::send_target_angles(const MountAngleTarget& angle_target_rad)
      62             : {
      63             :     // exit immediately if not initialised
      64           0 :     if (!_initialised) {
      65           0 :         return;
      66             :     }
      67             : 
      68             :     // check we have space for the message
      69           0 :     if (!HAVE_PAYLOAD_SPACE(_chan, COMMAND_LONG)) {
      70           0 :         return;
      71             :     }
      72             : 
      73             :     // send command_long command containing a do_mount_control command
      74             :     // Note: pitch and yaw are reversed
      75           0 :     mavlink_msg_command_long_send(_chan,
      76           0 :                                   _sysid,
      77           0 :                                   _compid,
      78             :                                   MAV_CMD_DO_MOUNT_CONTROL,
      79             :                                   0,        // confirmation of zero means this is the first time this message has been sent
      80           0 :                                   -degrees(angle_target_rad.pitch),
      81           0 :                                   degrees(angle_target_rad.roll),
      82           0 :                                   -degrees(angle_target_rad.get_bf_yaw()),
      83             :                                   0, 0, 0,  // param4 ~ param6 unused
      84             :                                   MAV_MOUNT_MODE_MAVLINK_TARGETING);
      85             : }
      86             : #endif // HAL_MOUNT_STORM32MAVLINK_ENABLED