From de9024b63371028e123743eb163ac52afadd3b2c Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Mon, 17 Apr 2023 00:40:18 +0200 Subject: [PATCH] AP_Mount: Siyi supports absolute zoom control Co-authored-by: davidsastresas Co-authored-by: Randy Mackay --- libraries/AP_Mount/AP_Mount_Siyi.cpp | 113 ++++++++++++++++++++++++--- libraries/AP_Mount/AP_Mount_Siyi.h | 29 ++++++- 2 files changed, 131 insertions(+), 11 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index 96c47953c2..c51bbdf87b 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -66,6 +66,9 @@ void AP_Mount_Siyi::update() _last_req_current_angle_rad_ms = now_ms; } + // run zoom control + update_zoom_control(); + // update based on mount mode switch (get_mode()) { // move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode? @@ -304,6 +307,9 @@ void AP_Mount_Siyi::process_packet() } _got_firmware_version = true; + // set hardware version based on message length + _hardware_model = (_parsed_msg.data_bytes_received <= 8) ? HardwareModel::A8 : HardwareModel::ZR10; + // display camera firmware version debug("Mount: SiyiCam fw:%u.%u.%u", (unsigned)_msg_buff[_msg_buff_data_start+2], // firmware major version @@ -349,8 +355,8 @@ void AP_Mount_Siyi::process_packet() #endif break; } - const float zoom_mult = UINT16_VALUE(_msg_buff[_msg_buff_data_start+1], _msg_buff[_msg_buff_data_start]) * 0.1; - debug("ZoomMult:%4.1f", (double)zoom_mult); + _zoom_mult = UINT16_VALUE(_msg_buff[_msg_buff_data_start+1], _msg_buff[_msg_buff_data_start]) * 0.1; + debug("ZoomMult:%4.1f", (double)_zoom_mult); break; } @@ -625,25 +631,112 @@ bool AP_Mount_Siyi::record_video(bool start_recording) return ret; } +// send zoom rate command to camera. zoom out = -1, hold = 0, zoom in = 1 +bool AP_Mount_Siyi::send_zoom_rate(float zoom_value) +{ + uint8_t zoom_step = 0; + if (zoom_value > 0) { + // zoom in + zoom_step = 1; + } + if (zoom_value < 0) { + // zoom out. Siyi API specifies -1 should be sent as 255 + zoom_step = UINT8_MAX; + } + return send_1byte_packet(SiyiCommandId::MANUAL_ZOOM_AND_AUTO_FOCUS, zoom_step); +} + +// send zoom multiple command to camera. e.g. 1x, 10x, 30x +// only works on ZR10 and ZR30 +bool AP_Mount_Siyi::send_zoom_mult(float zoom_mult) +{ + // separate zoom_mult into integral and fractional parts + float intpart; + uint8_t fracpart = (uint8_t)constrain_int16(modf(zoom_mult, &intpart) * 10, 0, UINT8_MAX); + + // create and send 2 byte array + const uint8_t zoom_mult_data[] {(uint8_t)(intpart), fracpart}; + return send_packet(SiyiCommandId::ABSOLUTE_ZOOM, zoom_mult_data, ARRAY_SIZE(zoom_mult_data)); +} + +// get zoom multiple max +float AP_Mount_Siyi::get_zoom_mult_max() const +{ + switch (_hardware_model) { + case HardwareModel::UNKNOWN: + return 0; + case HardwareModel::A8: + // a8 has 6x digital zoom + return 6; + case HardwareModel::ZR10: + // zr10 has 30x hybrid zoom (optical + digital) + return 30; + } + return 0; +} + // 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) { - uint8_t zoom_step = 0; - if (zoom_value > 0) { - zoom_step = 1; + // disable absolute zoom target + _zoom_mult_target = 0; + return send_zoom_rate(zoom_value); + } + + // absolute zoom + if (zoom_type == ZoomType::PCT) { + 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) + const float zoom_mult = linear_interpolate(1, zoom_mult_max, zoom_value, 0, 100); + switch (_hardware_model) { + case HardwareModel::UNKNOWN: + // unknown model + return false; + case HardwareModel::A8: + // set internal zoom control target + _zoom_mult_target = zoom_mult; + return true; + case HardwareModel::ZR10: + return send_zoom_mult(zoom_mult); + } } - if (zoom_value < 0) { - // Siyi API specifies -1 should be sent as 255 - zoom_step = UINT8_MAX; - } - return send_1byte_packet(SiyiCommandId::MANUAL_ZOOM_AND_AUTO_FOCUS, zoom_step); } // unsupported zoom type return false; } +// update absolute zoom controller +// only used for A8 that does not support abs zoom control +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; + + // 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 // focus in = -1, focus hold = 0, focus out = 1 bool AP_Mount_Siyi::set_focus(FocusType focus_type, float focus_value) diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h index 1fcd9ff4c4..b388f72a8b 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/libraries/AP_Mount/AP_Mount_Siyi.h @@ -91,7 +91,8 @@ private: ACQUIRE_GIMBAL_CONFIG_INFO = 0x0A, FUNCTION_FEEDBACK_INFO = 0x0B, PHOTO = 0x0C, - ACQUIRE_GIMBAL_ATTITUDE = 0x0D + ACQUIRE_GIMBAL_ATTITUDE = 0x0D, + ABSOLUTE_ZOOM = 0x0F }; // Function Feedback Info packet info_type values @@ -128,6 +129,13 @@ private: WAITING_FOR_CRC_HIGH, }; + // hardware model enum + enum class HardwareModel : uint8_t { + UNKNOWN, + A8, + ZR10 + } _hardware_model; + // reading incoming packets from gimbal and confirm they are of the correct format // results are held in the _parsed_msg structure void read_incoming_packets(); @@ -167,6 +175,20 @@ private: // yaw_is_ef should be true if yaw_rad target is an earth frame angle, false if body_frame void send_target_angles(float pitch_rad, float yaw_rad, bool yaw_is_ef); + // send zoom rate command to camera. zoom out = -1, hold = 0, zoom in = 1 + bool send_zoom_rate(float zoom_value); + + // send zoom multiple command to camera. e.g. 1x, 10x, 30x + // only supported by ZR10 and ZR30 + bool send_zoom_mult(float zoom_mult); + + // 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 + void update_zoom_control(); + // internal variables AP_HAL::UARTDriver *_uart; // uart connected to gimbal bool _initialised; // true once the driver has been initialised @@ -199,6 +221,11 @@ private: // variables for camera state 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 + 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 }; #endif // HAL_MOUNT_SIYISERIAL_ENABLED