mirror of https://github.com/ArduPilot/ardupilot
117 lines
5.1 KiB
C++
117 lines
5.1 KiB
C++
#include "AP_Camera_SoloGimbal.h"
|
|
#include <GCS_MAVLink/GCS.h>
|
|
|
|
#if HAL_SOLO_GIMBAL_ENABLED
|
|
|
|
GOPRO_CAPTURE_MODE AP_Camera_SoloGimbal::gopro_capture_mode;
|
|
GOPRO_HEARTBEAT_STATUS AP_Camera_SoloGimbal::gopro_status;
|
|
bool AP_Camera_SoloGimbal::gopro_is_recording;
|
|
mavlink_channel_t AP_Camera_SoloGimbal::heartbeat_channel;
|
|
|
|
// Toggle the shutter on the GoPro
|
|
// This is so ArduPilot can toggle the shutter directly, either for mission/GCS commands, or when the
|
|
// Solo's gimbal is installed on a vehicle other than a Solo. The usual GoPro controls thorugh the
|
|
// Solo app and Solo controller do not use this, as it is done offboard on the companion computer.
|
|
void AP_Camera_SoloGimbal::gopro_shutter_toggle()
|
|
{
|
|
if (gopro_status != GOPRO_HEARTBEAT_STATUS_CONNECTED) {
|
|
gcs().send_text(MAV_SEVERITY_ERROR, "GoPro Not Available");
|
|
return;
|
|
}
|
|
|
|
const uint8_t gopro_shutter_start[4] = { 1, 0, 0, 0};
|
|
const uint8_t gopro_shutter_stop[4] = { 0, 0, 0, 0};
|
|
|
|
if (gopro_capture_mode == GOPRO_CAPTURE_MODE_PHOTO) {
|
|
// Trigger shutter start to take a photo
|
|
gcs().send_text(MAV_SEVERITY_INFO, "GoPro Photo Trigger");
|
|
mavlink_msg_gopro_set_request_send(heartbeat_channel, mavlink_system.sysid, MAV_COMP_ID_GIMBAL,GOPRO_COMMAND_SHUTTER,gopro_shutter_start);
|
|
|
|
} else if (gopro_capture_mode == GOPRO_CAPTURE_MODE_VIDEO) {
|
|
if (gopro_is_recording) {
|
|
// GoPro is recording, so stop recording
|
|
gcs().send_text(MAV_SEVERITY_INFO, "GoPro Recording Stop");
|
|
mavlink_msg_gopro_set_request_send(heartbeat_channel, mavlink_system.sysid, MAV_COMP_ID_GIMBAL,GOPRO_COMMAND_SHUTTER,gopro_shutter_stop);
|
|
} else {
|
|
// GoPro is not recording, so start recording
|
|
gcs().send_text(MAV_SEVERITY_INFO, "GoPro Recording Start");
|
|
mavlink_msg_gopro_set_request_send(heartbeat_channel, mavlink_system.sysid, MAV_COMP_ID_GIMBAL,GOPRO_COMMAND_SHUTTER,gopro_shutter_start);
|
|
}
|
|
} else {
|
|
gcs().send_text(MAV_SEVERITY_ERROR, "GoPro Unsupported Capture Mode");
|
|
}
|
|
}
|
|
|
|
// Cycle the GoPro capture mode
|
|
// This is so ArduPilot can cycle through the capture modes of the GoPro directly, probably with an RC Aux function.
|
|
// This is primarily for Solo's gimbal being installed on a vehicle other than a Solo. The usual GoPro controls
|
|
// through the Solo app and Solo controller do not use this, as it is done offboard on the companion computer.
|
|
void AP_Camera_SoloGimbal::gopro_capture_mode_toggle()
|
|
{
|
|
uint8_t gopro_capture_mode_values[4] = { };
|
|
|
|
if (gopro_status != GOPRO_HEARTBEAT_STATUS_CONNECTED) {
|
|
gcs().send_text(MAV_SEVERITY_ERROR, "GoPro Not Available");
|
|
return;
|
|
}
|
|
|
|
switch(gopro_capture_mode) {
|
|
case GOPRO_CAPTURE_MODE_VIDEO:
|
|
if (gopro_is_recording) {
|
|
// GoPro is recording, cannot change modes
|
|
gcs().send_text(MAV_SEVERITY_INFO, "GoPro recording, can't change modes");
|
|
} else {
|
|
// Change to camera mode
|
|
gopro_capture_mode_values[0] = GOPRO_CAPTURE_MODE_PHOTO;
|
|
mavlink_msg_gopro_set_request_send(heartbeat_channel, mavlink_system.sysid, MAV_COMP_ID_GIMBAL,GOPRO_COMMAND_CAPTURE_MODE,gopro_capture_mode_values);
|
|
gcs().send_text(MAV_SEVERITY_INFO, "GoPro changing to mode photo");
|
|
}
|
|
break;
|
|
|
|
case GOPRO_CAPTURE_MODE_PHOTO:
|
|
default:
|
|
// Change to video mode
|
|
gopro_capture_mode_values[0] = GOPRO_CAPTURE_MODE_VIDEO;
|
|
mavlink_msg_gopro_set_request_send(heartbeat_channel, mavlink_system.sysid, MAV_COMP_ID_GIMBAL,GOPRO_COMMAND_CAPTURE_MODE,gopro_capture_mode_values);
|
|
gcs().send_text(MAV_SEVERITY_INFO, "GoPro changing to mode video");
|
|
break;
|
|
}
|
|
}
|
|
|
|
// heartbeat from the Solo gimbal GoPro
|
|
void AP_Camera_SoloGimbal::handle_gopro_heartbeat(mavlink_channel_t chan, const mavlink_message_t &msg)
|
|
{
|
|
mavlink_gopro_heartbeat_t report_msg;
|
|
mavlink_msg_gopro_heartbeat_decode(&msg, &report_msg);
|
|
gopro_is_recording = report_msg.flags & GOPRO_FLAG_RECORDING;
|
|
heartbeat_channel = chan;
|
|
|
|
switch((GOPRO_HEARTBEAT_STATUS)report_msg.status) {
|
|
case GOPRO_HEARTBEAT_STATUS_DISCONNECTED:
|
|
case GOPRO_HEARTBEAT_STATUS_INCOMPATIBLE:
|
|
case GOPRO_HEARTBEAT_STATUS_ERROR:
|
|
case GOPRO_HEARTBEAT_STATUS_CONNECTED:
|
|
gopro_status = (GOPRO_HEARTBEAT_STATUS)report_msg.status;
|
|
break;
|
|
case GOPRO_HEARTBEAT_STATUS_ENUM_END:
|
|
break;
|
|
}
|
|
|
|
switch((GOPRO_CAPTURE_MODE)report_msg.capture_mode){
|
|
case GOPRO_CAPTURE_MODE_VIDEO:
|
|
case GOPRO_CAPTURE_MODE_PHOTO:
|
|
case GOPRO_CAPTURE_MODE_BURST:
|
|
case GOPRO_CAPTURE_MODE_TIME_LAPSE:
|
|
case GOPRO_CAPTURE_MODE_MULTI_SHOT:
|
|
case GOPRO_CAPTURE_MODE_PLAYBACK:
|
|
case GOPRO_CAPTURE_MODE_SETUP:
|
|
case GOPRO_CAPTURE_MODE_UNKNOWN:
|
|
gopro_capture_mode = (GOPRO_CAPTURE_MODE)report_msg.capture_mode;
|
|
break;
|
|
case GOPRO_CAPTURE_MODE_ENUM_END:
|
|
break;
|
|
}
|
|
}
|
|
|
|
#endif // HAL_SOLO_GIMBAL_ENABLED
|