From 5f0bd3d4875db58b590634d5164a4770d0ed8b83 Mon Sep 17 00:00:00 2001
From: Gus Grubba <mavlink@grubba.com>
Date: Mon, 13 Feb 2017 23:02:33 -0500
Subject: [PATCH] Experimenting with Video / Photo commands

---
 src/uas/UAS.cc         | 45 +++++++++++++++++++++++++++++++++++-------
 src/uas/UAS.h          |  4 ++++
 src/uas/UASInterface.h |  4 ++++
 3 files changed, 46 insertions(+), 7 deletions(-)

diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 6be68ccaf..bbbd9acc8 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -1209,15 +1209,46 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll
 */
 void UAS::pairRX(int rxType, int rxSubType)
 {
-    if (!_vehicle) {
-        return;
+    if (_vehicle) {
+        _vehicle->sendMavCommand(_vehicle->defaultComponentId(),    // target component
+                                 MAV_CMD_IMAGE_START_CAPTURE,       // command id
+                                 MAV_CMD_START_RX_PAIR,             // command id
+                                 true,                              // showError
+                                 rxType,
+                                 rxSubType);
     }
+}
 
-    _vehicle->sendMavCommand(_vehicle->defaultComponentId(),    // target component
-                             MAV_CMD_START_RX_PAIR,             // command id
-                             true,                              // showError
-                             rxType,
-                             rxSubType);
+/**
+* Order the robot to take a picture (Testing -- Incomplete API)
+*/
+void UAS::takePhoto()
+{
+    if (_vehicle) {
+        _vehicle->sendMavCommand(_vehicle->defaultComponentId(),    // target component
+                                 MAV_CMD_IMAGE_START_CAPTURE,       // command id
+                                 0,                                 // Duration between two consecutive pictures (in seconds)
+                                 1,                                 // Number of images to capture total - 0 for unlimited capture
+                                 0,                                 // Resolution in megapixels (0.3 for 640x480, 1.3 for 1280x720, etc), set to 0 if param 4/5 are used
+                                 1920,                              // Resolution horizontal in pixels
+                                 1080);                             // Resolution horizontal in pixels
+    }
+}
+
+/**
+* Order the robot to toggle video recording (Testing -- Incomplete API)
+*/
+void UAS::toggleVideo()
+{
+    if (_vehicle) {
+        _vehicle->sendMavCommand(_vehicle->defaultComponentId(),    // target component
+                                 MAV_CMD_VIDEO_START_CAPTURE,       // command id
+                                 0,                                 // Camera ID (0 for all cameras), 1 for first, 2 for second, etc.
+                                 60,                                // Frames per second
+                                 0,                                 // Resolution in megapixels (0.3 for 640x480, 1.3 for 1280x720, etc), set to 0 if param 4/5 are used
+                                 1920,                              // Resolution horizontal in pixels
+                                 1080);                             // Resolution horizontal in pixels
+    }
 }
 
 /**
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index 4dad4d5fd..4820fb41a 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -262,6 +262,10 @@ public:
 public slots:
     /** @brief Order the robot to pair its receiver **/
     void pairRX(int rxType, int rxSubType);
+    /** @brief Order the robot to take a picture **/
+    void takePhoto();
+    /** @brief Order the robot to toggle video recording **/
+    void toggleVideo();
 
     /** @brief Enable / disable HIL */
 #ifndef __mobile__
diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h
index 8ce2460b9..515fb599b 100644
--- a/src/uas/UASInterface.h
+++ b/src/uas/UASInterface.h
@@ -137,6 +137,10 @@ public:
 public slots:
     /** @brief Order the robot to pair its receiver **/
     virtual void pairRX(int rxType, int rxSubType) = 0;
+    /** @brief Order the robot to take a picture **/
+    virtual void takePhoto() {}
+    /** @brief Order the robot to toggle video recording **/
+    virtual void toggleVideo() {}
 
     /** @brief Send the full HIL state to the MAV */
 #ifndef __mobile__
-- 
2.22.0