AP_Mount: support send-camera-info and settings

libraries/AP_Mount/AP_Mount.cpp

@@ -742,6 +742,28 @@ bool AP_Mount::set_focus(uint8_t instance, FocusType focus_type, float focus_val
     return backend->set_focus(focus_type, focus_value);
 }
 
+// send camera information message to GCS
+void AP_Mount::send_camera_information(mavlink_channel_t chan) const
+{
+    // call send_camera_information for each instance
+    for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
+        if (_backends[instance] != nullptr) {
+            _backends[instance]->send_camera_information(chan);
+        }
+    }
+}
+
+// send camera settings message to GCS
+void AP_Mount::send_camera_settings(mavlink_channel_t chan) const
+{
+    // call send_camera_settings for each instance
+    for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
+        if (_backends[instance] != nullptr) {
+            _backends[instance]->send_camera_settings(chan);
+        }
+    }
+}
+
 AP_Mount_Backend *AP_Mount::get_primary() const
 {
     return get_instance(_primary);

libraries/AP_Mount/AP_Mount.h

@@ -212,6 +212,12 @@ public:
     // focus in = -1, focus hold = 0, focus out = 1
     bool set_focus(uint8_t instance, FocusType focus_type, float focus_value);
 
+    // send camera information message to GCS
+    void send_camera_information(mavlink_channel_t chan) const;
+
+    // send camera settings message to GCS
+    void send_camera_settings(mavlink_channel_t chan) const;
+
     // parameter var table
     static const struct AP_Param::GroupInfo        var_info[];
 

libraries/AP_Mount/AP_Mount_Backend.h

@@ -154,6 +154,12 @@ public:
     // focus in = -1, focus hold = 0, focus out = 1
     virtual bool set_focus(FocusType focus_type, float focus_value) { return false; }
 
+    // send camera information message to GCS
+    virtual void send_camera_information(mavlink_channel_t chan) const {}
+
+    // send camera settings message to GCS
+    virtual void send_camera_settings(mavlink_channel_t chan) const {}
+
 protected:
 
     enum class MountTargetType {

libraries/AP_Mount/AP_Mount_Siyi.cpp

@@ -310,11 +310,15 @@ void AP_Mount_Siyi::process_packet()
         // set hardware version based on message length
         _hardware_model = (_parsed_msg.data_bytes_received <= 8) ? HardwareModel::A8 : HardwareModel::ZR10;
 
-        // display camera firmware version
+        // consume and display camera firmware version
+        _cam_firmware_version = {_msg_buff[_msg_buff_data_start+2],     // firmware major version
+                                 _msg_buff[_msg_buff_data_start+1],     // firmware minor version
+                                 _msg_buff[_msg_buff_data_start+0]};    // firmware revision (aka patch)
+
         debug("Mount: SiyiCam fw:%u.%u.%u",
-              (unsigned)_msg_buff[_msg_buff_data_start+2],      // firmware major version
+              (unsigned)_cam_firmware_version.major,            // firmware major version
-              (unsigned)_msg_buff[_msg_buff_data_start+1],      // firmware minor version
+              (unsigned)_cam_firmware_version.minor,            // firmware minor version
-              (unsigned)_msg_buff[_msg_buff_data_start+0]);     // firmware revision
+              (unsigned)_cam_firmware_version.patch);           // firmware revision
 
         // display gimbal info to user
         gcs().send_text(MAV_SEVERITY_INFO, "Mount: Siyi fw:%u.%u.%u",
@@ -779,4 +783,73 @@ bool AP_Mount_Siyi::set_focus(FocusType focus_type, float focus_value)
     return false;
 }
 
+// send camera information message to GCS
+void AP_Mount_Siyi::send_camera_information(mavlink_channel_t chan) const
+{
+    // exit immediately if not initialised
+    if (!_initialised || !_got_firmware_version) {
+        return;
+    }
+
+    const uint8_t vendor_name[32] = "Siyi";
+    uint8_t model_name[32] = "Unknown";
+    uint32_t fw_version = _cam_firmware_version.major | (_cam_firmware_version.minor << 8) | (_cam_firmware_version.patch << 16);
+    const char cam_definition_uri[140] {};
+
+    // focal length
+    float focal_length_mm = 0;
+    switch (_hardware_model) {
+    case HardwareModel::UNKNOWN:
+        break;
+    case HardwareModel::A8:
+        strncpy((char *)model_name, "A8", sizeof(model_name));
+        focal_length_mm = 21;
+        break;
+    case HardwareModel::ZR10:
+        strncpy((char *)model_name, "ZR10", sizeof(model_name));
+        // focal length range from 5.15 ~ 47.38
+        focal_length_mm = 5.15;
+        break;
+    }
+
+    // capability flags
+    const uint32_t flags = CAMERA_CAP_FLAGS_CAPTURE_VIDEO |
+                           CAMERA_CAP_FLAGS_CAPTURE_IMAGE |
+                           CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM |
+                           CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS;
+
+    // send CAMERA_INFORMATION message
+    mavlink_msg_camera_information_send(
+        chan,
+        AP_HAL::millis(),       // time_boot_ms
+        vendor_name,            // vendor_name uint8_t[32]
+        model_name,             // model_name uint8_t[32]
+        fw_version,             // firmware version uint32_t
+        focal_length_mm,        // focal_length float (mm)
+        0,                      // sensor_size_h float (mm)
+        0,                      // sensor_size_v float (mm)
+        0,                      // resolution_h uint16_t (pix)
+        0,                      // resolution_v uint16_t (pix)
+        0,                      // lens_id uint8_t
+        flags,                  // flags uint32_t (CAMERA_CAP_FLAGS)
+        0,                      // cam_definition_version uint16_t
+        cam_definition_uri);    // cam_definition_uri char[140]
+}
+
+// send camera settings message to GCS
+void AP_Mount_Siyi::send_camera_settings(mavlink_channel_t chan) const
+{
+    const float NaN = nanf("0x4152");
+    const float zoom_mult_max = get_zoom_mult_max();
+    const float zoom_pct = is_positive(zoom_mult_max) ? (_zoom_mult / zoom_mult_max * 100) : 0;
+
+    // send CAMERA_SETTINGS message
+    mavlink_msg_camera_settings_send(
+        chan,
+        AP_HAL::millis(),   // time_boot_ms
+        _last_record_video ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey)
+        zoom_pct,           // zoomLevel float, percentage from 0 to 100, NaN if unknown
+        NaN);               // focusLevel float, percentage from 0 to 100, NaN if unknown
+}
+
 #endif // HAL_MOUNT_SIYI_ENABLED

libraries/AP_Mount/AP_Mount_Siyi.h

@@ -72,6 +72,12 @@ public:
     // focus in = -1, focus hold = 0, focus out = 1
     bool set_focus(FocusType focus_type, float focus_value) override;
 
+    // send camera information message to GCS
+    void send_camera_information(mavlink_channel_t chan) const override;
+
+    // send camera settings message to GCS
+    void send_camera_settings(mavlink_channel_t chan) const override;
+
 protected:
 
     // get attitude as a quaternion.  returns true on success
@@ -200,6 +206,11 @@ private:
     AP_HAL::UARTDriver *_uart;                      // uart connected to gimbal
     bool _initialised;                              // true once the driver has been initialised
     bool _got_firmware_version;                     // true once gimbal firmware version has been received
+    struct {
+        uint8_t major;
+        uint8_t minor;
+        uint8_t patch;
+    } _cam_firmware_version;                        // camera firmware version (for reporting for GCS)
 
     // buffer holding bytes from latest packet.  This is only used to calculate the crc
     uint8_t _msg_buff[AP_MOUNT_SIYI_PACKETLEN_MAX];