AP_Mount: set_focus replaces set_manual/auto_focus

libraries/AP_Mission/AP_Mission_Commands.cpp

@@ -143,13 +143,15 @@ bool AP_Mission::start_command_camera(const AP_Mission::Mission_Command& cmd)
         if ((cmd.content.set_camera_focus.focus_type == FOCUS_TYPE_AUTO) ||
             (cmd.content.set_camera_focus.focus_type == FOCUS_TYPE_AUTO_SINGLE) ||
             (cmd.content.set_camera_focus.focus_type == FOCUS_TYPE_AUTO_CONTINUOUS)) {
-            camera->set_auto_focus();
-            return true;
+            return camera->set_focus(FocusType::AUTO, 0);
         }
-        // accept step or continuous manual focus
+        // accept continuous manual focus
         if (cmd.content.set_camera_focus.focus_type == FOCUS_TYPE_CONTINUOUS) {
-            camera->set_manual_focus_step(cmd.content.set_camera_focus.focus_value);
-            return true;
+            return camera->set_focus(FocusType::RATE, cmd.content.set_camera_focus.focus_value);
+        }
+        // accept range manual focus
+        if (cmd.content.set_camera_focus.focus_type == FOCUS_TYPE_RANGE) {
+            return camera->set_focus(FocusType::PCT, cmd.content.set_camera_focus.focus_value);
         }
         return false;
 

libraries/AP_Mount/AP_Mount.cpp

@@ -639,28 +639,17 @@ bool AP_Mount::set_zoom(uint8_t instance, ZoomType zoom_type, float zoom_value)
     return backend->set_zoom(zoom_type, zoom_value);
 }
 
-// set focus in, out or hold.  returns true on success
+// set focus specified as rate, percentage or auto
 // focus in = -1, focus hold = 0, focus out = 1
-bool AP_Mount::set_manual_focus_step(uint8_t instance, int8_t focus_step)
+bool AP_Mount::set_focus(uint8_t instance, FocusType focus_type, float focus_value)
 {
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return false;
     }
-    return backend->set_manual_focus_step(focus_step);
+    return backend->set_focus(focus_type, focus_value);
 }
 
-// auto focus.  returns true on success
-bool AP_Mount::set_auto_focus(uint8_t instance)
-{
-    auto *backend = get_instance(instance);
-    if (backend == nullptr) {
-        return false;
-    }
-    return backend->set_auto_focus();
-}
-
-
 AP_Mount_Backend *AP_Mount::get_primary() const
 {
     return get_instance(_primary);

libraries/AP_Mount/AP_Mount.h

@@ -187,12 +187,9 @@ public:
     // set zoom specified as a rate or percentage
     bool set_zoom(uint8_t instance, ZoomType zoom_type, float zoom_value);
 
-    // set focus in, out or hold
+    // set focus specified as rate, percentage or auto
     // focus in = -1, focus hold = 0, focus out = 1
-    bool set_manual_focus_step(uint8_t instance, int8_t focus_step);
-
-    // auto focus
-    bool set_auto_focus(uint8_t instance);
+    bool set_focus(uint8_t instance, FocusType focus_type, float focus_value);
 
     // parameter var table
     static const struct AP_Param::GroupInfo        var_info[];

libraries/AP_Mount/AP_Mount_Backend.h

@@ -137,12 +137,9 @@ public:
     // set zoom specified as a rate or percentage
     virtual bool set_zoom(ZoomType zoom_type, float zoom_value) { return false; }
 
-    // set focus in, out or hold.  returns true on success
+    // set focus specified as rate, percentage or auto
     // focus in = -1, focus hold = 0, focus out = 1
-    virtual bool set_manual_focus_step(int8_t focus_step) { return false; }
-
-    // auto focus.  returns true on success
-    virtual bool set_auto_focus() { return false; }
+    virtual bool set_focus(FocusType focus_type, float focus_value) { return false; }
 
 protected:
 

libraries/AP_Mount/AP_Mount_Siyi.cpp

@@ -644,17 +644,30 @@ bool AP_Mount_Siyi::set_zoom(ZoomType zoom_type, float zoom_value)
     return false;
 }
 
-// set focus in, out or hold.  returns true on success
+// set focus specified as rate, percentage or auto
 // focus in = -1, focus hold = 0, focus out = 1
-bool AP_Mount_Siyi::set_manual_focus_step(int8_t focus_step)
+bool AP_Mount_Siyi::set_focus(FocusType focus_type, float focus_value)
 {
-    return send_1byte_packet(SiyiCommandId::MANUAL_FOCUS, (uint8_t)focus_step);
-}
+    switch (focus_type) {
+    case FocusType::RATE: {
+        uint8_t focus_step = 0;
+        if (focus_value > 0) {
+            focus_step = 1;
+        } else if (focus_value < 0) {
+            // Siyi API specifies -1 should be sent as 255
+            focus_step = UINT8_MAX;
+        }
+        return send_1byte_packet(SiyiCommandId::MANUAL_FOCUS, (uint8_t)focus_step);
+    }
+    case FocusType::PCT:
+        // not supported
+        return false;
+    case FocusType::AUTO:
+        return send_1byte_packet(SiyiCommandId::AUTO_FOCUS, 1);
+    }
 
-// auto focus.  returns true on success
-bool AP_Mount_Siyi::set_auto_focus()
-{
-    return send_1byte_packet(SiyiCommandId::AUTO_FOCUS, 1);
+    // unsupported focus type
+    return false;
 }
 
 #endif // HAL_MOUNT_SIYI_ENABLED

libraries/AP_Mount/AP_Mount_Siyi.h

@@ -68,12 +68,9 @@ public:
     // set zoom specified as a rate or percentage
     bool set_zoom(ZoomType zoom_type, float zoom_value) override;
 
-    // set focus in, out or hold.  returns true on success
+    // set focus specified as rate, percentage or auto
     // focus in = -1, focus hold = 0, focus out = 1
-    bool set_manual_focus_step(int8_t focus_step) override;
-
-    // auto focus.  returns true on success
-    bool set_auto_focus() override;
+    bool set_focus(FocusType focus_type, float focus_value) override;
 
 protected: