仿真测试后调整，uavcan消息发送成功

ArduCopter/GCS_Mavlink.cpp

@@ -705,6 +705,28 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
         }
         return MAV_RESULT_FAILED;
 
+    case MAV_CMD_DO_GIMBAL_CONTROL:
+    {
+        // param3 : target angle [-180~180]
+        // param2 : speed during change [deg per second]
+        // param3 : direction (-1:ccw, +1:cw)
+        // param4 : relative offset (1) or absolute angle (0)
+        gcs().send_text(MAV_SEVERITY_INFO, "gcs gimbal:%.2f,%.2f,%.2f,%.2f",packet.param1,packet.param2,packet.param3,packet.param4);   
+        float angle = is_positive(packet.param3)?packet.param3:packet.param3+360.0f;
+        if ((packet.param3 >= -180.0f)   &&
+            (packet.param3 <= 180.0f)) {
+            copter.flightmode->auto_yaw.set_fixed_yaw(
+                angle,
+                0,
+                0,
+                0);
+
+            handle_command_camera(packet);
+            return MAV_RESULT_ACCEPTED;
+        }
+        }
+        return MAV_RESULT_FAILED;
+
     case MAV_CMD_DO_CHANGE_SPEED:
         // param1 : unused
         // param2 : new speed in m/s

ArduCopter/UserCode.cpp

@@ -126,6 +126,48 @@ void Copter::userhook_SuperSlowLoop()
         camera.take_picture();
     }
 #endif
+    if(1)
+    {
+        static uint8_t ccnt;
+        static float cpitch = 0.0;
+        static float cyaw = 0.0;
+        static uint8_t step = 0;
+        ccnt+=1;
+        if (ccnt < 3)
+        {
+            return;
+        }
+        
+        switch (step)
+        {
+        case 0:
+            cpitch = 0.0f;
+            cyaw = 0.0f;
+            step +=1;
+            break;
+        case 1:
+            cpitch = -30.0f;
+            cyaw = 1.0f;
+            step +=1;
+            break;
+        case 2:
+            cpitch = -60.0f;
+            cyaw = 2.0f;
+            step +=1;
+            break;
+        case 3:
+            cpitch = -90.0f;
+            cyaw = 3.0f;
+            step =0;
+            break;
+        
+        default:
+            break;
+        }
+        camera.control_att(cpitch,cyaw,1.0f);
+        ccnt = 0;
+
+    }
 
 }
 #endif

ArduCopter/mode_auto.cpp

@@ -491,6 +491,19 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
         do_yaw(cmd);
         break;
 
+    case MAV_CMD_DO_GIMBAL_CONTROL:             // 115
+    {
+        float angle = is_positive(cmd.content.gimbal_control.yaw)?cmd.content.gimbal_control.yaw:cmd.content.gimbal_control.yaw+360.0f;
+        auto_yaw.set_fixed_yaw(
+            angle,
+            0,
+            (angle - ahrs.yaw_sensor/100.0f),
+            0);
+           
+        // gcs().send_text(MAV_SEVERITY_INFO, "**GIMBAL yaw:%.2f,%d",angle,ahrs.yaw_sensor);
+        }
+        break;
+
     ///
     /// do commands
     ///
@@ -719,6 +732,7 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
         break;
 
     case MAV_CMD_CONDITION_YAW:
+    case MAV_CMD_DO_GIMBAL_CONTROL:
         cmd_complete = verify_yaw();
         break;
 

ArduCopter/version.h

@@ -5,15 +5,15 @@
 #endif
 
 #include "ap_version.h"
-#define GIT_VERSION "2"
-#define THISFIRMWARE "ZRUAV v4.1.18"  //"ArduCopter V4.0.0"
+#define GIT_VERSION "3"
+#define THISFIRMWARE "ZRUAV v4.1.19"  //"ArduCopter V4.0.0"
 
 // the following line is parsed by the autotest scripts
