diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index c1fe0504d73101..3fe2addd45a6f1 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -6,6 +6,7 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -54,6 +55,15 @@ void AP_Mount_Siyi::update() // reading incoming packets from gimbal read_incoming_packets(); + // send UTC time to the camera + if (sent_time_count < 5) { + uint64_t utc_usec; + if (AP::rtc().get_utc_usec(utc_usec) && + send_packet(SiyiCommandId::SET_TIME, (const uint8_t *)&utc_usec, sizeof(utc_usec))) { + sent_time_count++; + } + } + // request firmware version during startup at 1hz // during regular operation request configuration at 1hz uint32_t now_ms = AP_HAL::millis(); diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h index 60e40f4f2c164f..54f4d2b909b5c6 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/libraries/AP_Mount/AP_Mount_Siyi.h @@ -112,6 +112,7 @@ class AP_Mount_Siyi : public AP_Mount_Backend SET_CAMERA_IMAGE_TYPE = 0x11, READ_RANGEFINDER = 0x15, EXTERNAL_ATTITUDE = 0x22, + SET_TIME = 0x30, }; // Function Feedback Info packet info_type values @@ -335,6 +336,9 @@ class AP_Mount_Siyi : public AP_Mount_Backend const char* model_name; }; static const HWInfo hardware_lookup_table[]; + + // count of SET_TIME packets, we send 5 times to cope with packet loss + uint8_t sent_time_count; }; #endif // HAL_MOUNT_SIYISERIAL_ENABLED