增加一些拍照角度控制测试；camera类型枚举调整；先发角度后拍照

ArduCopter/UserCode.cpp

@@ -14,7 +14,7 @@
 //#include<uavcan/equipment/camera_gimbal/Status.hpp>
 #endif
 
-#define CAM_DEBUG  0
+#define CAM_DEBUG  1
 
 #ifdef USERHOOK_INIT
 void Copter::userhook_init()
@@ -122,11 +122,12 @@ void Copter::userhook_SuperSlowLoop()
         last_manual_avoid = avoid.get_avoid_action();
     }
 #if CAM_DEBUG 
-    if(in_debug_mode == 2 || control_mode==Mode::Number::ACRO){            
+    // if(in_debug_mode == 2 || control_mode==Mode::Number::ACRO){            
-        camera.take_picture();
+    //     camera.take_picture();
-    }
+    // }
-#endif
+    // if(1)
-    if(1)
+    // 拍照发送角度测试
+    if(in_debug_mode == 2 || control_mode==Mode::Number::ACRO)
     {
         static uint8_t ccnt;
         static float cpitch = 0.0;
@@ -168,6 +169,7 @@ void Copter::userhook_SuperSlowLoop()
         ccnt = 0;
 
     }
+#endif
 
 }
 #endif
@@ -187,11 +189,12 @@ void Copter::userhook_auxSwitch1(uint8_t ch_flag)
     //         break;
     //     }
     // }
+#if CAM_DEBUG      
     switch (ch_flag) {
         case 2: {
             // relay.on(2);
             gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode 2!");
-            in_debug_mode = 1;
+            in_debug_mode = 2;
             break;
         }
         // case 1: {
@@ -207,12 +210,12 @@ void Copter::userhook_auxSwitch1(uint8_t ch_flag)
             break;
         }
     }
-#if CAM_DEBUG      
+#else
     switch (ch_flag) {
         case 2: {
             // relay.on(2);
             gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode 2!");
-            in_debug_mode = 2;
+            in_debug_mode = 1;
             break;
         }
         // case 1: {
@@ -252,6 +255,18 @@ void Copter::zr_set_uav_state_to_uavcan()
         {
             zr_uav_state_data_t data{0};
             data.armd = motors->armed();
+            
+#if CAM_DEBUG     
+    static bool into_debug = false;
+    if(in_debug_mode == 2 || control_mode==Mode::Number::ACRO)
+    {
+        into_debug = true;
+        copter.updownStatus = UpDown_InMission; // 任务模式云台才转角度
+    }else if(into_debug){
+        copter.updownStatus = UpDown_RequestDescent; // 降落阶段角度回到水平用
+        into_debug = false;
+    }
+#endif
             data.fly_status = copter.updownStatus;
             data.gps_state = AP::gps().status();
             data.wp_number = g.hardware_flag&0xFF;//硬件版本

ArduCopter/mode_auto.cpp

@@ -499,8 +499,6 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
             0,
             (angle - ahrs.yaw_sensor/100.0f),
             0);
-           
-        // gcs().send_text(MAV_SEVERITY_INFO, "**GIMBAL yaw:%.2f,%d",angle,ahrs.yaw_sensor);
         }
         break;
 

hrs100h.sh

@@ -1,3 +1,3 @@
 ./waf configure --board rs100h
 ./waf copter
-cp ./build/rs100h/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/rs100h-v4.1.18.px4
+cp ./build/rs100h/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/rs100h-v4.1.19.px4

libraries/AP_Camera/AP_Camera.cpp

@@ -184,21 +184,41 @@ AP_Camera::trigger_pic_cleanup()
         _trigger_counter--;
     } else {
         switch (_trigger_type) {
-        case AP_CAMERA_TRIGGER_TYPE_SERVO:
+            case AP_CAMERA_TRIGGER_TYPE_SERVO:
-            SRV_Channels::set_output_pwm(SRV_Channel::k_cam_trigger, _servo_off_pwm);
+                SRV_Channels::set_output_pwm(SRV_Channel::k_cam_trigger, _servo_off_pwm);
-            break;
+                break;
-        case AP_CAMERA_TRIGGER_TYPE_RELAY: {
+            case AP_CAMERA_TRIGGER_TYPE_RELAY: {
-            AP_Relay *_apm_relay = AP::relay();
+                AP_Relay *_apm_relay = AP::relay();
-            if (_apm_relay == nullptr) {
+                if (_apm_relay == nullptr) {
+                    break;
+                }
+                if (_relay_on) {
+                    _apm_relay->off(0);
+                } else {
+                    _apm_relay->on(0);
+                }
                 break;
             }
-            if (_relay_on) {
+            case AP_CAMERA_TRIGGER_TYPE_UAVCAN_ATT:{    // 角度和拍照分开发送
-                _apm_relay->off(0);
+                // #if HAL_WITH_UAVCAN
-            } else {
+                // if(new_att){
-                _apm_relay->on(0);
+                //     uint8_t can_num_drivers = AP::can().get_num_drivers();
-            }
+                //     for (uint8_t i = 0; i < can_num_drivers; i++)
-            break;
+                //     {
-        }
+                //         AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i);
+                //         if (uavcan != nullptr)
+                //         {
+                //             uavcan->set_cam_angular(camera_state.id,2,_camera_pitch,0,_camera_yaw);
+                //         }
+                //     }
+                //     new_att = false;
+                // }
+
+                // #endif
+                }
+                break;
+            default:
+                break;
         }
     }
 
@@ -525,10 +545,11 @@ void AP_Camera::uavcan_pic_att()
         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);
             uavcan->set_cam_angular(camera_state.id,2,_camera_pitch,0,_camera_yaw);
+            uavcan->set_zr_cam_cmd(camera_state.id, mode, 1, _image_index&0xff, _image_index>>8, 0);
         }
     }
+    // new_att = true;
     _trigger_counter = constrain_int16(_trigger_duration * 5, 0, 255);
 #endif
 }

libraries/AP_Camera/AP_Camera.h

@@ -95,8 +95,8 @@ public:
     enum trigger_types {
         AP_CAMERA_TRIGGER_TYPE_SERVO,
         AP_CAMERA_TRIGGER_TYPE_RELAY,
-        AP_CAMERA_TRIGGER_TYPE_UAVCAN,
+        AP_CAMERA_TRIGGER_TYPE_UAVCAN_ATT,
-        AP_CAMERA_TRIGGER_TYPE_UAVCAN_ATT
+        AP_CAMERA_TRIGGER_TYPE_UAVCAN
     };
 
 
@@ -214,6 +214,7 @@ private:
     float _camera_pitch;     // 航线传入相机姿态pitch
     float _camera_yaw;     // 航线传入相机姿态yaw
    // void send_can_msg(void);
+   bool new_att;
 };
 namespace AP {
 AP_Camera *camera();