From 57a50b9c7b0533c2834c8de43667b30870c54ee7 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 19 Apr 2024 11:30:26 +0900 Subject: [PATCH] Mount: Siyi waits for cam version for 30 sec only --- libraries/AP_Mount/AP_Mount_Siyi.cpp | 15 ++++++++++++++- libraries/AP_Mount/AP_Mount_Siyi.h | 1 + 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index c33fb3ec94a52d..6d0613e6ee32a5 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -358,8 +358,21 @@ void AP_Mount_Siyi::process_packet() _fw_version.gimbal.minor = _msg_buff[_msg_buff_data_start+5]; // firmware minor version _fw_version.gimbal.patch = _msg_buff[_msg_buff_data_start+4]; // firmware revision (aka patch) + // check how long we've been trying to get the firmware version + const uint32_t now_ms = AP_HAL::millis(); + if (_fw_version_first_recv_ms == 0) { + _fw_version_first_recv_ms = now_ms; + } + + // wait for camera version for up to 30 seconds + const bool giveup = (now_ms - _fw_version_first_recv_ms) > 30000; + // camera firmware version may be all zero soon after startup. giveup and try again later - if (_fw_version.camera.major == 0 && _fw_version.camera.minor == 0 && _fw_version.camera.patch == 0) { + if (!giveup && _fw_version.camera.major == 0 && _fw_version.camera.minor == 0 && _fw_version.camera.patch == 0) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mount: Siyi camera fw v%u.%u.%u", + _fw_version.camera.major, + _fw_version.camera.minor, + _fw_version.camera.patch); break; } diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h index 2245e4ca0dd346..85f2d37dd73040 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/libraries/AP_Mount/AP_Mount_Siyi.h @@ -294,6 +294,7 @@ class AP_Mount_Siyi : public AP_Mount_Backend bool _got_hardware_id; // true once hardware id ha been received FirmwareVersion _fw_version; // firmware version (for reporting for GCS) + uint32_t _fw_version_first_recv_ms; // system time firmware version was first received (we giveup eventually) // buffer holding bytes from latest packet. This is only used to calculate the crc uint8_t _msg_buff[AP_MOUNT_SIYI_PACKETLEN_MAX];