diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index c1fe0504d7..bbe829d476 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; @@ -68,6 +69,17 @@ void AP_Mount_Siyi::update() } else { request_configuration(); } + +#if AP_RTC_ENABLED + // 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++; + } + } +#endif } // request attitude at regular intervals diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h index 60e40f4f2c..54f4d2b909 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/libraries/AP_Mount/AP_Mount_Siyi.h @@ -112,6 +112,7 @@ private: 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 @@ private: 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