AP_Mount: Siyi fix for continuous zoom

Co-authored-by: Randy Mackay <rmackay9@yahoo.com>
ArduPilot · Aug 23, 2023 · 724f03c · 724f03c
1 parent 0bcb3ec
commit 724f03c
Show file tree

Hide file tree

Showing 2 changed files with 49 additions and 32 deletions.
diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp
@@ -699,14 +699,16 @@ float AP_Mount_Siyi::get_zoom_mult_max() const
 // set zoom specified as a rate or percentage
 bool AP_Mount_Siyi::set_zoom(ZoomType zoom_type, float zoom_value)
 {
-    if (zoom_type == ZoomType::RATE) {
-        // disable absolute zoom target
-        _zoom_mult_target = 0;
-        return send_zoom_rate(zoom_value);
-    }
-
-    // absolute zoom
-    if (zoom_type == ZoomType::PCT) {
+    switch (zoom_type) {
+    case ZoomType::RATE:
+        if (send_zoom_rate(zoom_value)) {
+            _zoom_type = zoom_type;
+            _zoom_rate_target = zoom_value;
+            return true;
+        }
+        return false;
+    case ZoomType::PCT: {
+        // absolute zoom
         float zoom_mult_max = get_zoom_mult_max();
         if (is_positive(zoom_mult_max)) {
             // convert zoom percentage (0~100) to target zoom multiple (e.g. 0~6x or 0~30x)
@@ -717,45 +719,59 @@ bool AP_Mount_Siyi::set_zoom(ZoomType zoom_type, float zoom_value)
                 return false;
             case HardwareModel::A8:
                 // set internal zoom control target
+                _zoom_type = zoom_type;
                 _zoom_mult_target = zoom_mult;
                 return true;
             case HardwareModel::ZR10:
+                _zoom_type = zoom_type;
+                _zoom_mult_target = 0;
                 return send_zoom_mult(zoom_mult);
             }
         }
+        return false;
+    }
     }
 
     // unsupported zoom type
     return false;
 }
 
-// update absolute zoom controller
-// only used for A8 that does not support abs zoom control
+// update zoom controller
 void AP_Mount_Siyi::update_zoom_control()
 {
-    // exit immediately if no target
-    if (!is_positive(_zoom_mult_target)) {
-        return;
-    }
-
-    // limit update rate to 20hz
     const uint32_t now_ms = AP_HAL::millis();
-    if ((now_ms - _last_zoom_control_ms) <= 50) {
-        return;
-    }
-    _last_zoom_control_ms = now_ms;
+    const uint32_t update_diff_ms = now_ms - _last_zoom_control_ms;
 
-    // zoom towards target zoom multiple
-    if (_zoom_mult_target > _zoom_mult + 0.1f) {
-        send_zoom_rate(1);
-    } else if (_zoom_mult_target < _zoom_mult - 0.1f) {
-        send_zoom_rate(-1);
-    } else {
-        send_zoom_rate(0);
-        _zoom_mult_target = 0;
+    switch (_zoom_type) {
+    case ZoomType::RATE:
+        // limit updates to 1hz
+        if (update_diff_ms < 1000) {
+            return;
+        }
+        // only send zoom rate target if it's non-zero because if zero it has already been sent
+        // and sending zero rate also triggers autofocus
+        if (!is_zero(_zoom_rate_target)) {
+            send_zoom_rate(_zoom_rate_target);
+        }
+        _last_zoom_control_ms = now_ms;
+        return;
+    case ZoomType::PCT:
+        // limit updates to 20hz and only when we have a zoom target
+        if (!is_positive(_zoom_mult_target) || (update_diff_ms < 50)) {
+            return;
+        }
+        // zoom towards target zoom multiple
+        if (_zoom_mult_target > _zoom_mult + 0.1f) {
+            send_zoom_rate(1);
+        } else if (_zoom_mult_target < _zoom_mult - 0.1f) {
+            send_zoom_rate(-1);
+        } else {
+            send_zoom_rate(0);
+            _zoom_mult_target = 0;
+        }
+        _last_zoom_control_ms = now_ms;
+        break;
     }
-
-    debug("Siyi zoom targ:%f act:%f", (double)_zoom_mult_target, (double)_zoom_mult);
 }
 
 // set focus specified as rate, percentage or auto

diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h
@@ -195,8 +195,7 @@ class AP_Mount_Siyi : public AP_Mount_Backend
     // get zoom multiple max
     float get_zoom_mult_max() const;
 
-    // update absolute zoom controller
-    // only used for A8 that does not support abs zoom control
+    // update zoom controller
     void update_zoom_control();
 
     // internal variables
@@ -238,6 +237,8 @@ class AP_Mount_Siyi : public AP_Mount_Backend
     bool _last_record_video;                        // last record_video state sent to gimbal
 
     // absolute zoom control.  only used for A8 that does not support abs zoom control
+    ZoomType _zoom_type;                            // current zoom type
+    float _zoom_rate_target;                        // current zoom rate target
     float _zoom_mult_target;                        // current zoom multiple target.  0 if no target
     float _zoom_mult;                               // most recent actual zoom multiple received from camera
     uint32_t _last_zoom_control_ms;                 // system time that zoom control was last run