diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index a2f2227516..a063ce18a9 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -10,6 +10,7 @@ #include #include #include +#include "AP_Camera_SoloGimbal.h" // ------------------------------ #define CAM_DEBUG DISABLED @@ -18,7 +19,7 @@ const AP_Param::GroupInfo AP_Camera::var_info[] = { // @Param: TRIGG_TYPE // @DisplayName: Camera shutter (trigger) type // @Description: how to trigger the camera to take a picture - // @Values: 0:Servo,1:Relay + // @Values: 0:Servo,1:Relay, 2:GoPro in Solo Gimbal // @User: Standard AP_GROUPINFO("TRIGG_TYPE", 0, AP_Camera, _trigger_type, AP_CAMERA_TRIGGER_DEFAULT_TRIGGER_TYPE), @@ -140,7 +141,6 @@ AP_Camera::relay_pic() } /// single entry point to take pictures -/// set send_mavlink_msg to true to send DO_DIGICAM_CONTROL message to all components void AP_Camera::trigger_pic() { setup_feedback_callback(); @@ -153,6 +153,9 @@ void AP_Camera::trigger_pic() case AP_CAMERA_TRIGGER_TYPE_RELAY: relay_pic(); // basic relay activation break; + case AP_CAMERA_TRIGGER_TYPE_GOPRO: // gopro in Solo Gimbal + AP_Camera_SoloGimbal::gopro_shutter_toggle(); + break; } log_picture(); @@ -196,6 +199,35 @@ AP_Camera::trigger_pic_cleanup() } } +void AP_Camera::handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) +{ + switch (msg.msgid) { + case MAVLINK_MSG_ID_DIGICAM_CONTROL: + control_msg(msg); + break; + case MAVLINK_MSG_ID_GOPRO_HEARTBEAT: + // heartbeat from the Solo gimbal with a GoPro + if (_trigger_type == AP_CAMERA_TRIGGER_TYPE_GOPRO) { + AP_Camera_SoloGimbal::handle_gopro_heartbeat(chan, msg); + break; + } + break; + } +} + +/// momentary switch to cycle camera modes +void AP_Camera::cam_mode_toggle() +{ + switch (_trigger_type) { + case AP_CAMERA_TRIGGER_TYPE_GOPRO: + AP_Camera_SoloGimbal::gopro_capture_mode_toggle(); + break; + default: + // no other cameras use this yet + break; + } +} + /// decode deprecated MavLink message that controls camera. void AP_Camera::control_msg(const mavlink_message_t &msg) diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 4f4baa350c..a756937adf 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -7,6 +7,7 @@ #define AP_CAMERA_TRIGGER_TYPE_SERVO 0 #define AP_CAMERA_TRIGGER_TYPE_RELAY 1 +#define AP_CAMERA_TRIGGER_TYPE_GOPRO 2 // GoPro in Solo gimbal #define AP_CAMERA_TRIGGER_DEFAULT_TRIGGER_TYPE AP_CAMERA_TRIGGER_TYPE_SERVO // default is to use servo to trigger camera @@ -41,7 +42,8 @@ public: } // MAVLink methods - void control_msg(const mavlink_message_t &msg); + void handle_message(mavlink_channel_t chan, + const mavlink_message_t &msg); void send_feedback(mavlink_channel_t chan); // Command processing @@ -55,6 +57,9 @@ public: _trigg_dist.set(distance_m); } + // momentary switch to change camera modes + void cam_mode_toggle(); + void take_picture(); // Update - to be called periodically @at least 10Hz @@ -80,7 +85,9 @@ private: static AP_Camera *_singleton; - AP_Int8 _trigger_type; // 0:Servo,1:Relay + void control_msg(const mavlink_message_t &msg); + + AP_Int8 _trigger_type; // 0:Servo,1:Relay, 2:GoPro in Solo Gimbal AP_Int8 _trigger_duration; // duration in 10ths of a second that the camera shutter is held open AP_Int8 _relay_on; // relay value to trigger camera AP_Int16 _servo_on_pwm; // PWM value to move servo to when shutter is activated diff --git a/libraries/AP_Camera/AP_Camera_SoloGimbal.cpp b/libraries/AP_Camera/AP_Camera_SoloGimbal.cpp new file mode 100644 index 0000000000..717e4d8519 --- /dev/null +++ b/libraries/AP_Camera/AP_Camera_SoloGimbal.cpp @@ -0,0 +1,112 @@ +#include "AP_Camera_SoloGimbal.h" +#include + +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; + } +} diff --git a/libraries/AP_Camera/AP_Camera_SoloGimbal.h b/libraries/AP_Camera/AP_Camera_SoloGimbal.h new file mode 100644 index 0000000000..08ffdb1902 --- /dev/null +++ b/libraries/AP_Camera/AP_Camera_SoloGimbal.h @@ -0,0 +1,18 @@ +#pragma once + +#include + +class AP_Camera_SoloGimbal { +public: + + static void gopro_shutter_toggle(); + static void gopro_capture_mode_toggle(); + static void handle_gopro_heartbeat(mavlink_channel_t chan, const mavlink_message_t &msg); + +private: + + static GOPRO_CAPTURE_MODE gopro_capture_mode; + static GOPRO_HEARTBEAT_STATUS gopro_status; + static bool gopro_is_recording; + static mavlink_channel_t heartbeat_channel; +};