AP_Camera: Add handling of GoPro mavlink commands

libraries/AP_Camera/AP_Camera.cpp

@@ -10,6 +10,7 @@
 #include <SRV_Channel/SRV_Channel.h>
 #include <AP_Logger/AP_Logger.h>
 #include <AP_GPS/AP_GPS.h>
+#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)

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

libraries/AP_Camera/AP_Camera_SoloGimbal.cpp

@@ -0,0 +1,112 @@
+#include "AP_Camera_SoloGimbal.h"
+#include <GCS_MAVLink/GCS.h>
+
+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;
+    }
+}

libraries/AP_Camera/AP_Camera_SoloGimbal.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include <GCS_MAVLink/GCS_MAVLink.h>
+
+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;
+};