Camera: image start/stop capture fixes

libraries/AP_Camera/AP_Camera.cpp

@@ -77,16 +77,20 @@ void AP_Camera::cam_mode_toggle()
 }
 
 // take a picture
-void AP_Camera::take_picture()
+bool AP_Camera::take_picture()
 {
     WITH_SEMAPHORE(_rsem);
 
     // call for each instance
+    bool success = false;
     for (uint8_t i = 0; i < AP_CAMERA_MAX_INSTANCES; i++) {
         if (_backends[i] != nullptr) {
-            _backends[i]->take_picture();
+            success |= _backends[i]->take_picture();
         }
     }
+
+    // return true if at least once pic taken
+    return success;
 }
 
 bool AP_Camera::take_picture(uint8_t instance)
@@ -97,28 +101,45 @@ bool AP_Camera::take_picture(uint8_t instance)
     if (backend == nullptr) {
         return false;
     }
-    backend->take_picture();
-    return true;
+    return backend->take_picture();
 }
 
 // take multiple pictures, time_interval between two consecutive pictures is in miliseconds
+// if instance is not provided, all available cameras affected
+// time_interval_ms must be positive
 // total_num is number of pictures to be taken, -1 means capture forever
-void AP_Camera::take_multiple_pictures(uint32_t time_interval_ms, int16_t total_num)
+// returns true if at least one camera is successful
+bool AP_Camera::take_multiple_pictures(uint32_t time_interval_ms, int16_t total_num)
 {
     WITH_SEMAPHORE(_rsem);
 
+    // sanity check time interval
+    if (time_interval_ms == 0) {
+        return false;
+    }
+
     // call for all instances
+    bool success = false;
     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);
+            success = true;
         }
     }
+
+    // return true if at least once backend was successful
+    return success;
 }
 
 bool AP_Camera::take_multiple_pictures(uint8_t instance, uint32_t time_interval_ms, int16_t total_num)
 {
     WITH_SEMAPHORE(_rsem);
 
+    // sanity check time interval
+    if (time_interval_ms == 0) {
+        return false;
+    }
+
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return false;
@@ -304,45 +325,48 @@ MAV_RESULT AP_Camera::handle_command_long(const mavlink_command_long_t &packet)
         }
         return MAV_RESULT_DENIED;
     case MAV_CMD_IMAGE_START_CAPTURE:
-        // check if this is a single picture request
-        if (is_equal(packet.param3, 1.0f)) {
+        // param1 : camera id
+        // param2 : interval (in seconds)
+        // param3 : total num images
+        // sanity check instance
+        if (is_negative(packet.param1)) {
+            return MAV_RESULT_UNSUPPORTED;
+        }
+        // check if this is a single picture request (e.g. total images is 1 or interval and total images are zero)
+        if (is_equal(packet.param3, 1.0f) ||
+            (is_zero(packet.param2) && is_zero(packet.param3))) {
             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;
+                return take_picture() ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
             }
+            // take picture for specified instance
+            return take_picture(packet.param1-1) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
         } 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;
+                return take_multiple_pictures(packet.param2*1000, -1) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
             }
+            return take_multiple_pictures(packet.param1-1, packet.param2*1000, -1) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
         } 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 take_multiple_pictures(packet.param2*1000, packet.param3) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
             }
+            return take_multiple_pictures(packet.param1-1, packet.param2*1000, packet.param3) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
         }
-        return MAV_RESULT_UNSUPPORTED;
     case MAV_CMD_IMAGE_STOP_CAPTURE:
+        // param1 : camera id
+        if (is_negative(packet.param1)) {
+            return MAV_RESULT_UNSUPPORTED;
+        }
         if (is_zero(packet.param1)) {
             // stop capture for every backend
             stop_capture();
             return MAV_RESULT_ACCEPTED;
         }
-        if (stop_capture(packet.param1)) {
+        if (stop_capture(packet.param1-1)) {
             return MAV_RESULT_ACCEPTED;
         }
         return MAV_RESULT_UNSUPPORTED;

libraries/AP_Camera/AP_Camera.h

@@ -111,13 +111,17 @@ class AP_Camera {
     void cam_mode_toggle();
     void cam_mode_toggle(uint8_t instance);
 
-    // take a picture
-    void take_picture();
+    // take a picture.  If instance is not provided, all available cameras affected
+    // returns true if at least one camera took a picture
+    bool take_picture();
     bool take_picture(uint8_t instance);
 
     // take multiple pictures, time_interval between two consecutive pictures is in miliseconds
+    // if instance is not provided, all available cameras affected
+    // time_interval_ms must be positive
     // 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);
+    // returns true if at least one camera is successful
+    bool 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);
 
     // stop capturing multiple image sequence

libraries/AP_Camera/AP_Camera_Mount.cpp

@@ -10,8 +10,7 @@ bool AP_Camera_Mount::trigger_pic()
 {
     AP_Mount* mount = AP::mount();
     if (mount != nullptr) {
-        mount->take_picture(get_mount_instance());
-        return true;
+        return mount->take_picture(get_mount_instance());
     }
     return false;
 }

libraries/AP_Mission/AP_Mission_Commands.cpp

@@ -163,39 +163,35 @@ bool AP_Mission::start_command_camera(const AP_Mission::Mission_Command& cmd)
         return false;
 
     case MAV_CMD_IMAGE_START_CAPTURE:
-        // check if this is a single picture request
-        if (cmd.content.image_start_capture.total_num_images == 1) {
+        // check if this is a single picture request (e.g. total images is 1 or interval and total images are zero)
+        if ((cmd.content.image_start_capture.total_num_images == 1) ||
+            (cmd.content.image_start_capture.total_num_images == 0 && is_zero(cmd.content.image_start_capture.interval_s))) {
             if (cmd.content.image_start_capture.instance == 0) {
                 // take pictures for every backend
-                camera->take_picture();
-                return true;
+                return camera->take_picture();
             }
             return camera->take_picture(cmd.content.image_start_capture.instance-1);
         } else if (cmd.content.image_start_capture.total_num_images == 0) {
             // multiple picture request, take pictures forever
             if (cmd.content.image_start_capture.instance == 0) {
                 // take pictures for every backend
-                camera->take_multiple_pictures(cmd.content.image_start_capture.interval_s*1000, -1);
-                return true;
+                return camera->take_multiple_pictures(cmd.content.image_start_capture.interval_s*1000, -1);
             }
             return camera->take_multiple_pictures(cmd.content.image_start_capture.instance-1, cmd.content.image_start_capture.interval_s*1000, -1);
         } else {
             if (cmd.content.image_start_capture.instance == 0) {
                 // take pictures for every backend
-                camera->take_multiple_pictures(cmd.content.image_start_capture.interval_s*1000, cmd.content.image_start_capture.total_num_images);
-                return true;
+                return camera->take_multiple_pictures(cmd.content.image_start_capture.interval_s*1000, cmd.content.image_start_capture.total_num_images);
             }
             return camera->take_multiple_pictures(cmd.content.image_start_capture.instance-1, cmd.content.image_start_capture.interval_s*1000, cmd.content.image_start_capture.total_num_images);
         }
-        return false;
-
     case MAV_CMD_IMAGE_STOP_CAPTURE:
         if (cmd.p1 == 0) {
             // stop capture for each backend
             camera->stop_capture();
             return true;
         }
-        return camera->stop_capture(cmd.p1);
+        return camera->stop_capture(cmd.p1 - 1);
 
     case MAV_CMD_VIDEO_START_CAPTURE:
     case MAV_CMD_VIDEO_STOP_CAPTURE: