基本通信可行

libraries/AP_Camera/AP_Camera.cpp

@@ -1,5 +1,4 @@
 #include "AP_Camera.h"
-
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Relay/AP_Relay.h>
 #include <AP_Math/AP_Math.h>
@@ -10,7 +9,11 @@
 #include <SRV_Channel/SRV_Channel.h>
 #include <AP_Logger/AP_Logger.h>
 #include <AP_GPS/AP_GPS.h>
-
+#if HAL_WITH_UAVCAN
+#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
+#include <AP_UAVCAN/AP_UAVCAN.h>
+//#include<uavcan/equipment/camera_gimbal/Status.hpp>
+#endif
 // ------------------------------
 #define CAM_DEBUG DISABLED
 
@@ -122,8 +125,7 @@ AP_Camera::servo_pic()
 }
 
 /// basic relay activation
-void
-AP_Camera::relay_pic()
+void AP_Camera::relay_pic()
 {
     AP_Relay *_apm_relay = AP::relay();
     if (_apm_relay == nullptr) {
@@ -153,6 +155,9 @@ void AP_Camera::trigger_pic()
     case AP_CAMERA_TRIGGER_TYPE_RELAY:
         relay_pic();                    // basic relay activation
         break;
+     case AP_CAMERA_TRIGGER_TYPE_UAVCAN:
+      uavcan_pic();
+        break;
     }
 
     log_picture();
@@ -460,8 +465,162 @@ void AP_Camera::update_trigger()
             }
         }
     }
+    send_can_msg();
+}
+
+void AP_Camera::uavcan_pic()
+{
+    #if HAL_WITH_UAVCAN
+    const AP_AHRS &ahrs = AP::ahrs();
+    camera_can_msg.mode = 12;
+    camera_can_msg.lng = current_loc.lng;
+    camera_can_msg.lat = current_loc.lat;
+    camera_can_msg.alt = current_loc.alt;
+    camera_can_msg.pitch = ahrs.pitch_sensor*1e-2f;
+    camera_can_msg.roll = ahrs.roll_sensor*1e-2f;
+    camera_can_msg.yaw = ahrs.yaw_sensor*1e-2f;
+    send_msg_step = 1;    
+    _trigger_counter = constrain_int16(_trigger_duration*5,0,255);
+        #endif
+}
+
+void AP_Camera::send_can_msg(void)
+{
+#if HAL_WITH_UAVCAN
+    if(send_msg_step != 0){
+        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) {
+                switch (send_msg_step)
+                {
+                    case 1:
+                        uavcan->send_zr_cam_cmd(camera_state.id,camera_can_msg.mode,1,_image_index,0,0); 
+                        send_msg_step = 2;    
+                        break;
+                    case 2:
+                        uavcan->send_cam_geopoi(camera_state.id,camera_can_msg.mode,camera_can_msg.lng,camera_can_msg.lat,camera_can_msg.alt,0);
+                        send_msg_step = 3;    
+                        break;
+                    case 3:        
+                        uavcan->send_cam_angular(camera_state.id, camera_can_msg.mode,camera_can_msg.pitch,camera_can_msg.roll,camera_can_msg.yaw);
+                        send_msg_step = 0;    
+                        break;
+                    
+                    default:
+                        break;
+                }
+            }
+        }
+    }
+#endif
+}
+
+void AP_Camera::send_uavcan_camera_zr_control(const mavlink_camera_zr_control_t &packet){
+
+#if HAL_WITH_UAVCAN
+    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->send_zr_cam_cmd(camera_state.id,packet.mode, packet.cmd1, packet.cmd2, packet.cmd3, packet.cmd4);
+        }
+    }
+#endif
+}
+
+const char* AP_Camera::get_camera_sn(void)
+{
+    return  &sn[0];
+}
+
+const char* AP_Camera::get_camrea_ver(void )
+{
+     return  &sver[0];
+}
+
+void AP_Camera::set_camera_sn(char* sn_temp)
+{  
+      memcpy(&sn[0],&sn_temp[0],14);
+      sn[14] ='\0';
+}
+void AP_Camera::set_camera_ver(char *ver_temp)
+{
+      memcpy(&sver[0],&ver_temp[0],11);
+      sn[11] ='\0';
+}
+
+void AP_Camera::request_camera_sn(void){
+#if HAL_WITH_UAVCAN
+    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->send_zr_cam_cmd(camera_state.id, 19, 1, 0, 0, 0);
+            }
+        }
+#endif
+}
+
+
+void AP_Camera::update_zr_camera_status()
+{
+     notice_send_camera_status_to_gcs();
 }
 
+
+void AP_Camera::update_zr_camera_version(void)
+{
+
+     if ((strcmp(&sn[0], "\0")==0)||(strcmp(&sver[0],"\0")==0))
+        {
+           request_camera_sn();
+        }
+        else 
+        {
+            gcs().send_text(MAV_SEVERITY_INFO, "CAMSN:%s", &sn[0]);
+            gcs().send_text(MAV_SEVERITY_INFO, "CAMVer:%s", &sver[0]);
+        }
+}
+void AP_Camera::send_camera_zr_status(mavlink_channel_t chan)
+{
+    uint8_t feedback_shutter_speed = 0;
+    uint8_t feedback_gimbal = camera_state.feedback_code;
+    uint8_t feedback_error_fixed = 0;
+    uint8_t feedback_format = 0;
+    // struct AP_Camera::Cam_ZR_Info status = {};
+   
+    mavlink_msg_camera_zr_status_send(
+        chan,
+        0,
+        0,
+        camera_state.id,
+        camera_state.shutter_speed,
+        camera_state.err_code,
+        camera_state.connect_state,
+        camera_state.usb_enable,
+        feedback_shutter_speed,
+        feedback_gimbal,
+        feedback_format,
+        feedback_error_fixed,
+        camera_state.num1,
+        camera_state.num2,
+        camera_state.num3,
+        camera_state.num4,
+        camera_state.num5,
+        camera_state.trigger_num,
+        camera_state.temperature);
+}
+
+// struct Camera_State  &  AP_Camera::get_camera_state()
+// {
+// return camera_state;
+// }
+
 // singleton instance
 AP_Camera *AP_Camera::_singleton;
 

libraries/AP_Camera/AP_Camera.h

@@ -6,7 +6,8 @@
 #include <GCS_MAVLink/GCS.h>
 
 #define AP_CAMERA_TRIGGER_TYPE_SERVO                0
-#define AP_CAMERA_TRIGGER_TYPE_RELAY                1
+#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
 
@@ -56,13 +57,23 @@ public:
     }
 
     void take_picture();
-
+    void  uavcan_pic();
+    uint8_t  send_msg_step;
     // Update - to be called periodically @at least 10Hz
     void update();
 
     // update camera trigger - 50Hz
     void update_trigger();
-
+    const char* get_camera_sn(void);
+    const char* get_camrea_ver(void);
+    void  set_camera_sn(char *  sn);
+    void  set_camera_ver(char * ver);
+    void request_camera_sn(void);
+    void update_zr_camera_status();
+    void send_uavcan_camera_zr_control(const mavlink_camera_zr_control_t &packet);
+    void notice_send_camera_status_to_gcs(void);
+    void update_zr_camera_version(void);
+    void send_camera_zr_status(mavlink_channel_t chan);
     static const struct AP_Param::GroupInfo        var_info[];
 
     // set if vehicle is in AUTO mode
@@ -84,10 +95,43 @@ public:
 
     uint16_t        _image_index_log;       // number of pictures taken to log @Brown
     
+    struct Camera_State
+    {
+        float temperature;
+        uint16_t num1;
+        uint16_t num2;
+        uint16_t num3;
+        uint16_t num4;
+        uint16_t num5;
+        uint16_t trigger_num;  
+        uint8_t instance; // the instance number of this Camera
+        uint8_t id;
+        uint8_t mode;
+        uint8_t shutter_speed;
+        uint8_t err_code;
+        uint8_t connect_state;
+        uint8_t uavcan_node_id;
+        uint8_t feedback_code;
+        uint8_t usb_enable;
+    };
+      struct Camera_Can_Msg
+    {
+        int32_t lat; // @Brown,for get camera pos
+        int32_t lng;
+        int32_t alt;
+        float roll;
+        float pitch;
+        float yaw;
+        uint8_t mode;
+    };
+    struct Camera_Can_Msg  camera_can_msg;
+   struct Camera_State  &get_camera_state(){
+       return camera_state;
+   }
 private:
 
     static AP_Camera *_singleton;
-
+    struct  Camera_State  camera_state;
     AP_Int8         _trigger_type;      // 0:Servo,1:Relay
     AP_Int8         _trigger_duration;  // duration in 10ths of a second that the camera shutter is held open
     AP_Int8         _relay_on;          // relay value to trigger camera
@@ -144,8 +188,11 @@ private:
     int32_t last_pos_lat; // @Brown,for get camera pos
     int32_t last_pos_lng;
 
-};
+    char sn[15];   //sn 10 byte   str  11byte   give 15byte for  tomorrow used
+    char sver[12]; //software version
 
+    void send_can_msg(void);
+};
 namespace AP {
 AP_Camera *camera();
 };

libraries/AP_HAL/AP_HAL_Boards.h

@@ -162,7 +162,7 @@
 #endif
 
 #ifndef HAL_WITH_UAVCAN
-#define HAL_WITH_UAVCAN 0
+#define HAL_WITH_UAVCAN 1
 #endif
 
 #ifndef HAL_RCINPUT_WITH_AP_RADIO

libraries/AP_Logger/AP_Logger.h

@@ -284,6 +284,7 @@ public:
     void Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& point);
     void Write_OABendyRuler(bool active, float target_yaw, float margin, const Location &final_dest, const Location &oa_dest);
     void Write_OADijkstra(uint8_t state, uint8_t error_id, uint8_t curr_point, uint8_t tot_points, const Location &final_dest, const Location &oa_dest);
+    void Write_Camera_Feedback(uint16_t index,uint16_t cam1,uint16_t cam2,uint16_t cam3,uint16_t cam4,uint16_t cam5,uint8_t flag);
 
     void Write(const char *name, const char *labels, const char *fmt, ...);
     void Write(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, ...);

libraries/AP_Logger/LogFile.cpp

@@ -1011,3 +1011,20 @@ void AP_Logger::Write_OADijkstra(uint8_t state, uint8_t error_id, uint8_t curr_p
     };
     WriteBlock(&pkt, sizeof(pkt));
 }
+
+void AP_Logger::Write_Camera_Feedback(uint16_t index,uint16_t cam1,uint16_t cam2,uint16_t cam3,uint16_t cam4,uint16_t cam5,uint8_t flag)
+{
+    struct log_Camera_FeedBack pkt
+    {
+        LOG_PACKET_HEADER_INIT(LOG_CAM_FEEDBACK_MSG),
+            time_us : AP_HAL::micros64(),
+            index: index,
+            cam1_count:cam1,
+            cam2_count:cam2,
+            cam3_count:cam3,
+            cam4_count:cam4,
+            cam5_count:cam5,
+            flag:flag
+    };
+      WriteBlock(&pkt, sizeof(pkt));
+}

libraries/AP_Logger/LogStructure.h

@@ -1204,6 +1204,19 @@ struct PACKED log_Arm_Disarm {
     uint16_t arm_checks;
 };
 
+struct  PACKED log_Camera_FeedBack
+{
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint16_t  index;
+    uint16_t cam1_count;
+    uint16_t cam2_count;
+    uint16_t cam3_count;
+    uint16_t cam4_count;
+    uint16_t cam5_count;
+    uint8_t flag;
+};
+
 // FMT messages define all message formats other than FMT
 // UNIT messages define units which can be referenced by FMTU messages
 // FMTU messages associate types (e.g. centimeters/second/second) to FMT message fields
