From 1bde74f68f56bed753924f197e5f507ea3330f60 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 9 Dec 2022 14:18:08 +0900 Subject: [PATCH] AP_Mount: Siyi fix for record ON OFF reporting --- libraries/AP_Mount/AP_Mount_Siyi.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index d8f6f76bbd..90a001a474 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -47,14 +47,17 @@ void AP_Mount_Siyi::update() // reading incoming packets from gimbal read_incoming_packets(); - // request firmware version at 1hz + // request firmware version during startup at 1hz + // during regular operation request configuration at 1hz uint32_t now_ms = AP_HAL::millis(); - if (!_got_firmware_version) { - if ((now_ms - _last_send_ms) >= 1000) { + if ((now_ms - _last_send_ms) >= 1000) { + _last_send_ms = now_ms; + if (!_got_firmware_version) { request_firmware_version(); - _last_send_ms = now_ms; + return; + } else { + request_configuration(); } - return; } // request attitude at regular intervals @@ -597,8 +600,6 @@ bool AP_Mount_Siyi::record_video(bool start_recording) // check desired recording state has changed bool ret = true; if (_last_record_video != start_recording) { - _last_record_video = start_recording; - // request recording start or stop (sadly the same message is used) const uint8_t func_type = (uint8_t)PhotoFunction::RECORD_VIDEO_TOGGLE; ret = send_packet(SiyiCommandId::PHOTO, &func_type, 1);