AP_Camera: support camera info and settings

libraries/AP_Camera/AP_Camera.cpp

@@ -376,6 +376,32 @@ void AP_Camera::send_feedback(mavlink_channel_t chan)
     }
 }
 
+// send camera information message to GCS
+void AP_Camera::send_camera_information(mavlink_channel_t chan)
+{
+    WITH_SEMAPHORE(_rsem);
+
+    // call each instance
+    for (uint8_t instance = 0; instance < AP_CAMERA_MAX_INSTANCES; instance++) {
+        if (_backends[instance] != nullptr) {
+            _backends[instance]->send_camera_information(chan);
+        }
+    }
+}
+
+// send camera settings message to GCS
+void AP_Camera::send_camera_settings(mavlink_channel_t chan)
+{
+    WITH_SEMAPHORE(_rsem);
+
+    // call each instance
+    for (uint8_t instance = 0; instance < AP_CAMERA_MAX_INSTANCES; instance++) {
+        if (_backends[instance] != nullptr) {
+            _backends[instance]->send_camera_settings(chan);
+        }
+    }
+}
+
 /*
   update; triggers by distance moved and camera trigger
 */

libraries/AP_Camera/AP_Camera.h

@@ -89,6 +89,12 @@ public:
     // send camera feedback message to GCS
     void send_feedback(mavlink_channel_t chan);
 
+    // send camera information message to GCS
+    void send_camera_information(mavlink_channel_t chan);
+
+    // send camera settings message to GCS
+    void send_camera_settings(mavlink_channel_t chan);
+
     // configure camera
     void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time);
     void configure(uint8_t instance, float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time);

libraries/AP_Camera/AP_Camera_Backend.cpp

@@ -141,6 +141,48 @@ void AP_Camera_Backend::send_camera_feedback(mavlink_channel_t chan)
         camera_feedback.feedback_trigger_logged_count); // completed image captures
 }
 
+// send camera information message to GCS
+void AP_Camera_Backend::send_camera_information(mavlink_channel_t chan) const
+{
+    // prepare vendor, model and cam definition strings
+    const uint8_t vendor_name[32] {};
+    const uint8_t model_name[32] {};
+    const char cam_definition_uri[140] {};
+    const uint32_t cap_flags = CAMERA_CAP_FLAGS_CAPTURE_IMAGE;
+    const float NaN = nanf("0x4152");
+
+    // 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]
+        0,                      // firmware version uint32_t
+        NaN,                    // focal_length float (mm)
+        NaN,                    // sensor_size_h float (mm)
+        NaN,                    // sensor_size_v float (mm)
+        0,                      // resolution_h uint16_t (pix)
+        0,                      // resolution_v uint16_t (pix)
+        0,                      // lens_id, uint8_t
+        cap_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_Camera_Backend::send_camera_settings(mavlink_channel_t chan) const
+{
+    const float NaN = nanf("0x4152");
+
+    // send CAMERA_SETTINGS message
+    mavlink_msg_camera_settings_send(
+        chan,
+        AP_HAL::millis(),   // time_boot_ms
+        CAMERA_MODE_IMAGE,  // camera mode (0:image, 1:video, 2:image survey)
+        NaN,                // zoomLevel float, percentage from 0 to 100, NaN if unknown
+        NaN);               // focusLevel float, percentage from 0 to 100, NaN if unknown
+}
+
 // setup a callback for a feedback pin. When on PX4 with the right FMU
 // mode we can use the microsecond timer.
 void AP_Camera_Backend::setup_feedback_callback()

libraries/AP_Camera/AP_Camera_Backend.h

@@ -85,6 +85,12 @@ public:
     // send camera feedback message to GCS
     void send_camera_feedback(mavlink_channel_t chan);
 
+    // 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;
+
 #if AP_CAMERA_SCRIPTING_ENABLED
     // accessor to allow scripting backend to retrieve state
     // returns true on success and cam_state is filled in

libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp

@@ -23,7 +23,7 @@ void AP_Camera_MAVLinkCamV2::update()
 bool AP_Camera_MAVLinkCamV2::trigger_pic()
 {
     // exit immediately if have not found camera or does not support taking pictures
-    if (_link == nullptr || !(_cap_flags & CAMERA_CAP_FLAGS_CAPTURE_IMAGE)) {
+    if (_link == nullptr || !(_cam_info.flags & CAMERA_CAP_FLAGS_CAPTURE_IMAGE)) {
         return false;
     }
 
@@ -43,7 +43,7 @@ bool AP_Camera_MAVLinkCamV2::trigger_pic()
 bool AP_Camera_MAVLinkCamV2::record_video(bool start_recording)
 {
     // exit immediately if have not found camera or does not support recording video
-    if (_link == nullptr || !(_cap_flags & CAMERA_CAP_FLAGS_CAPTURE_VIDEO)) {
+    if (_link == nullptr || !(_cam_info.flags & CAMERA_CAP_FLAGS_CAPTURE_VIDEO)) {
         return false;
     }
 
@@ -68,7 +68,7 @@ bool AP_Camera_MAVLinkCamV2::record_video(bool start_recording)
 bool AP_Camera_MAVLinkCamV2::set_zoom(ZoomType zoom_type, float zoom_value)
 {
     // exit immediately if have not found camera or does not support zoom
-    if (_link == nullptr || !(_cap_flags & CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM)) {
+    if (_link == nullptr || !(_cam_info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM)) {
         return false;
     }
 
@@ -95,7 +95,7 @@ bool AP_Camera_MAVLinkCamV2::set_zoom(ZoomType zoom_type, float zoom_value)
 bool AP_Camera_MAVLinkCamV2::set_focus(FocusType focus_type, float focus_value)
 {
     // exit immediately if have not found camera or does not support focus
-    if (_link == nullptr || !(_cap_flags & CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS)) {
+    if (_link == nullptr || !(_cam_info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS)) {
         return false;
     }
 
@@ -133,30 +133,52 @@ void AP_Camera_MAVLinkCamV2::handle_message(mavlink_channel_t chan, const mavlin
 
     // handle CAMERA_INFORMATION
     if (msg.msgid == MAVLINK_MSG_ID_CAMERA_INFORMATION) {
-        mavlink_camera_information_t cam_info;
+        mavlink_msg_camera_information_decode(&msg, &_cam_info);
-        mavlink_msg_camera_information_decode(&msg, &cam_info);
 
-        const uint8_t fw_ver_major = cam_info.firmware_version & 0x000000FF;
+        const uint8_t fw_ver_major = _cam_info.firmware_version & 0x000000FF;
-        const uint8_t fw_ver_minor = (cam_info.firmware_version & 0x0000FF00) >> 8;
+        const uint8_t fw_ver_minor = (_cam_info.firmware_version & 0x0000FF00) >> 8;
-        const uint8_t fw_ver_revision = (cam_info.firmware_version & 0x00FF0000) >> 16;
+        const uint8_t fw_ver_revision = (_cam_info.firmware_version & 0x00FF0000) >> 16;
-        const uint8_t fw_ver_build = (cam_info.firmware_version & 0xFF000000) >> 24;
+        const uint8_t fw_ver_build = (_cam_info.firmware_version & 0xFF000000) >> 24;
 
         // display camera info to user
         gcs().send_text(MAV_SEVERITY_INFO, "Camera: %s.32 %s.32 fw:%u.%u.%u.%u",
-                cam_info.vendor_name,
+                _cam_info.vendor_name,
-                cam_info.model_name,
+                _cam_info.model_name,
                 (unsigned)fw_ver_major,
                 (unsigned)fw_ver_minor,
                 (unsigned)fw_ver_revision,
                 (unsigned)fw_ver_build);
 
-        // capability flags
-        _cap_flags = cam_info.flags;
-
         _got_camera_info = true;
     }
 }
 
+// send camera information message to GCS
+void AP_Camera_MAVLinkCamV2::send_camera_information(mavlink_channel_t chan) const
+{
+    // exit immediately if we have not yet received cam info
+    if (!_got_camera_info) {
+        return;
+    }
+
+    // send CAMERA_INFORMATION message
+    mavlink_msg_camera_information_send(
+        chan,
+        AP_HAL::millis(),           // time_boot_ms
+        _cam_info.vendor_name,      // vendor_name uint8_t[32]
+        _cam_info.model_name,       // model_name uint8_t[32]
+        _cam_info.firmware_version, // firmware version uint32_t
+        _cam_info.focal_length,     // focal_length float (mm)
+        _cam_info.sensor_size_h,    // sensor_size_h float (mm)
+        _cam_info.sensor_size_v,    // sensor_size_v float (mm)
+        _cam_info.resolution_h,     // resolution_h uint16_t (pix)
+        _cam_info.resolution_v,     // resolution_v uint16_t (pix)
+        _cam_info.lens_id,          // lens_id, uint8_t
+        _cam_info.flags,            // flags uint32_t (CAMERA_CAP_FLAGS)
+        _cam_info.cam_definition_version,   // cam_definition_version uint16_t
+        _cam_info.cam_definition_uri);      // cam_definition_uri char[140]
+}
+
 // search for camera in GCS_MAVLink routing table
 void AP_Camera_MAVLinkCamV2::find_camera()
 {

libraries/AP_Camera/AP_Camera_MAVLinkCamV2.h

@@ -53,6 +53,9 @@ public:
     // handle MAVLink messages from the camera
     void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) override;
 
+    // send camera information message to GCS
+    void send_camera_information(mavlink_channel_t chan) const override;
+
 private:
 
     // search for camera in GCS_MAVLink routing table
@@ -64,11 +67,11 @@ private:
     // internal members
     bool _initialised;          // true once the camera has provided a CAMERA_INFORMATION
     bool _got_camera_info;      // true once camera has provided CAMERA_INFORMATION
+    mavlink_camera_information_t _cam_info {}; // latest camera information received from camera
     uint32_t _last_caminfo_req_ms;  // system time that CAMERA_INFORMATION was last requested (used to throttle requests)
     class GCS_MAVLINK *_link;   // link we have found the camera on. nullptr if not seen yet
     uint8_t _sysid;             // sysid of camera
     uint8_t _compid;            // component id of gimbal
-    uint32_t _cap_flags;        // capability flags from CAMERA_INFORMATION msg, see MAVLink CAMERA_CAP_FLAGS enum
 };
 
 #endif // AP_CAMERA_MAVLINKCAMV2_ENABLED

libraries/AP_Camera/AP_Camera_Mount.cpp

@@ -48,4 +48,22 @@ bool AP_Camera_Mount::set_focus(FocusType focus_type, float focus_value)
     return false;
 }
 
+// send camera information message to GCS
+void AP_Camera_Mount::send_camera_information(mavlink_channel_t chan) const
+{
+    AP_Mount* mount = AP::mount();
+    if (mount != nullptr) {
+        return mount->send_camera_information(chan);
+    }
+}
+
+// send camera settings message to GCS
+void AP_Camera_Mount::send_camera_settings(mavlink_channel_t chan) const
+{
+    AP_Mount* mount = AP::mount();
+    if (mount != nullptr) {
+        return mount->send_camera_settings(chan);
+    }
+}
+
 #endif // AP_CAMERA_MOUNT_ENABLED

libraries/AP_Camera/AP_Camera_Mount.h

@@ -45,6 +45,12 @@ public:
     // set focus specified as rate, percentage or auto
     // 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;
 };
 
 #endif // AP_CAMERA_MOUNT_ENABLED

libraries/AP_Mount/AP_Mount_Siyi.cpp

@@ -311,9 +311,11 @@ void AP_Mount_Siyi::process_packet()
         _hardware_model = (_parsed_msg.data_bytes_received <= 8) ? HardwareModel::A8 : HardwareModel::ZR10;
 
         // consume and display camera firmware version
-        _cam_firmware_version = {_msg_buff[_msg_buff_data_start+2],     // firmware major version
+        _cam_firmware_version = {
-                                 _msg_buff[_msg_buff_data_start+1],     // firmware minor version
+            _msg_buff[_msg_buff_data_start+2],      // firmware major version
-                                 _msg_buff[_msg_buff_data_start+0]};    // firmware revision (aka patch)
+            _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)_cam_firmware_version.major,            // firmware major version
@@ -791,9 +793,9 @@ void AP_Mount_Siyi::send_camera_information(mavlink_channel_t chan) const
         return;
     }
 
-    const uint8_t vendor_name[32] = "Siyi";
+    static const uint8_t vendor_name[32] = "Siyi";
-    uint8_t model_name[32] = "Unknown";
+    static 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 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