AP_Mission: add DO_GIMBAL_MANAGER_PITCHYAW support

libraries/AP_Mission/AP_Mission.cpp

@@ -363,6 +363,8 @@ bool AP_Mission::start_command(const Mission_Command& cmd)
         return start_command_do_sprayer(cmd);
     case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
         return command_do_set_repeat_dist(cmd);
+    case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
+        return start_command_do_gimbal_manager_pitchyaw(cmd);
     default:
         return _cmd_start_fn(cmd);
     }
@@ -1201,7 +1203,16 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_
     case MAV_CMD_DO_PAUSE_CONTINUE:
         cmd.p1 = packet.param1;
         break;
-        
+
+    case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
+        cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg = packet.param1;
+        cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg = packet.param2;
+        cmd.content.gimbal_manager_pitchyaw.pitch_rate_degs = packet.param3;
+        cmd.content.gimbal_manager_pitchyaw.yaw_rate_degs = packet.param4;
+        cmd.content.gimbal_manager_pitchyaw.flags = packet.x;
+        cmd.content.gimbal_manager_pitchyaw.gimbal_id = packet.z;
+        break;
+
     default:
         // unrecognised command
         return MAV_MISSION_UNSUPPORTED;
@@ -1289,6 +1300,7 @@ MAV_MISSION_RESULT AP_Mission::convert_MISSION_ITEM_to_MISSION_ITEM_INT(const ma
     case MAV_CMD_DO_DIGICAM_CONTROL:
     case MAV_CMD_DO_DIGICAM_CONFIGURE:
     case MAV_CMD_NAV_ATTITUDE_TIME:
+    case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
         mav_cmd.x = packet.x;
         mav_cmd.y = packet.y;
         break;
@@ -1331,6 +1343,7 @@ MAV_MISSION_RESULT AP_Mission::convert_MISSION_ITEM_INT_to_MISSION_ITEM(const ma
     case MAV_CMD_DO_DIGICAM_CONTROL:
     case MAV_CMD_DO_DIGICAM_CONFIGURE:
     case MAV_CMD_NAV_ATTITUDE_TIME:
+    case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
         item.x = item_int.x;
         item.y = item_int.y;
         break;
@@ -1677,7 +1690,16 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c
     case MAV_CMD_DO_PAUSE_CONTINUE:
         packet.param1 = cmd.p1;
         break;
-        
+
+    case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
+        packet.param1 = cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg;
+        packet.param2 = cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg;
+        packet.param3 = cmd.content.gimbal_manager_pitchyaw.pitch_rate_degs;
+        packet.param4 = cmd.content.gimbal_manager_pitchyaw.yaw_rate_degs;
+        packet.x = cmd.content.gimbal_manager_pitchyaw.flags;
+        packet.z = cmd.content.gimbal_manager_pitchyaw.gimbal_id;
+        break;
+
     default:
         // unrecognised command
         return false;
@@ -2402,7 +2424,8 @@ const char *AP_Mission::Mission_Command::type() const
         return "NavAttitudeTime";
     case MAV_CMD_DO_PAUSE_CONTINUE:
         return "PauseContinue";
-
+    case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
+        return "GimbalPitchYaw";
     default:
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
         AP_HAL::panic("Mission command with ID %u has no string", id);

libraries/AP_Mission/AP_Mission.h

@@ -239,6 +239,16 @@ public:
         float climb_rate;
     };
 
+    // MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW support
+    struct PACKED gimbal_manager_pitchyaw_Command {
+        int8_t pitch_angle_deg;
+        int16_t yaw_angle_deg;
+        int8_t pitch_rate_degs;
+        int8_t yaw_rate_degs;
+        uint8_t flags;
+        uint8_t gimbal_id;
+    };
+
     union Content {
         // jump structure
         Jump_Command jump;
@@ -315,6 +325,9 @@ public:
         // nav attitude time
         nav_attitude_time_Command nav_attitude_time;
 
+        // MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW
+        gimbal_manager_pitchyaw_Command gimbal_manager_pitchyaw;
+
         // location
         Location location{};      // Waypoint location
     };
@@ -760,7 +773,7 @@ private:
 
     bool start_command_do_sprayer(const AP_Mission::Mission_Command& cmd);
     bool start_command_do_scripting(const AP_Mission::Mission_Command& cmd);
-
+    bool start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Mission_Command& cmd);
 };
 
 namespace AP

libraries/AP_Mission/AP_Mission_Commands.cpp

@@ -8,6 +8,7 @@
 #include <AC_Sprayer/AC_Sprayer.h>
 #include <AP_Scripting/AP_Scripting.h>
 #include <RC_Channel/RC_Channel.h>
+#include <AP_Mount/AP_Mount.h>
 
 bool AP_Mission::start_command_do_aux_function(const AP_Mission::Mission_Command& cmd)
 {
@@ -204,3 +205,40 @@ bool AP_Mission::start_command_do_scripting(const AP_Mission::Mission_Command& c
     return false;
 #endif // AP_SCRIPTING_ENABLED
 }
+
+bool AP_Mission::start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Mission_Command& cmd)
+{
+#if HAL_MOUNT_ENABLED
+    AP_Mount *mount = AP::mount();
+    if (mount == nullptr) {
+        return false;
+    }
+    // check flags for change to RETRACT
+    if ((cmd.content.gimbal_manager_pitchyaw.flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) {
+        mount->set_mode(MAV_MOUNT_MODE_RETRACT);
+        return true;
+    }
+    // check flags for change to NEUTRAL
+    if ((cmd.content.gimbal_manager_pitchyaw.flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) {
+        mount->set_mode(MAV_MOUNT_MODE_NEUTRAL);
+        return true;
+    }
+
+    // To-Do: handle earth-frame vs body-frame angles
+    //bool earth_frame = cmd.content.gimbal_manager_pitchyaw.flags.GIMBAL_MANAGER_FLAGS_ROLL_LOCK |
+    //                   cmd.content.gimbal_manager_pitchyaw.flags.GIMBAL_MANAGER_FLAGS_PITCH_LOCK |
+    //                   cmd.content.gimbal_manager_pitchyaw.flags.GIMBAL_MANAGER_FLAGS_YAW_LOCK;
+    // To-Do: handle pitch and yaw rates
+    // To-Do: handle gimbal device id
+
+    if (!isnan(cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg) && !isnan(cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg)) {
+        mount->set_angle_targets(0, cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg, cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg);
+        return true;
+    }
+
+    // if we got this far then message is not handled
+    return false;
+#else
+    return false;
+#endif // HAL_MOUNT_ENABLED
+}