@@ -1399,8 +1412,9 @@ struct PACKED log_Arm_Disarm {
     { LOG_OA_BENDYRULER_MSG, sizeof(log_OABendyRuler), \
       "OABR","QBHHfLLLL","TimeUS,Active,DesYaw,Yaw,Mar,DLat,DLng,OALat,OALng", "sbddmDUDU", "F----GGGG" }, \
     { LOG_OA_DIJKSTRA_MSG, sizeof(log_OADijkstra), \
-      "OADJ","QBBBBLLLL","TimeUS,State,Err,CurrPoint,TotPoints,DLat,DLng,OALat,OALng", "sbbbbDUDU", "F----GGGG" }
-
+      "OADJ","QBBBBLLLL","TimeUS,State,Err,CurrPoint,TotPoints,DLat,DLng,OALat,OALng", "sbbbbDUDU", "F----GGGG" },\
+    { LOG_CAM_FEEDBACK_MSG, sizeof(log_Camera_FeedBack), \
+      "CAMF","QHHHHHHB","TimeUS,Index,CAM1,CAM2,CAM3,CAM4,CAM5,FLAG", "s-------", "F-------" }
 // messages for more advanced boards
 #define LOG_EXTRA_STRUCTURES \
     { LOG_IMU2_MSG, sizeof(log_IMU), \
@@ -1775,6 +1789,7 @@ enum LogMessages : uint8_t {
     LOG_OA_BENDYRULER_MSG,
     LOG_OA_DIJKSTRA_MSG,
     LOG_RFND2_MSG,
+    LOG_CAM_FEEDBACK_MSG,
     _LOG_LAST_MSG_
 };
 

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -41,7 +41,15 @@
 #include <ardupilot/equipment/trafficmonitor/TrafficReport.hpp>
 #include <uavcan/equipment/gnss/RTCMStream.hpp>
 
-#include<zrzk/equipment/indication/ZrRGB.hpp>
+#include <zrzk/equipment/indication/ZrRGB.hpp>
+
+#include <zrzk/equipment/camera/Version.hpp>
+#include <zrzk/equipment/camera/Status.hpp>
+#include <zrzk/equipment/camera/CamFeedBack.hpp>
+#include <zrzk/equipment/camera/Command.hpp>
+
+#include <uavcan/equipment/camera_gimbal/GEOPOICommand.hpp>
+#include <uavcan/equipment/camera_gimbal/AngularCommand.hpp>
 
 #include <AP_Baro/AP_Baro_UAVCAN.h>
 #include <AP_RangeFinder/AP_RangeFinder_UAVCAN.h>
@@ -53,6 +61,8 @@
 #include <AP_OpticalFlow/AP_OpticalFlow_HereFlow.h>
 #include <AP_ADSB/AP_ADSB.h>
 #include "AP_UAVCAN_Server.h"
+#include <AP_Logger/AP_Logger.h>
+#include <AP_Camera/AP_Camera.h>
 
 #define LED_DELAY_US 50000
 
@@ -113,6 +123,10 @@ static uavcan::Publisher<uavcan::equipment::gnss::RTCMStream>* rtcm_stream[MAX_N
 
 static uavcan::Publisher<zrzk::equipment::indication::ZrRGB>* zr_rgb_led[MAX_NUMBER_OF_CAN_DRIVERS];
 
+static uavcan::Publisher<zrzk::equipment::camera::Command>* zr_camera_command[MAX_NUMBER_OF_CAN_DRIVERS];
+static uavcan::Publisher<uavcan::equipment::camera_gimbal::AngularCommand>* cam_angular[MAX_NUMBER_OF_CAN_DRIVERS];
+static uavcan::Publisher<uavcan::equipment::camera_gimbal::GEOPOICommand>* cam_geopoi[MAX_NUMBER_OF_CAN_DRIVERS];
+
 // subscribers
 
 // handler SafteyButton
@@ -123,6 +137,15 @@ static uavcan::Subscriber<ardupilot::indication::Button, ButtonCb> *safety_butto
 UC_REGISTRY_BINDER(TrafficReportCb, ardupilot::equipment::trafficmonitor::TrafficReport);
 static uavcan::Subscriber<ardupilot::equipment::trafficmonitor::TrafficReport, TrafficReportCb> *traffic_report_listener[MAX_NUMBER_OF_CAN_DRIVERS];
 
+//handler  cameras status @binsir
+UC_REGISTRY_BINDER(ZrCamStatusCb,zrzk::equipment::camera::Status);
+static uavcan::Subscriber<zrzk::equipment::camera::Status,ZrCamStatusCb> *zr_camera_status_listener[MAX_NUMBER_OF_CAN_DRIVERS];
+
+UC_REGISTRY_BINDER(ZrCamVersionCb,zrzk::equipment::camera::Version);
+static uavcan::Subscriber<zrzk::equipment::camera::Version,ZrCamVersionCb> *zr_camera_version_listener[MAX_NUMBER_OF_CAN_DRIVERS];
+
+UC_REGISTRY_BINDER(ZrCamFeedBackCb,zrzk::equipment::camera::CamFeedBack);
+static uavcan::Subscriber<zrzk::equipment::camera::CamFeedBack,ZrCamFeedBackCb> *zr_camera_feedback_listener[MAX_NUMBER_OF_CAN_DRIVERS];
 
 AP_UAVCAN::AP_UAVCAN() :
     _node_allocator()
@@ -211,7 +234,6 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
     uint8_t uid_len = uid_buf_len;
     uint8_t unique_id[uid_buf_len];
 
-
     if (hal.util->get_system_id_unformatted(unique_id, uid_len)) {
         //This is because we are maintaining a common Server Record for all UAVCAN Instances.
         //In case the node IDs are different, and unique id same, it will create
@@ -273,6 +295,18 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
     zr_rgb_led[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
 
     
+    zr_camera_command[driver_index] = new uavcan::Publisher<zrzk::equipment::camera::Command>(*_node);
+    zr_camera_command[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
+    zr_camera_command[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
+    
+    cam_angular[driver_index] = new uavcan::Publisher<uavcan::equipment::camera_gimbal::AngularCommand>(*_node);
+    cam_angular[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
+    cam_angular[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
+
+    cam_geopoi[driver_index] = new uavcan::Publisher<uavcan::equipment::camera_gimbal::GEOPOICommand>(*_node);
+    cam_geopoi[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
+    cam_geopoi[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
+
     safety_button_listener[driver_index] = new uavcan::Subscriber<ardupilot::indication::Button, ButtonCb>(*_node);
     if (safety_button_listener[driver_index]) {
         safety_button_listener[driver_index]->start(ButtonCb(this, &handle_button));
@@ -283,6 +317,21 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
         traffic_report_listener[driver_index]->start(TrafficReportCb(this, &handle_traffic_report));
     }
 
+  zr_camera_status_listener[driver_index] = new uavcan::Subscriber<zrzk::equipment::camera::Status,ZrCamStatusCb>(*_node);
+   if(zr_camera_status_listener[driver_index]){
+        zr_camera_status_listener[driver_index]->start(ZrCamStatusCb(this, &handle_zr_camera_status));
+   }
+
+    zr_camera_version_listener[driver_index] = new uavcan::Subscriber<zrzk::equipment::camera::Version,ZrCamVersionCb>(*_node);
+   if(zr_camera_version_listener[driver_index]){
+        zr_camera_version_listener[driver_index]->start(ZrCamVersionCb(this, &handle_zr_camera_version));
+   }
+
+   zr_camera_feedback_listener[driver_index] = new uavcan::Subscriber<zrzk::equipment::camera::CamFeedBack,ZrCamFeedBackCb>(*_node);
+   if(zr_camera_feedback_listener[driver_index]){
+        zr_camera_feedback_listener[driver_index]->start(ZrCamFeedBackCb(this, &handle_zr_camera_feedback));
+   }
+
     _led_conf.devices_count = 0;
     if (enable_filters) {
         configureCanAcceptanceFilters(*_node);
@@ -359,9 +408,7 @@ void AP_UAVCAN::loop(void)
     }
 }
 
-
 ///// SRV output /////
-
 void AP_UAVCAN::SRV_send_actuator(void)
 {
     uint8_t starting_servo = 0;
@@ -732,5 +779,114 @@ bool AP_UAVCAN::send_zr_rgb_led(uint8_t mode, uint8_t red, uint8_t green, uint8_
   return true;
 }
 
+void AP_UAVCAN::send_zr_cam_cmd(uint8_t gimbal_id, uint8_t mode,uint8_t v1, uint8_t v2,uint8_t v3,uint8_t v4)
+{
+    zrzk::equipment::camera::Command msg;
+ 
+    msg.id = gimbal_id;
+    msg.command_mode = mode;
+    msg.command_1 =v1;
+    msg.command_2 =v2;
+    msg.command_3 =v3;
+    msg.command_4 =v4;
+    zr_camera_command[_driver_index]->broadcast(msg);
+}
+
+void AP_UAVCAN::send_cam_geopoi(uint8_t id,uint8_t mode,int32_t longitude_deg_1e7,int32_t latitude_deg_1e7,int32_t  height_cm,int8_t height_type)
+{  
+    uavcan::equipment::camera_gimbal::GEOPOICommand msg;
+
+    uint8_t gimbal_id = id;
+    msg.gimbal_id = gimbal_id;
+    msg.mode.command_mode = mode;
+    msg.latitude_deg_1e7 =latitude_deg_1e7;
+    msg.longitude_deg_1e7 = longitude_deg_1e7;
+    msg.height_cm = height_cm;
+
+    cam_geopoi[_driver_index]->broadcast(msg);
+}
+
+void AP_UAVCAN:: send_cam_angular(uint8_t id,uint8_t mode,float pitch,float roll,float yaw)
+{
+    uavcan::equipment::camera_gimbal::AngularCommand  msg;
+
+    uint8_t gimbal_id = id;
+    msg.gimbal_id = gimbal_id;
+    msg.mode.command_mode = mode;
+    msg.quaternion_xyzw[0] = pitch;//ahrs.pitch_sensor*1e-2f;
+    msg.quaternion_xyzw[1] =roll;
+    msg.quaternion_xyzw[2] = yaw;
+
+    cam_angular[_driver_index]->broadcast(msg);
+}
+
+void AP_UAVCAN::handle_zr_camera_feedback(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ZrCamFeedBackCb &cb)
+{
+    AP_Logger *logger = AP_Logger::get_singleton();
+    if ( logger!=nullptr )
+    {
+            AP::logger().Write_Camera_Feedback(
+
+            cb.msg->index,
+            cb.msg->camera_photo_number[0],
+            cb.msg->camera_photo_number[1],
+            cb.msg->camera_photo_number[2],
+            cb.msg->camera_photo_number[3],
+            cb.msg->camera_photo_number[4],
+            cb.msg->camera_photoed);
+    }
+}
+
+void AP_UAVCAN::handle_zr_camera_status(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ZrCamStatusCb &cb)
+{
+    // if(cb.msg->gimbal_id ==0xC5){  //根据id 判断是五镜头数据  197 ==C5
+    AP_Camera *camera = AP::camera();
+    if (camera == nullptr)
+    {
+        return;
+    }
+    AP_Camera::Camera_State   &interim_state  = AP::camera()->get_camera_state();
+    interim_state.mode = cb.msg->mode;
+    interim_state.id = cb.msg->id;
+    interim_state.num1 = (uint16_t)cb.msg->camera_photo_number[0];
+    interim_state.num2 = (uint16_t)cb.msg->camera_photo_number[1];
+    interim_state.num3 = (uint16_t)cb.msg->camera_photo_number[2];
+    interim_state.num4 = (uint16_t)cb.msg->camera_photo_number[3];
+    interim_state.num5 = (uint16_t)cb.msg->camera_photo_number[4];
+    interim_state.temperature = cb.msg->temperature;
+    interim_state.shutter_speed = cb.msg->shutter_speed;
+    interim_state.err_code = cb.msg->error_code;
+    interim_state.feedback_code = cb.msg->feedback_code;
+    interim_state.usb_enable = cb.msg->usb_enable;
+
+    camera->update_zr_camera_status();
+}
+
+void AP_UAVCAN::handle_zr_camera_version(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ZrCamVersionCb &cb)
+{
+    AP_Camera *camera = AP::camera();
+
+    if (camera == nullptr)
+    {
+        return;
+    }
+    char sn[15];
+    char ver[12];
+    memcpy(&sn[0],&cb.msg->camera_sn[0],15);
+    memcpy(&ver[0],&cb.msg->software_version[0],12);
+    camera->set_camera_sn(&sn[0]);
+    camera->set_camera_ver(&ver[0]);
+    camera->update_zr_camera_version( );
+}
+
+
+void AP_Camera::notice_send_camera_status_to_gcs(void)
+{
+     //add  control send time 
+        gcs().send_message(MSG_CAMERA_ZR_STATUS);     
+}
+
+
+
 
 #endif // HAL_WITH_UAVCAN

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -50,6 +50,10 @@
 class ButtonCb;
 class TrafficReportCb;
 
+class ZrCamStatusCb;
+class ZrCamVersionCb;
+class ZrCamFeedBackCb;
+
 /*
     Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received,
     the Callback will invoke registery to register the node as separate backend.
@@ -93,6 +97,10 @@ public:
 
     bool send_zr_rgb_led(uint8_t mode, uint8_t red, uint8_t green, uint8_t blue);
 
+    void send_zr_cam_cmd(uint8_t gimbal_id,  uint8_t mode,uint8_t v1, uint8_t v2,uint8_t v3,uint8_t v4);
+    void send_cam_geopoi(uint8_t id,uint8_t mode,int32_t longitude_deg_1e7,int32_t latitude_deg_1e7,int32_t  height_cm,int8_t height_type);
+    void send_cam_angular(uint8_t id,uint8_t mode,float pitch,float roll,float yaw);
+
     template <typename DataType_>
     class RegistryBinder {
     protected:
@@ -228,6 +236,9 @@ private:
     // safety button handling
     static void handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb);
     static void handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb);
+    static void handle_zr_camera_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ZrCamStatusCb &cb);
+    static void handle_zr_camera_version(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ZrCamVersionCb &cb);
+    static void handle_zr_camera_feedback(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ZrCamFeedBackCb &cb);
 };
 
 #endif /* AP_UAVCAN_H_ */

libraries/AP_UAVCAN/dsdl/zrzk/equipment/camera/30010.Status.uavcan

@@ -0,0 +1,27 @@
+  
+#
+# Generic zrzk camera status.
+#
+
+uint8 id
+
+uint6 mode    #delete
+
+uint2 usb_enable
+
+uint8 shutter_speed
+
+uint8 error_code
+
+uint8 NONE                  = 0
+uint8 SHUTTER_SPEED_DONE    = 1
+uint8 FORMAT_DONE           = 2
+uint8 GIMBAL_DONE           = 4
+uint8 ERROR_FIX_DONE        = 8
+
+uint8 feedback_code
+
+float16 temperature
+uint16 trigger_num
+uint16[<=5] camera_photo_number
+#uint8[<40] message

libraries/AP_UAVCAN/dsdl/zrzk/equipment/camera/30011.Version.uavcan

@@ -0,0 +1,17 @@
+  
+#
+# Generic zr camera status.
+#
+
+uint8 id
+
+uint8 mode
+
+#
+# Camera axis orientation in body frame (not in fixed frame).
+# Please refer to the UAVCAN coordinate frame conventions.
+#
+uint8[12] software_version
+uint8[12] hardware_version
+uint8[15] camera_sn
+uint8[15] camera_name

libraries/AP_UAVCAN/dsdl/zrzk/equipment/camera/30012.Command.uavcan

@@ -0,0 +1,27 @@
+  
+#
+# Generic zr camera commond.
+#
+
+uint8 id
+
+uint8 CAM_RECOVER       = 11
+uint8 TAKE_PHOTO        = 12
+uint8 SHATTER_SPEED     = 13
+uint8 GIMBAL_CONTROL    = 15
+uint8 ERROR_FIX         = 16
+uint8 FORMAT            = 17
+uint8 GET_PHOTO_NUMBER  = 18
+uint8 GET_CAMERA_VERSION = 19
+
+uint8 command_mode
+
+#
+# Camera axis orientation in body frame (not in fixed frame).
+# Please refer to the UAVCAN coordinate frame conventions.
+#
+
+uint8  command_1
+uint8  command_2
+uint8  command_3
+uint8  command_4

libraries/AP_UAVCAN/dsdl/zrzk/equipment/camera/30014.CamFeedBack.uavcan

@@ -0,0 +1,21 @@
+  
+#
+# Generic zr camera commond.
+#
+
+uint8 id
+
+#
+#0x01 cam1
+#0x02 cam2
+#0x04 cam3
+#0x08 cam4
+#0x10 cam5
+#
+uint8 camera_photoed
+uint16 index
+uint16[<=5] camera_photo_number
+#
+# Camera axis orientation in body frame (not in fixed frame).
+# Please refer to the UAVCAN coordinate frame conventions.
+#

libraries/GCS_MAVLink/GCS.h

@@ -371,7 +371,7 @@ protected:
     MAV_RESULT handle_command_request_autopilot_capabilities(const mavlink_command_long_t &packet);
 
     virtual void send_banner();
-
+   virtual void send_camera_banner();
     void handle_device_op_read(const mavlink_message_t &msg);
     void handle_device_op_write(const mavlink_message_t &msg);
 
@@ -421,7 +421,7 @@ protected:
     MAV_RESULT handle_command_do_fence_enable(const mavlink_command_long_t &packet);
 
     void handle_optical_flow(const mavlink_message_t &msg);
-
+    void handle_camera_zr_control(const mavlink_message_t &msg);
     // vehicle-overridable message send function
     virtual bool try_send_message(enum ap_message id);
     virtual void send_global_position_int();

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -796,6 +796,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
         { MAVLINK_MSG_ID_EXTENDED_SYS_STATE,    MSG_EXTENDED_SYS_STATE},
         { MAVLINK_MSG_ID_AUTOPILOT_VERSION,     MSG_AUTOPILOT_VERSION},
          { MAVLINK_MSG_ID_ZR_FLYING_STATUS,     MSG_ZR_FLYING_STATUS},
+         {MAVLINK_MSG_ID_CAMERA_ZR_STATUS,   MSG_CAMERA_ZR_STATUS},
             };
 
     for (uint8_t i=0; i<ARRAY_SIZE(map); i++) {
@@ -3042,6 +3043,17 @@ void GCS_MAVLINK::handle_optical_flow(const mavlink_message_t &msg)
     optflow->handle_msg(msg);
 }
 
+void GCS_MAVLINK::handle_camera_zr_control(const mavlink_message_t &msg)
+{
+    mavlink_camera_zr_control_t packet;
+    mavlink_msg_camera_zr_control_decode(&msg, &packet);
+    AP_Camera *camera = AP::camera();
+    if (camera != nullptr)
+    {
+        camera->send_uavcan_camera_zr_control(packet);
+    }
+}
+
 /*
   handle messages which don't require vehicle specific data
  */
@@ -3217,6 +3229,9 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg)
         handle_optical_flow(msg);
         break;
 
+    case MAVLINK_MSG_ID_CAMERA_ZR_CONTROL:
+        handle_camera_zr_control(msg);
+        break;
     }
 }
 
@@ -4323,6 +4338,17 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
         }
         break;
 
+           case MSG_CAMERA_ZR_STATUS:
+    {
+        AP_Camera *camera = AP::camera();
+        if(camera ==nullptr ){
+            break;
+        }    
+        CHECK_PAYLOAD_SIZE(CAMERA_ZR_STATUS);
+        camera->send_camera_zr_status(chan) ;       
+    }
+    break;
+
     case MSG_SYSTEM_TIME:
         CHECK_PAYLOAD_SIZE(SYSTEM_TIME);
         send_system_time();
@@ -4821,6 +4847,27 @@ void GCS_MAVLINK::manual_override(RC_Channel *c, int16_t value_in, const uint16_
     c->set_override(override_value, tnow);
 }
 
+
+void GCS_MAVLINK::send_camera_banner(void)
+{
+#if HAL_WITH_UAVCAN
+    AP_Camera *camera = AP::camera();
+
+    if (camera == nullptr)
+        return;
+    if ((strcmp(camera->get_camera_sn(), "\0") == 0) || (strcmp(camera->get_camrea_ver(), "\0") == 0))
+    {
+        camera->request_camera_sn();
+    }
+    else
+    {
+        send_text(MAV_SEVERITY_INFO, "CAMSN:%s", camera->get_camera_sn());
+        //send_text(MAV_SEVERITY_INFO, "CAMId: %d", camera->interim_state.id);
+        send_text(MAV_SEVERITY_INFO, "CAMVer:%s", camera->get_camrea_ver());
+    }
+#endif
+
+}
 /*
   handle a SET_MODE MAVLink message
  */

libraries/GCS_MAVLink/ap_message.h

@@ -77,5 +77,6 @@ enum ap_message : uint8_t {
     MSG_ZR_CAMERA_STATUS,
     MSG_BATTGO_INFO,
     MSG_BATTGO_HISTORY,
+    MSG_CAMERA_ZR_STATUS,
     MSG_LAST // MSG_LAST must be the last entry in this enum
 };

modules/mavlink

@@ -1 +1 @@
-Subproject commit 18cab6950986cd7b9b7a507ec35f3501d4a7e019
+Subproject commit 4a6577557847fcb13f8bd9b7ebbe54f4a5a9f00e