AP_Camera: MAVLinkCamV2 driver

libraries/AP_Camera/AP_Camera.cpp

@@ -13,9 +13,10 @@
 #include "AP_Camera_Backend.h"
 #include "AP_Camera_Servo.h"
 #include "AP_Camera_Relay.h"
+#include "AP_Camera_SoloGimbal.h"
 #include "AP_Camera_Mount.h"
 #include "AP_Camera_MAVLink.h"
-#include "AP_Camera_SoloGimbal.h"
+#include "AP_Camera_MAVLinkCamV2.h"
 
 const AP_Param::GroupInfo AP_Camera::var_info[] = {
 
@@ -163,6 +164,11 @@ void AP_Camera::init()
         case CameraType::MAVLINK:
             _backends[instance] = new AP_Camera_MAVLink(*this, _params[instance], instance);
             break;
+
+        // check for MAVLink Camv2 driver
+        case CameraType::MAVLINK_CAMV2:
+            _backends[instance] = new AP_Camera_MAVLinkCamV2(*this, _params[instance], instance);
+            break;
 #endif
         case CameraType::NONE:
             break;

libraries/AP_Camera/AP_Camera.h

@@ -21,6 +21,8 @@ class AP_Camera_Servo;
 class AP_Camera_Relay;
 class AP_Camera_SoloGimbal;
 class AP_Camera_Mount;
+class AP_Camera_MAVLink;
+class AP_Camera_MAVLinkCamV2;
 
 /// @class	Camera
 /// @brief	Object managing a Photo or video camera
@@ -33,6 +35,7 @@ class AP_Camera {
     friend class AP_Camera_SoloGimbal;
     friend class AP_Camera_Mount;
     friend class AP_Camera_MAVLink;
+    friend class AP_Camera_MAVLinkCamV2;
 
 public:
 
@@ -62,6 +65,7 @@ public:
 #endif
 #if AP_CAMERA_MAVLINK_ENABLED
         MAVLINK = 5,        // MAVLink enabled camera
+        MAVLINK_CAMV2 = 6,  // MAVLink camera v2
 #endif
     };
 

libraries/AP_Camera/AP_Camera_MAVLink.h

@@ -14,7 +14,7 @@
  */
 
 /*
-  Camera driver for cameras included in Mount
+  Camera driver for cameras that implement the older MAVLink camera protocol
  */
 #pragma once
 

libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp

@@ -0,0 +1,222 @@
+#include "AP_Camera_MAVLinkCamV2.h"
+
+#if AP_CAMERA_MAVLINK_ENABLED
+#include <GCS_MAVLink/GCS.h>
+
+extern const AP_HAL::HAL& hal;
+
+#define AP_CAMERA_MAVLINKCAMV2_SEARCH_MS    60000   // search for camera for 60 seconds after startup
+
+// update - should be called at 50hz
+void AP_Camera_MAVLinkCamV2::update()
+{
+    // exit immediately if not initialised
+    if (!_initialised) {
+        find_camera();
+    }
+
+    // call parent update
+    AP_Camera_Backend::update();
+}
+
+// entry point to actually take a picture.  returns true on success
+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)) {
+        return false;
+    }
+
+    // prepare and send message
+    mavlink_command_long_t pkt {};
+    pkt.command = MAV_CMD_IMAGE_START_CAPTURE;
+    pkt.param3 = 1;             // number of images to take
+    pkt.param4 = image_index+1; // starting sequence number
+
+    _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);
+
+    return true;
+}
+
+// start or stop video recording.  returns true on success
+// set start_recording = true to start record, false to stop recording
+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)) {
+        return false;
+    }
+
+    // prepare and send message
+    mavlink_command_long_t pkt {};
+
+    if (start_recording) {
+        pkt.command = MAV_CMD_VIDEO_START_CAPTURE;
+        // param1 = 0, video stream id. 0 for all streams
+        // param2 = 0, status frequency.  frequency that CAMERA_CAPTURE_STATUS messages should be sent while recording. 0 for no messages
+    } else {
+        pkt.command = MAV_CMD_VIDEO_STOP_CAPTURE;
+        // param1 = 0, video stream id. 0 for all streams
+    }
+
+    _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);
+
+    return true;
+}
+
+// set camera zoom step.  returns true on success
+// zoom out = -1, hold = 0, zoom in = 1
+bool AP_Camera_MAVLinkCamV2::set_zoom_step(int8_t zoom_step)
+{
+    // exit immediately if have not found camera or does not support zoom
+    if (_link == nullptr || !(_cap_flags & CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM)) {
+        return false;
+    }
+
+    // prepare and send message
+    mavlink_command_long_t pkt {};
+    pkt.command = MAV_CMD_SET_CAMERA_ZOOM;
+    pkt.param1 = ZOOM_TYPE_CONTINUOUS;  // Zoom Type, 0:ZOOM_TYPE_STEP, 1:ZOOM_TYPE_CONTINUOUS, 2:ZOOM_TYPE_RANGE, 3:ZOOM_TYPE_FOCAL_LENGTH
+    pkt.param2 = zoom_step;             // Zoom Value
+
+    _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);
+
+    return true;
+}
+
+// set focus in, out or hold.  returns true on success
+// focus in = -1, focus hold = 0, focus out = 1
+bool AP_Camera_MAVLinkCamV2::set_manual_focus_step(int8_t focus_step)
+{
+    // exit immediately if have not found camera or does not support focus
+    if (_link == nullptr || !(_cap_flags & CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS)) {
+        return false;
+    }
+
+    // prepare and send message
+    mavlink_command_long_t pkt {};
+    pkt.command = MAV_CMD_SET_CAMERA_FOCUS;
+    pkt.param1 = FOCUS_TYPE_CONTINUOUS; // Focus Type, 0:FOCUS_TYPE_STEP, 1:FOCUS_TYPE_CONTINUOUS, 2:FOCUS_TYPE_RANGE, 3:FOCUS_TYPE_METERS, 4:FOCUS_TYPE_AUTO, 5:FOCUS_TYPE_AUTO_SINGLE, 5:FOCUS_TYPE_AUTO_CONTINUOUS
+    pkt.param2 = focus_step;            // Focus Value
+
+    _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);
+
+    return true;
+}
+
+// auto focus.  returns true on success
+bool AP_Camera_MAVLinkCamV2::set_auto_focus()
+{
+    // exit immediately if have not found camera or does not support focus
+    if (_link == nullptr || !(_cap_flags & CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS)) {
+        return false;
+    }
+
+    // prepare and send message
+    mavlink_command_long_t pkt {};
+    pkt.command = MAV_CMD_SET_CAMERA_FOCUS;
+    pkt.param1 = FOCUS_TYPE_AUTO;   // Focus Type, 0:FOCUS_TYPE_STEP, 1:FOCUS_TYPE_CONTINUOUS, 2:FOCUS_TYPE_RANGE, 3:FOCUS_TYPE_METERS, 4:FOCUS_TYPE_AUTO, 5:FOCUS_TYPE_AUTO_SINGLE, 5:FOCUS_TYPE_AUTO_CONTINUOUS
+    pkt.param2 = 0;                 // Focus Value
+
+    _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);
+
+    return true;
+}
+
+// handle incoming mavlink message including CAMERA_INFORMATION
+void AP_Camera_MAVLinkCamV2::handle_message(mavlink_channel_t chan, const mavlink_message_t &msg)
+{
+    // exit immediately if this is not our message
+    if (msg.sysid != _sysid || msg.compid != _compid) {
+        return;
+    }
+
+    // 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);
+
+        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_revision = (cam_info.firmware_version & 0x00FF0000) >> 16;
+        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 %s fw:%u.%u.%u.%u",
+                cam_info.vendor_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;
+    }
+}
+
+// search for camera in GCS_MAVLink routing table
+void AP_Camera_MAVLinkCamV2::find_camera()
+{
+    // do not look for camera for first 10 seconds so user may see banner
+    uint32_t now_ms = AP_HAL::millis();
+    if (now_ms < 10000) {
+        return;
+    }
+
+    // search for camera for 60 seconds or until armed
+    if ((now_ms > AP_CAMERA_MAVLINKCAMV2_SEARCH_MS) && hal.util->get_soft_armed()) {
+        return;
+    }
+
+    // search for a mavlink enabled camera
+    if (_link == nullptr) {
+        // we expect that instance 0 has compid = MAV_COMP_ID_CAMERA, instance 1 has compid = MAV_COMP_ID_CAMERA2, etc
+        uint8_t compid = MIN(MAV_COMP_ID_CAMERA + _instance, MAV_COMP_ID_CAMERA6);
+        _link = GCS_MAVLINK::find_by_mavtype_and_compid(MAV_TYPE_CAMERA, compid, _sysid);
+        if (_link == nullptr) {
+            // have not yet found a camera so return
+            return;
+        }
+        _compid = compid;
+    }
+
+    // request CAMERA_INFORMATION
+    if (!_got_camera_info) {
+        if (now_ms - _last_caminfo_req_ms > 1000) {
+            _last_caminfo_req_ms = now_ms;
+            request_camera_information();
+        }
+        return;
+    }
+
+    _initialised = true;
+}
+
+// request CAMERA_INFORMATION (holds vendor and model name)
+void AP_Camera_MAVLinkCamV2::request_camera_information() const
+{
+    if (_link == nullptr) {
+        return;
+    }
+
+    const mavlink_command_long_t pkt {
+        MAVLINK_MSG_ID_CAMERA_INFORMATION,  // param1
+        0,  // param2
+        0,  // param3
+        0,  // param4
+        0,  // param5
+        0,  // param6
+        0,  // param7
+        MAV_CMD_REQUEST_MESSAGE,
+        _sysid,
+        _compid,
+        0  // confirmation
+    };
+
+    _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);
+}
+
+#endif // AP_CAMERA_MAVLINK_ENABLED

libraries/AP_Camera/AP_Camera_MAVLinkCamV2.h

@@ -0,0 +1,78 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+  Camera driver for cameras that implement the newer MAVLink camera v2 protocol
+  see https://mavlink.io/en/services/camera.html
+ */
+#pragma once
+
+#include "AP_Camera_Backend.h"
+
+#if AP_CAMERA_MAVLINK_ENABLED
+
+class AP_Camera_MAVLinkCamV2 : public AP_Camera_Backend
+{
+public:
+
+    // Constructor
+    using AP_Camera_Backend::AP_Camera_Backend;
+
+    /* Do not allow copies */
+    CLASS_NO_COPY(AP_Camera_MAVLinkCamV2);
+
+    // update - should be called at 50hz
+    void update() override;
+
+    // entry point to actually take a picture
+    bool trigger_pic() override;
+
+    // start or stop video recording.  returns true on success
+    // set start_recording = true to start record, false to stop recording
+    bool record_video(bool start_recording) override;
+
+    // set camera zoom step.  returns true on success
+    // zoom out = -1, hold = 0, zoom in = 1
+    bool set_zoom_step(int8_t zoom_step) override;
+
+    // set focus in, out or hold.  returns true on success
+    // focus in = -1, focus hold = 0, focus out = 1
+    bool set_manual_focus_step(int8_t focus_step) override;
+
+    // auto focus.  returns true on success
+    bool set_auto_focus() override;
+
+    // handle incoming mavlink message
+    void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) override;
+
+private:
+
+    // search for camera in GCS_MAVLink routing table
+    void find_camera();
+
+    // request CAMERA_INFORMATION (holds vendor and model name)
+    void request_camera_information() const;
+
+    // internal members
+    bool _initialised;          // true once the camera has provided a CAMERA_INFORMATION
+    bool _got_camera_info;      // true once camera has provided CAMERA_INFORMATION
+    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_MAVLINK_ENABLED

libraries/AP_Camera/AP_Camera_Params.cpp

@@ -8,7 +8,7 @@ const AP_Param::GroupInfo AP_Camera_Params::var_info[] = {
     // @Param: _TYPE
     // @DisplayName: Camera shutter (trigger) type
     // @Description: how to trigger the camera to take a picture
-    // @Values: 1:Servo,2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi), 5:MAVLink
+    // @Values: 1:Servo,2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi), 5:MAVLink, 6:MAVLinkCamV2
     // @User: Standard
     AP_GROUPINFO_FLAGS("_TYPE",  1, AP_Camera_Params, type, 0, AP_PARAM_FLAG_ENABLE),