diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index e9dad0de90..0b69871864 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/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; diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 274e9f8696..014c729cc9 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/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 }; diff --git a/libraries/AP_Camera/AP_Camera_MAVLink.h b/libraries/AP_Camera/AP_Camera_MAVLink.h index 21a7308a6c..0be92f09c0 100644 --- a/libraries/AP_Camera/AP_Camera_MAVLink.h +++ b/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 diff --git a/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp b/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp new file mode 100644 index 0000000000..c7d671b7b5 --- /dev/null +++ b/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp @@ -0,0 +1,222 @@ +#include "AP_Camera_MAVLinkCamV2.h" + +#if AP_CAMERA_MAVLINK_ENABLED +#include + +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 diff --git a/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.h b/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.h new file mode 100644 index 0000000000..70bb86db3a --- /dev/null +++ b/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 . + */ + +/* + 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 diff --git a/libraries/AP_Camera/AP_Camera_Params.cpp b/libraries/AP_Camera/AP_Camera_Params.cpp index 623aac18dd..0ecec96b12 100644 --- a/libraries/AP_Camera/AP_Camera_Params.cpp +++ b/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),