2023-03-03 02:50:07 -04:00
|
|
|
#include "AP_Camera_MAVLinkCamV2.h"
|
|
|
|
|
2023-04-04 03:36:54 -03:00
|
|
|
#if AP_CAMERA_MAVLINKCAMV2_ENABLED
|
2023-03-03 02:50:07 -04:00
|
|
|
#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
|
2023-06-09 23:17:12 -03:00
|
|
|
if (_link == nullptr || !(_cam_info.flags & CAMERA_CAP_FLAGS_CAPTURE_IMAGE)) {
|
2023-03-03 02:50:07 -04:00
|
|
|
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
|
2023-06-09 23:17:12 -03:00
|
|
|
if (_link == nullptr || !(_cam_info.flags & CAMERA_CAP_FLAGS_CAPTURE_VIDEO)) {
|
2023-03-03 02:50:07 -04:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
2023-04-06 21:49:46 -03:00
|
|
|
// set zoom specified as a rate or percentage
|
2023-04-18 22:06:16 -03:00
|
|
|
bool AP_Camera_MAVLinkCamV2::set_zoom(ZoomType zoom_type, float zoom_value)
|
2023-03-03 02:50:07 -04:00
|
|
|
{
|
|
|
|
// exit immediately if have not found camera or does not support zoom
|
2023-06-09 23:17:12 -03:00
|
|
|
if (_link == nullptr || !(_cam_info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM)) {
|
2023-03-03 02:50:07 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// prepare and send message
|
|
|
|
mavlink_command_long_t pkt {};
|
|
|
|
pkt.command = MAV_CMD_SET_CAMERA_ZOOM;
|
2023-04-06 21:49:46 -03:00
|
|
|
switch (zoom_type) {
|
2023-04-18 22:06:16 -03:00
|
|
|
case ZoomType::RATE:
|
2023-04-06 21:49:46 -03:00
|
|
|
pkt.param1 = ZOOM_TYPE_CONTINUOUS;
|
|
|
|
break;
|
2023-04-18 22:06:16 -03:00
|
|
|
case ZoomType::PCT:
|
2023-04-06 21:49:46 -03:00
|
|
|
pkt.param1 = ZOOM_TYPE_RANGE;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
pkt.param2 = zoom_value; // Zoom Value
|
2023-03-03 02:50:07 -04:00
|
|
|
|
|
|
|
_link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2023-04-24 09:23:07 -03:00
|
|
|
// set focus specified as rate, percentage or auto
|
2023-03-03 02:50:07 -04:00
|
|
|
// focus in = -1, focus hold = 0, focus out = 1
|
2023-06-21 03:03:39 -03:00
|
|
|
SetFocusResult AP_Camera_MAVLinkCamV2::set_focus(FocusType focus_type, float focus_value)
|
2023-03-03 02:50:07 -04:00
|
|
|
{
|
|
|
|
// exit immediately if have not found camera or does not support focus
|
2023-06-09 23:17:12 -03:00
|
|
|
if (_link == nullptr || !(_cam_info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS)) {
|
2023-06-21 03:03:39 -03:00
|
|
|
return SetFocusResult::FAILED;
|
2023-03-03 02:50:07 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// prepare and send message
|
|
|
|
mavlink_command_long_t pkt {};
|
|
|
|
pkt.command = MAV_CMD_SET_CAMERA_FOCUS;
|
2023-04-24 09:23:07 -03:00
|
|
|
switch (focus_type) {
|
|
|
|
case FocusType::RATE:
|
|
|
|
// focus in, out or hold (focus in = -1, hold = 0, focus out = 1). Same as FOCUS_TYPE_CONTINUOUS
|
|
|
|
pkt.param1 = FOCUS_TYPE_CONTINUOUS;
|
|
|
|
break;
|
|
|
|
case FocusType::PCT:
|
|
|
|
// focus to a percentage (from 0 to 100) of the full range. Same as FOCUS_TYPE_RANGE
|
|
|
|
pkt.param1 = FOCUS_TYPE_RANGE;
|
|
|
|
break;
|
|
|
|
case FocusType::AUTO:
|
|
|
|
// focus automatically. Same as FOCUS_TYPE_AUTO
|
|
|
|
pkt.param1 = FOCUS_TYPE_AUTO;
|
|
|
|
break;
|
2023-03-03 02:50:07 -04:00
|
|
|
}
|
2023-04-24 09:23:07 -03:00
|
|
|
pkt.param2 = focus_value;
|
2023-03-03 02:50:07 -04:00
|
|
|
|
|
|
|
_link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);
|
|
|
|
|
2023-06-21 03:03:39 -03:00
|
|
|
return SetFocusResult::ACCEPTED;
|
2023-03-03 02:50:07 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// 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) {
|
2023-06-09 23:17:12 -03:00
|
|
|
mavlink_msg_camera_information_decode(&msg, &_cam_info);
|
2023-03-03 02:50:07 -04:00
|
|
|
|
2023-06-09 23:17:12 -03:00
|
|
|
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;
|
2023-03-03 02:50:07 -04:00
|
|
|
|
|
|
|
// display camera info to user
|
2023-04-04 03:41:23 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Camera: %s.32 %s.32 fw:%u.%u.%u.%u",
|
2023-06-09 23:17:12 -03:00
|
|
|
_cam_info.vendor_name,
|
|
|
|
_cam_info.model_name,
|
2023-03-03 02:50:07 -04:00
|
|
|
(unsigned)fw_ver_major,
|
|
|
|
(unsigned)fw_ver_minor,
|
|
|
|
(unsigned)fw_ver_revision,
|
|
|
|
(unsigned)fw_ver_build);
|
|
|
|
|
|
|
|
_got_camera_info = true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2023-06-09 23:17:12 -03:00
|
|
|
// 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]
|
|
|
|
}
|
|
|
|
|
2023-03-03 02:50:07 -04:00
|
|
|
// 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);
|
|
|
|
}
|
|
|
|
|
2023-04-04 03:36:54 -03:00
|
|
|
#endif // AP_CAMERA_MAVLINKCAMV2_ENABLED
|