AP_Camera: add support for camera instance in SET_CAMERA_FOCUS

libraries/AP_Camera/AP_Camera.cpp

@@ -351,6 +351,56 @@ MAV_RESULT AP_Camera::handle_mav_SET_CAMERA_ZOOM(uint8_t instance_id, CAMERA_ZOO
 
     return result;
 }
+
+MAV_RESULT AP_Camera::handle_mav_SET_CAMERA_FOCUS(uint8_t instance_id, SET_FOCUS_TYPE mav_focus_type, float focus_value)
+{
+    // note: focus_value can be modified before it is used
+
+    FocusType focus_type;
+    switch (mav_focus_type) {
+    case FOCUS_TYPE_AUTO:
+    case FOCUS_TYPE_AUTO_SINGLE:
+    case FOCUS_TYPE_AUTO_CONTINUOUS:
+        // accept any of the auto focus types
+        focus_type = FocusType::AUTO;
+        focus_value = 0;
+        break;
+    case FOCUS_TYPE_CONTINUOUS:
+        // accept continuous manual focus
+        focus_type = FocusType::RATE;
+        break;
+    case FOCUS_TYPE_RANGE:
+        // accept focus as percentage
+        focus_type = FocusType::PCT;
+        break;
+    case SET_FOCUS_TYPE_ENUM_END:
+    case FOCUS_TYPE_STEP:
+    case FOCUS_TYPE_METERS:
+    default:  // mav_focus_type comes off the wire so could be anything
+        // unsupported focus (bad parameter)
+        return MAV_RESULT_DENIED;
+    }
+
+    MAV_RESULT result = MAV_RESULT_ACCEPTED;
+    for (uint8_t i=0; i<AP_CAMERA_MAX_INSTANCES; i++) {
+        if (_backends[i] == nullptr) {
+            continue;
+        }
+        // honour packet instance number:
+        if (instance_id != 0 && i+1 != instance_id) {
+            continue;
+        }
+        // all backends must succeed.  If any fail we pass back the
+        // result from the last camera that failed.
+        const SetFocusResult backend_result = _backends[i]->set_focus(focus_type, focus_value);
+        if (backend_result != SetFocusResult::ACCEPTED) {
+            // for now FocusResult can just be cast into MAV_RESULT:
+            result = (MAV_RESULT)backend_result;
+        }
+    }
+
+    return result;
+}
 #endif  // HAL_MAVLINK_BINDINGS_ENABLED
 
 // handle command_long mavlink messages
@@ -376,25 +426,11 @@ MAV_RESULT AP_Camera::handle_command(const mavlink_command_int_t &packet)
             packet.param2                    // zoom level
         );
     case MAV_CMD_SET_CAMERA_FOCUS:
-        // accept any of the auto focus types
-        switch ((SET_FOCUS_TYPE)packet.param1) {
-        case FOCUS_TYPE_AUTO:
-        case FOCUS_TYPE_AUTO_SINGLE:
-        case FOCUS_TYPE_AUTO_CONTINUOUS:
-            return (MAV_RESULT)set_focus(FocusType::AUTO, 0);
-        case FOCUS_TYPE_CONTINUOUS:
-        // accept continuous manual focus
-            return (MAV_RESULT)set_focus(FocusType::RATE, packet.param2);
-        // accept focus as percentage
-        case FOCUS_TYPE_RANGE:
-            return (MAV_RESULT)set_focus(FocusType::PCT, packet.param2);
-        case SET_FOCUS_TYPE_ENUM_END:
-        case FOCUS_TYPE_STEP:
-        case FOCUS_TYPE_METERS:
-            // unsupported focus (bad parameter)
-            break;
-        }
-        return MAV_RESULT_DENIED;
+        return handle_mav_SET_CAMERA_FOCUS(
+            packet.param3,                   // instance
+            SET_FOCUS_TYPE(packet.param1),   // focus type
+            packet.param2                    // focus value
+        );
 
 #if AP_CAMERA_SET_CAMERA_SOURCE_ENABLED
     case MAV_CMD_SET_CAMERA_SOURCE:

libraries/AP_Camera/AP_Camera.h

@@ -105,6 +105,7 @@ public:
     // methods to handle mavlink-style instance-id (0 meaning all cameras)
     MAV_RESULT handle_mav_DO_SET_CAM_TRIGG_DISTANCE(uint8_t instance_id, bool trigger, float dist_m);
     MAV_RESULT handle_mav_SET_CAMERA_ZOOM(uint8_t instance_id, CAMERA_ZOOM_TYPE mav_zoom_type, float zoom_value);
+    MAV_RESULT handle_mav_SET_CAMERA_FOCUS(uint8_t instance_id, SET_FOCUS_TYPE mav_focus_type, float focus_value);
 #endif  // HAL_MAVLINK_BINDINGS_ENABLED
 
     // select which instance to send on the next deferred MSG_CAMERA_INFORMATION send