-#define FIRMWARE_VERSION 4,1,18,FIRMWARE_VERSION_TYPE_OFFICIAL
+#define FIRMWARE_VERSION 4,1,19,FIRMWARE_VERSION_TYPE_OFFICIAL
 
 #define FW_MAJOR 4
 #define FW_MINOR 1
-#define FW_PATCH 18
+#define FW_PATCH 19
 #define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL
 /**
  * AVDv6: avoid.get_zr_mode() = 3时可以用Loiter模式测试自动避障

libraries/AP_Camera/AP_Camera.cpp

@@ -292,13 +292,12 @@ void AP_Camera::control(float session, float zoom_pos, float zoom_step, float fo
 void AP_Camera::control_att(float pitch,float yaw,float shooting_cmd )
 {
     // take picture
-    
     _camera_pitch = pitch;
-    _camera_pitch = yaw;
+    _camera_yaw = 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);
+    gcs().send_text(MAV_SEVERITY_INFO, "camera p:%.2f,y:%.2f,trg:%f",_camera_pitch,_camera_yaw,shooting_cmd);
 }
 
 /*
@@ -516,7 +515,7 @@ void AP_Camera::uavcan_pic()
 void AP_Camera::uavcan_pic_att()
 {
     
-    gcs().send_text(MAV_SEVERITY_INFO, "camera p:%.2f,y:%.2f",_camera_pitch,_camera_yaw);
+    // gcs().send_text(MAV_SEVERITY_INFO, "camera p:%.2f,y:%.2f",_camera_pitch,_camera_yaw);
     //TODO: 增加发送云台角度控制
 #if HAL_WITH_UAVCAN
     uint8_t mode = 12;
@@ -527,6 +526,7 @@ void AP_Camera::uavcan_pic_att()
         if (uavcan != nullptr)
         {
             uavcan->set_zr_cam_cmd(camera_state.id, mode, 1, _image_index&0xff, _image_index>>8, 0);
+            uavcan->set_cam_angular(camera_state.id,2,_camera_pitch,0,_camera_yaw);
         }
     }
     _trigger_counter = constrain_int16(_trigger_duration * 5, 0, 255);

libraries/AP_Mission/AP_Mission.cpp

@@ -289,7 +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:
+    // case MAV_CMD_DO_GIMBAL_CONTROL:
         return true;
     default:
         return _cmd_verify_fn(cmd);
@@ -311,10 +311,12 @@ 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);
+    case MAV_CMD_DO_GIMBAL_CONTROL:
+        start_command_camera(cmd);
+        return _cmd_start_fn(cmd);
     default:
         return _cmd_start_fn(cmd);
     }

libraries/AP_Mission/AP_Mission_Commands.cpp

@@ -101,6 +101,7 @@ bool AP_Mission::start_command_camera(const AP_Mission::Mission_Command& cmd)
         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|
+        //TODO 这里调用到相机控制
         camera->control_att(cmd.content.gimbal_control.pitch,
             cmd.content.gimbal_control.yaw,
             cmd.content.gimbal_control.shooting_cmd);

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -483,7 +483,7 @@ void AP_UAVCAN::loop(void)
             }
         }
         zr_cam_cmd_send();
-        //cam_aular_send();
+        cam_aular_send();
         //cam_geopoi_send();
         zr_rgb_led_send();
         led_out_send();

modules/mavlink

@@ -1 +1 @@
-Subproject commit 959dea22dafbae0e5682cd2191e608b45a7566a9
+Subproject commit 73dc48ca0ab259e16ec940db7aec95e44383c877

px4v3.sh

@@ -1,4 +1,3 @@
-git submodule update
-./waf configure --board CubeBlack
+./waf configure --board fmuv3
 ./waf  copter
-cp build/CubeBlack/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/zr-v4/wsl/dev-px4v3.px4
+cp build/fmuv3/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/编译/dev-px4v3.px4