From dffeaf089742f01202ee06dacfed29f0814af6d7 Mon Sep 17 00:00:00 2001 From: Asif Khan Date: Wed, 23 Aug 2023 00:38:19 +0530 Subject: [PATCH] AP_Mount: Siyi fix for continuous zoom Co-authored-by: Randy Mackay --- libraries/AP_Mount/AP_Mount_Siyi.cpp | 82 +++++++++++++++++----------- libraries/AP_Mount/AP_Mount_Siyi.h | 5 +- 2 files changed, 54 insertions(+), 33 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index a2910f2d9b..4bec384720 100644 --- 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,63 @@ 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: - return send_zoom_mult(zoom_mult); + if (send_zoom_mult(zoom_mult)) { + _zoom_type = zoom_type; + _zoom_mult_target = 0; + return true; + } + return false; } } + 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) { + const uint32_t update_diff_ms = now_ms - _last_zoom_control_ms; + + 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; + debug("Siyi zoom targ:%f act:%f", (double)_zoom_mult_target, (double)_zoom_mult); + break; } - _last_zoom_control_ms = now_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; - } - - 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 index c774134094..b870184d29 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/libraries/AP_Mount/AP_Mount_Siyi.h @@ -195,8 +195,7 @@ private: // 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 @@ private: 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