AP_Camera: add and use SetFocusResult

libraries/AP_Camera/AP_Camera.cpp

@@ -220,23 +220,24 @@ MAV_RESULT AP_Camera::handle_command_long(const mavlink_command_long_t &packet)
         return MAV_RESULT_UNSUPPORTED;
     case MAV_CMD_SET_CAMERA_FOCUS:
         // accept any of the auto focus types
-        if ((is_equal(packet.param1, (float)FOCUS_TYPE_AUTO) ||
+        switch ((SET_FOCUS_TYPE)packet.param1) {
-             is_equal(packet.param1, (float)FOCUS_TYPE_AUTO_SINGLE) ||
+        case FOCUS_TYPE_AUTO:
-             is_equal(packet.param1, (float)FOCUS_TYPE_AUTO_CONTINUOUS)) &&
+        case FOCUS_TYPE_AUTO_SINGLE:
-             set_focus(FocusType::AUTO, 0)) {
+        case FOCUS_TYPE_AUTO_CONTINUOUS:
-            return MAV_RESULT_ACCEPTED;
+            return (MAV_RESULT)set_focus(FocusType::AUTO, 0);
-        }
+        case FOCUS_TYPE_CONTINUOUS:
         // accept continuous manual focus
-        if (is_equal(packet.param1, (float)FOCUS_TYPE_CONTINUOUS) &&
+            return (MAV_RESULT)set_focus(FocusType::RATE, packet.param2);
-            set_focus(FocusType::RATE, packet.param2)) {
-            return MAV_RESULT_ACCEPTED;
-        }
         // accept focus as percentage
-        if (is_equal(packet.param1, (float)FOCUS_TYPE_RANGE) &&
+        case FOCUS_TYPE_RANGE:
-            set_focus(FocusType::PCT, packet.param2)) {
+            return (MAV_RESULT)set_focus(FocusType::PCT, packet.param2);
-            return MAV_RESULT_ACCEPTED;
+        case SET_FOCUS_TYPE_ENUM_END:
+        case FOCUS_TYPE_STEP:
+        case FOCUS_TYPE_METERS:
+            // unsupported focus (bad parameter)
+            break;
         }
-        return MAV_RESULT_UNSUPPORTED;
+        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)) {
             // time interval is not supported
@@ -474,25 +475,25 @@ bool AP_Camera::set_zoom(uint8_t instance, ZoomType zoom_type, float zoom_value)
 
 // set focus specified as rate, percentage or auto
 // focus in = -1, focus hold = 0, focus out = 1
-bool AP_Camera::set_focus(FocusType focus_type, float focus_value)
+SetFocusResult AP_Camera::set_focus(FocusType focus_type, float focus_value)
 {
     WITH_SEMAPHORE(_rsem);
 
     if (primary == nullptr) {
-        return false;
+        return SetFocusResult::FAILED;
     }
     return primary->set_focus(focus_type, focus_value);
 }
 
 // set focus specified as rate, percentage or auto
 // focus in = -1, focus hold = 0, focus out = 1
-bool AP_Camera::set_focus(uint8_t instance, FocusType focus_type, float focus_value)
+SetFocusResult AP_Camera::set_focus(uint8_t instance, FocusType focus_type, float focus_value)
 {
     WITH_SEMAPHORE(_rsem);
 
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
-        return false;
+        return SetFocusResult::FAILED;
     }
 
     // call each instance

libraries/AP_Camera/AP_Camera.h

@@ -126,8 +126,8 @@ public:
 
     // set focus specified as rate, percentage or auto
     // focus in = -1, focus hold = 0, focus out = 1
-    bool set_focus(FocusType focus_type, float focus_value);
+    SetFocusResult set_focus(FocusType focus_type, float focus_value);
-    bool set_focus(uint8_t instance, FocusType focus_type, float focus_value);
+    SetFocusResult set_focus(uint8_t instance, FocusType focus_type, float focus_value);
 
     // set tracking to none, point or rectangle (see TrackingType enum)
     // if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right

libraries/AP_Camera/AP_Camera_Backend.h

@@ -68,7 +68,7 @@ public:
 
     // set focus specified as rate, percentage or auto
     // focus in = -1, focus hold = 0, focus out = 1
-    virtual bool set_focus(FocusType focus_type, float focus_value) { return false; }
+    virtual SetFocusResult set_focus(FocusType focus_type, float focus_value) { return SetFocusResult::UNSUPPORTED; }
 
     // set tracking to none, point or rectangle (see TrackingType enum)
     // if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right

libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp

@@ -92,11 +92,11 @@ bool AP_Camera_MAVLinkCamV2::set_zoom(ZoomType zoom_type, float zoom_value)
 
 // set focus specified as rate, percentage or auto
 // focus in = -1, focus hold = 0, focus out = 1
-bool AP_Camera_MAVLinkCamV2::set_focus(FocusType focus_type, float focus_value)
+SetFocusResult AP_Camera_MAVLinkCamV2::set_focus(FocusType focus_type, float focus_value)
 {
     // exit immediately if have not found camera or does not support focus
     if (_link == nullptr || !(_cam_info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS)) {
-        return false;
+        return SetFocusResult::FAILED;
     }
 
     // prepare and send message
@@ -120,7 +120,7 @@ bool AP_Camera_MAVLinkCamV2::set_focus(FocusType focus_type, float focus_value)
 
     _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);
 
-    return true;
+    return SetFocusResult::ACCEPTED;
 }
 
 // handle incoming mavlink message including CAMERA_INFORMATION

