AP_Camera_MAVLinkCamV2.cpp

#include "AP_Camera_MAVLinkCamV2.h"

#if AP_CAMERA_MAVLINKCAMV2_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 || !(_cam_info.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 || !(_cam_info.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 zoom specified as a rate or percentage
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 || !(_cam_info.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;
    switch (zoom_type) {
    case ZoomType::RATE:
        pkt.param1 = ZOOM_TYPE_CONTINUOUS;
        break;
    case ZoomType::PCT:
        pkt.param1 = ZOOM_TYPE_RANGE;
        break;
    }
    pkt.param2 = zoom_value;            // Zoom Value

    _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);

    return true;
}

// set focus specified as rate, percentage or auto
// focus in = -1, focus hold = 0, focus out = 1
SetFocusResult 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 || !(_cam_info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS)) {
        return SetFocusResult::FAILED;
    }

    // prepare and send message
    mavlink_command_long_t pkt {};
    pkt.command = MAV_CMD_SET_CAMERA_FOCUS;
    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;
    }
    pkt.param2 = focus_value;

    _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);

    return SetFocusResult::ACCEPTED;
}

// 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_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: %.32s %.32s 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);

        _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]
        get_gimbal_device_id());    // gimbal_device_id uint8_t
}

// 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_MAVLINKCAMV2_ENABLED