From 2cc080620ccd2f23a220e2aa269207d2377f099d Mon Sep 17 00:00:00 2001
From: Asif Khan <asif.kgauri@gmail.com>
Date: Thu, 7 Sep 2023 15:35:40 +0530
Subject: [PATCH] AP_Camera: fix MAV_CMD_START_CAPTURE handling

---
 libraries/AP_Camera/AP_Camera.cpp | 93 +++++++++++++++++++++----------
 libraries/AP_Camera/AP_Camera.h   |  3 +-
 2 files changed, 66 insertions(+), 30 deletions(-)

diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp
index f3551bc5f1..eb6aaff877 100644
--- a/libraries/AP_Camera/AP_Camera.cpp
+++ b/libraries/AP_Camera/AP_Camera.cpp
@@ -81,10 +81,24 @@ void AP_Camera::take_picture()
 {
     WITH_SEMAPHORE(_rsem);
 
-    if (primary == nullptr) {
-        return;
+    // call for each instance
+    for (uint8_t i = 0; i < AP_CAMERA_MAX_INSTANCES; i++) {
+        if (_backends[i] != nullptr) {
+            _backends[i]->take_picture();
+        }
     }
-    primary->take_picture();
+}
+
+bool AP_Camera::take_picture(uint8_t instance)
+{
+    WITH_SEMAPHORE(_rsem);
+
+    auto *backend = get_instance(instance);
+    if (backend == nullptr) {
+        return false;
+    }
+    backend->take_picture();
+    return true;
 }
 
 // take multiple pictures, time_interval between two consecutive pictures is in miliseconds
@@ -93,10 +107,24 @@ void AP_Camera::take_multiple_pictures(uint32_t time_interval_ms, int16_t total_
 {
     WITH_SEMAPHORE(_rsem);
 
-    if (primary == nullptr) {
-        return;
+    // call for all instances
+    for (uint8_t i = 0; i < AP_CAMERA_MAX_INSTANCES; i++) {
+        if (_backends[i] != nullptr) {
+            _backends[i]->take_multiple_pictures(time_interval_ms, total_num);
+        }
     }
-    primary->take_multiple_pictures(time_interval_ms, total_num);
+}
+
+bool AP_Camera::take_multiple_pictures(uint8_t instance, uint32_t time_interval_ms, int16_t total_num)
+{
+    WITH_SEMAPHORE(_rsem);
+
+    auto *backend = get_instance(instance);
+    if (backend == nullptr) {
+        return false;
+    }
+    backend->take_multiple_pictures(time_interval_ms, total_num);
+    return true;
 }
 
 // start/stop recording video
@@ -251,17 +279,38 @@ MAV_RESULT AP_Camera::handle_command_long(const mavlink_command_long_t &packet)
         }
         return MAV_RESULT_DENIED;
     case MAV_CMD_IMAGE_START_CAPTURE:
-        if (!is_zero(packet.param2) || !is_equal(packet.param3, 1.0f) || !is_zero(packet.param4)) {
-            // Its a multiple picture request
-            if (is_equal(packet.param3, 0.0f)) {
-                take_multiple_pictures(packet.param2*1000, -1);
-            } else {
-                take_multiple_pictures(packet.param2*1000, packet.param3);
+        // check if this is a single picture request
+        if (is_equal(packet.param3, 1.0f)) {
+            if (is_zero(packet.param1)) {
+                // take pictures for every backend
+                take_picture();
+                return MAV_RESULT_ACCEPTED;
+            }
+            if (take_picture(packet.param1-1)) {
+                return MAV_RESULT_ACCEPTED;
+            }
+        } else if (is_zero(packet.param3)) {
+            // multiple picture request, take pictures forever
+            if (is_zero(packet.param1)) {
+                // take pictures for every backend
+                take_multiple_pictures(packet.param2*1000, -1);
+                return MAV_RESULT_ACCEPTED;
+            }
+            if (take_multiple_pictures(packet.param1-1,packet.param2*1000, -1)) {
+                return MAV_RESULT_ACCEPTED;
+            }
+        } else {
+            // take multiple pictures equal to the number specified in param3
+            if (is_zero(packet.param1)) {
+                // take pictures for every backend
+                take_multiple_pictures(packet.param2*1000, packet.param3);
+                return MAV_RESULT_ACCEPTED;
+            }
+            if (take_multiple_pictures(packet.param1-1,packet.param2*1000, packet.param3)) {
+                return MAV_RESULT_ACCEPTED;
             }
-            return MAV_RESULT_ACCEPTED;
         }
-        take_picture();
-        return MAV_RESULT_ACCEPTED;
+        return MAV_RESULT_UNSUPPORTED;
     case MAV_CMD_CAMERA_TRACK_POINT:
         if (set_tracking(TrackingType::TRK_POINT, Vector2f{packet.param1, packet.param2}, Vector2f{})) {
             return MAV_RESULT_ACCEPTED;
@@ -433,20 +482,6 @@ void AP_Camera::update()
     }
 }
 
-// take_picture - take a picture
-void AP_Camera::take_picture(uint8_t instance)
-{
-    WITH_SEMAPHORE(_rsem);
-
-    auto *backend = get_instance(instance);
-    if (backend == nullptr) {
-        return;
-    }
-
-    // call backend
-    backend->take_picture();
-}
-
 // start/stop recording video.  returns true on success
 // start_recording should be true to start recording, false to stop recording
 bool AP_Camera::record_video(uint8_t instance, bool start_recording)
diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h
index 631651147f..78d0d640c4 100644
--- a/libraries/AP_Camera/AP_Camera.h
+++ b/libraries/AP_Camera/AP_Camera.h
@@ -113,11 +113,12 @@ public:
 
     // take a picture
     void take_picture();
-    void take_picture(uint8_t instance);
+    bool take_picture(uint8_t instance);
 
     // take multiple pictures, time_interval between two consecutive pictures is in miliseconds
     // total_num is number of pictures to be taken, -1 means capture forever
     void take_multiple_pictures(uint32_t time_interval_ms, int16_t total_num);
+    bool take_multiple_pictures(uint8_t instance, uint32_t time_interval_ms, int16_t total_num);
 
     // start/stop recording video
     // start_recording should be true to start recording, false to stop recording