libraries/AP_Camera/AP_Camera_MAVLinkCamV2.h

@@ -48,7 +48,7 @@ public:
 
     // set focus specified as rate, percentage or auto
     // focus in = -1, focus hold = 0, focus out = 1
-    bool set_focus(FocusType focus_type, float focus_value) override;
+    SetFocusResult set_focus(FocusType focus_type, float focus_value) override;
 
     // handle MAVLink messages from the camera
     void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) override;

libraries/AP_Camera/AP_Camera_Mount.cpp

@@ -39,13 +39,13 @@ bool AP_Camera_Mount::set_zoom(ZoomType zoom_type, float zoom_value)
 
 // set focus specified as rate, percentage or auto
 // focus in = -1, focus hold = 0, focus out = 1
-bool AP_Camera_Mount::set_focus(FocusType focus_type, float focus_value)
+SetFocusResult AP_Camera_Mount::set_focus(FocusType focus_type, float focus_value)
 {
     AP_Mount* mount = AP::mount();
     if (mount != nullptr) {
         return mount->set_focus(0, focus_type, focus_value);
     }
-    return false;
+    return SetFocusResult::FAILED;
 }
 
 // send camera information message to GCS

libraries/AP_Camera/AP_Camera_Mount.h

@@ -44,7 +44,7 @@ public:
 
     // set focus specified as rate, percentage or auto
     // focus in = -1, focus hold = 0, focus out = 1
-    bool set_focus(FocusType focus_type, float focus_value) override;
+    SetFocusResult set_focus(FocusType focus_type, float focus_value) override;
 
     // send camera information message to GCS
     void send_camera_information(mavlink_channel_t chan) const override;

libraries/AP_Camera/AP_Camera_Scripting.cpp

@@ -30,11 +30,11 @@ bool AP_Camera_Scripting::set_zoom(ZoomType zoom_type, float zoom_value)
 
 // set focus specified as rate, percentage or auto
 // focus in = -1, focus hold = 0, focus out = 1
-bool AP_Camera_Scripting::set_focus(FocusType focus_type, float focus_value)
+SetFocusResult AP_Camera_Scripting::set_focus(FocusType focus_type, float focus_value)
 {
     _cam_state.focus_type = (uint8_t)focus_type;
     _cam_state.focus_value = focus_value;
-    return true;
+    return SetFocusResult::ACCEPTED;
 }
 
 // set tracking to none, point or rectangle (see TrackingType enum)

libraries/AP_Camera/AP_Camera_Scripting.h

@@ -44,7 +44,7 @@ public:
 
     // set focus specified as rate, percentage or auto
     // focus in = -1, focus hold = 0, focus out = 1
-    bool set_focus(FocusType focus_type, float focus_value) override;
+    SetFocusResult set_focus(FocusType focus_type, float focus_value) override;
 
     // set tracking to none, point or rectangle (see TrackingType enum)
     // if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right

libraries/AP_Camera/AP_Camera_shareddefs.h

@@ -20,9 +20,19 @@ enum class FocusType : uint8_t {
     AUTO = 4    // focus automatically. Same as FOCUS_TYPE_AUTO
 };
 
+// result type of set_focus.  Assumptions are made that this
+// enumeration can be cast directly to MAV_RESULT.
+enum class SetFocusResult : uint8_t {
+    ACCEPTED = 0,
+    INVALID_PARAMETERS = 2,  // supported but invalid parameters, like MAV_RESULT_DENIED
+    UNSUPPORTED = 3,
+    FAILED = 4,
+};
+
 // tracking types when tracking an object in the video stream
 enum class TrackingType : uint8_t {
     TRK_NONE = 0,       // tracking is inactive
     TRK_POINT = 1,      // tracking a point
     TRK_RECTANGLE = 2   // tracking a rectangle
 };
+