Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Camera: Add handling of GoPro mavlink commands

This commit is contained in:
Matt Lawrence 2020-02-09 00:47:52 -05:00 committed by Andrew Tridgell
parent 070b7c3202
commit a2af217adf
4 changed files with 173 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_Camera/AP_Camera.cpp
View File

@ -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)

11
libraries/AP_Camera/AP_Camera.h
View File

@ -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

112
libraries/AP_Camera/AP_Camera_SoloGimbal.cpp Normal file
View File

@ -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;
}
}

18
libraries/AP_Camera/AP_Camera_SoloGimbal.h Normal file
View File

@ -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;
};