新增航点类型，传相机角度控制

libraries/AP_Camera/AP_Camera.cpp

@@ -164,6 +164,10 @@ void AP_Camera::trigger_pic()
      case AP_CAMERA_TRIGGER_TYPE_UAVCAN:
       uavcan_pic();
         break;
+     case AP_CAMERA_TRIGGER_TYPE_UAVCAN_ATT:
+      uavcan_pic_att();
+        break;
+
     }
 
     log_picture();
@@ -285,6 +289,18 @@ void AP_Camera::control(float session, float zoom_pos, float zoom_step, float fo
     GCS_MAVLINK::send_to_components(MAVLINK_MSG_ID_COMMAND_LONG, (char*)&mav_cmd_long, sizeof(mav_cmd_long));
 }
 
+void AP_Camera::control_att(float pitch,float yaw,float shooting_cmd )
+{
+    // take picture
+    
+    _camera_pitch = pitch;
+    _camera_pitch = yaw;
+    if (is_equal(shooting_cmd,1.0f)) {
+        trigger_pic();
+    }
+    gcs().send_text(MAV_SEVERITY_INFO, "camera p:%.2f,y:%.2f,trg:%d",_camera_pitch,_camera_yaw,shooting_cmd);
+}
+
 /*
   Send camera feedback to the GCS
  */
@@ -497,6 +513,25 @@ void AP_Camera::uavcan_pic()
 #endif
 }
 
+void AP_Camera::uavcan_pic_att()
+{
+    
+    gcs().send_text(MAV_SEVERITY_INFO, "camera p:%.2f,y:%.2f",_camera_pitch,_camera_yaw);
+    //TODO: 增加发送云台角度控制
+#if HAL_WITH_UAVCAN
+    uint8_t mode = 12;
+    uint8_t can_num_drivers = AP::can().get_num_drivers();
+    for (uint8_t i = 0; i < can_num_drivers; i++)
+    {
+        AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i);
+        if (uavcan != nullptr)
+        {
+            uavcan->set_zr_cam_cmd(camera_state.id, mode, 1, _image_index&0xff, _image_index>>8, 0);
+        }
+    }
+    _trigger_counter = constrain_int16(_trigger_duration * 5, 0, 255);
+#endif
+}
 void AP_Camera::send_uavcan_camera_zr_control(const mavlink_camera_zr_control_t &packet){
 
 #if HAL_WITH_UAVCAN

libraries/AP_Camera/AP_Camera.h

@@ -5,10 +5,6 @@
 #include <AP_Param/AP_Param.h>
 #include <GCS_MAVLink/GCS.h>
 
-#define AP_CAMERA_TRIGGER_TYPE_SERVO                0
-#define AP_CAMERA_TRIGGER_TYPE_RELAY                 1
-#define AP_CAMERA_TRIGGER_TYPE_UAVCAN             2
-
 #define AP_CAMERA_TRIGGER_DEFAULT_TRIGGER_TYPE  AP_CAMERA_TRIGGER_TYPE_RELAY    // default is to use servo to trigger camera
 
 #define AP_CAMERA_TRIGGER_DEFAULT_DURATION  3      // default duration servo or relay is held open in 10ths of a second (i.e. 10 = 1 second)
@@ -58,6 +54,8 @@ public:
 
     void take_picture();
     void  uavcan_pic();
+    void  uavcan_pic_att();
+    void  control_att(float pitch,float yaw,float shooting_cmd );
     uint8_t  send_msg_step;
     // Update - to be called periodically @at least 10Hz
     void update();
@@ -94,6 +92,14 @@ public:
         CAMERA_TYPE_BMMCC
     };
 
+    enum trigger_types {
+        AP_CAMERA_TRIGGER_TYPE_SERVO,
+        AP_CAMERA_TRIGGER_TYPE_RELAY,
+        AP_CAMERA_TRIGGER_TYPE_UAVCAN,
+        AP_CAMERA_TRIGGER_TYPE_UAVCAN_ATT
+    };
+
+
     void get_last_trigger_pos(int32_t &lat,int32_t &lng){ lat=last_pos_lat;lng=last_pos_lng; } // @Brown,for get camera pos
 
     //  modivy by @Brown
@@ -205,6 +211,8 @@ private:
     uint8_t  hv_minor;
     uint8_t  hv_patch;
     mavlink_zr_cam_sum_t zr_cam_sum;
+    float _camera_pitch;     // 航线传入相机姿态pitch
+    float _camera_yaw;     // 航线传入相机姿态yaw
    // void send_can_msg(void);
 };
 namespace AP {

libraries/AP_Mission/AP_Mission.cpp

@@ -289,6 +289,7 @@ bool AP_Mission::verify_command(const Mission_Command& cmd)
     case MAV_CMD_DO_DIGICAM_CONTROL:
     case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
     case MAV_CMD_DO_PARACHUTE:
+    case MAV_CMD_DO_GIMBAL_CONTROL:
         return true;
     default:
         return _cmd_verify_fn(cmd);
@@ -310,6 +311,7 @@ bool AP_Mission::start_command(const Mission_Command& cmd)
     case MAV_CMD_DO_DIGICAM_CONFIGURE:
     case MAV_CMD_DO_DIGICAM_CONTROL:
     case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
+    case MAV_CMD_DO_GIMBAL_CONTROL:
         return start_command_camera(cmd);
     case MAV_CMD_DO_PARACHUTE:
         return start_command_parachute(cmd);
@@ -927,6 +929,13 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_
         cmd.content.mount_control.yaw = packet.param3;
         break;
 
+    case MAV_CMD_DO_GIMBAL_CONTROL:                      // MAV ID: 225
+        cmd.content.gimbal_control.pitch = packet.param1;
+        cmd.content.gimbal_control.roll = packet.param2;
+        cmd.content.gimbal_control.yaw = packet.param3;
+        cmd.content.gimbal_control.shooting_cmd = packet.param4;
+        break;
+        
     case MAV_CMD_DO_SET_CAM_TRIGG_DIST:                 // MAV ID: 206
         cmd.content.cam_trigg_dist.meters = packet.param1;  // distance between camera shots in meters
         break;
@@ -1363,6 +1372,13 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c
         packet.param3 = cmd.content.mount_control.yaw;
         break;
 
+    case MAV_CMD_DO_GIMBAL_CONTROL:                      // MAV ID: 225
+        packet.param1 = cmd.content.gimbal_control.pitch;
+        packet.param2 = cmd.content.gimbal_control.roll;
+        packet.param3 = cmd.content.gimbal_control.yaw;
+        packet.param4 = cmd.content.gimbal_control.shooting_cmd;
+        break;
+
     case MAV_CMD_DO_SET_CAM_TRIGG_DIST:                 // MAV ID: 206
         packet.param1 = cmd.content.cam_trigg_dist.meters;  // distance between camera shots in meters
         break;
@@ -1936,6 +1952,8 @@ const char *AP_Mission::Mission_Command::type() const {
         return "DigiCamCfg";
     case MAV_CMD_DO_DIGICAM_CONTROL:
         return "DigiCamCtrl";
+    case MAV_CMD_DO_GIMBAL_CONTROL:
+        return "GimbalCamCtrl";
     case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
         return "SetCamTrigDst";
     case MAV_CMD_DO_SET_ROI:

libraries/AP_Mission/AP_Mission.h

@@ -108,7 +108,12 @@ public:
         float roll;             // roll angle in degrees
         float yaw;              // yaw angle (relative to vehicle heading) in degrees
     };
-
+    struct PACKED Gimbal_Control {
+        float pitch;            // pitch angle in degrees
+        float roll;             // roll angle in degrees
+        float yaw;              // yaw angle (relative to vehicle heading) in degrees
+        uint8_t shooting_cmd;              // 
+    };
     // digicam control command structure
     struct PACKED Digicam_Configure {
         uint8_t shooting_mode;  // ProgramAuto = 1, AV = 2, TV = 3, Man=4, IntelligentAuto=5, SuperiorAuto=6
@@ -255,6 +260,7 @@ public:
         // do-winch
         Winch_Command winch;
 
+        Gimbal_Control gimbal_control;
         // location
         Location location{};      // Waypoint location
     };

libraries/AP_Mission/AP_Mission_Commands.cpp

@@ -100,6 +100,12 @@ bool AP_Mission::start_command_camera(const AP_Mission::Mission_Command& cmd)
             cmd.content.digicam_control.cmd_id);
         return true;
 
+    case MAV_CMD_DO_GIMBAL_CONTROL:                    // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|
+        camera->control_att(cmd.content.gimbal_control.pitch,
+            cmd.content.gimbal_control.yaw,
+            cmd.content.gimbal_control.shooting_cmd);
+        return true;
+
     case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
         camera->set_trigger_distance(cmd.content.cam_trigg_dist.meters);
         return true;

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -2777,6 +2777,14 @@ MAV_RESULT GCS_MAVLINK::handle_command_camera(const mavlink_command_long_t &pack
         camera->set_trigger_distance(packet.param1);
         result = MAV_RESULT_ACCEPTED;
         break;
+
+    case MAV_CMD_DO_GIMBAL_CONTROL:
+        camera->control_att(packet.param1,
+                        packet.param3,
+                        packet.param4);
+        camera->cameras_take_enable = 0;
+        result = MAV_RESULT_ACCEPTED;
+        break;
     default:
         result = MAV_RESULT_UNSUPPORTED;
         break;
@@ -3721,6 +3729,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
     case MAV_CMD_DO_DIGICAM_CONFIGURE:
     case MAV_CMD_DO_DIGICAM_CONTROL:
     case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
+    case MAV_CMD_DO_GIMBAL_CONTROL:
         result = handle_command_camera(packet);
         break;