From 6748a077fd4e1ef5982f2dc565098c53fa39fbc3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 15 Nov 2023 13:24:28 +1100 Subject: [PATCH 1/2] AP_Mount: set clock on SIYI this means photos on microSD have correct date --- libraries/AP_Mount/AP_Mount_Siyi.cpp | 12 ++++++++++++ libraries/AP_Mount/AP_Mount_Siyi.h | 4 ++++ 2 files changed, 16 insertions(+) diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index c1fe0504d7310..bbe829d47673b 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 60e40f4f2c164..54f4d2b909b5c 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 From 715d235fad22e4089f66a48d580080cb4aedff8d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 16 Nov 2023 09:36:30 +1100 Subject: [PATCH 2/2] AP_Mount: added gimbal rates for SIYI this helps diagnose issues with gyro bias versus control when camera is spinning --- libraries/AP_Mount/AP_Mount_Backend.cpp | 8 +++++--- libraries/AP_Mount/AP_Mount_Backend.h | 3 +++ libraries/AP_Mount/AP_Mount_Siyi.cpp | 8 ++++---- libraries/AP_Mount/AP_Mount_Siyi.h | 7 +++++++ 4 files changed, 19 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index d3cc0cb490ddc..f05e7430eb95d 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -142,6 +142,8 @@ void AP_Mount_Backend::send_gimbal_device_attitude_status(mavlink_channel_t chan if (!get_attitude_quaternion(att_quat)) { return; } + Vector3f ang_velocity { nanf(""), nanf(""), nanf("") }; + IGNORE_RETURN(get_angular_velocity(ang_velocity)); // construct quaternion array const float quat_array[4] = {att_quat.q1, att_quat.q2, att_quat.q3, att_quat.q4}; @@ -152,9 +154,9 @@ void AP_Mount_Backend::send_gimbal_device_attitude_status(mavlink_channel_t chan AP_HAL::millis(), // autopilot system time get_gimbal_device_flags(), quat_array, // attitude expressed as quaternion - std::numeric_limits::quiet_NaN(), // roll axis angular velocity (NaN for unknown) - std::numeric_limits::quiet_NaN(), // pitch axis angular velocity (NaN for unknown) - std::numeric_limits::quiet_NaN(), // yaw axis angular velocity (NaN for unknown) + ang_velocity.x, // roll axis angular velocity (NaN for unknown) + ang_velocity.y, // pitch axis angular velocity (NaN for unknown) + ang_velocity.z, // yaw axis angular velocity (NaN for unknown) 0, // failure flags (not supported) std::numeric_limits::quiet_NaN(), // delta_yaw (NaN for unknonw) std::numeric_limits::quiet_NaN(), // delta_yaw_velocity (NaN for unknonw) diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 0503d02ae648a..1c873e48e6947 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -67,6 +67,9 @@ class AP_Mount_Backend // yaw is in body-frame. virtual bool get_attitude_quaternion(Quaternion& att_quat) = 0; + // get angular velocity of mount. Only available on some backends + virtual bool get_angular_velocity(Vector3f& rates) { return false; } + // get mount's mode enum MAV_MOUNT_MODE get_mode() const { return _mode; } diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index bbe829d47673b..0cdc907f9dee8 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -99,7 +99,7 @@ void AP_Mount_Siyi::update() _last_attitude_send_ms = now_ms; send_attitude(); } - + // run zoom control update_zoom_control(); @@ -540,9 +540,9 @@ void AP_Mount_Siyi::process_packet() _current_angle_rad.z = -radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+1], _msg_buff[_msg_buff_data_start]) * 0.1); // yaw angle _current_angle_rad.y = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+3], _msg_buff[_msg_buff_data_start+2]) * 0.1); // pitch angle _current_angle_rad.x = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+5], _msg_buff[_msg_buff_data_start+4]) * 0.1); // roll angle - //const float yaw_rate_degs = -(int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+7], _msg_buff[_msg_buff_data_start+6]) * 0.1; // yaw rate - //const float pitch_rate_deg = (int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+9], _msg_buff[_msg_buff_data_start+8]) * 0.1; // pitch rate - //const float roll_rate_deg = (int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+11], _msg_buff[_msg_buff_data_start+10]) * 0.1; // roll rate + _current_rates_rads.z = -radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+7], _msg_buff[_msg_buff_data_start+6]) * 0.1); // yaw rate + _current_rates_rads.y = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+9], _msg_buff[_msg_buff_data_start+8]) * 0.1); // pitch rate + _current_rates_rads.x = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+11], _msg_buff[_msg_buff_data_start+10]) * 0.1); // roll rate break; } diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h index 54f4d2b909b5c..2245e4ca0dd34 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/libraries/AP_Mount/AP_Mount_Siyi.h @@ -93,6 +93,12 @@ class AP_Mount_Siyi : public AP_Mount_Backend // get attitude as a quaternion. returns true on success bool get_attitude_quaternion(Quaternion& att_quat) override; + // get angular velocity of mount. Only available on some backends + bool get_angular_velocity(Vector3f& rates) override { + rates = _current_rates_rads; + return true; + } + private: // serial protocol command ids @@ -309,6 +315,7 @@ class AP_Mount_Siyi : public AP_Mount_Backend // actual attitude received from gimbal Vector3f _current_angle_rad; // current angles in radians received from gimbal (x=roll, y=pitch, z=yaw) + Vector3f _current_rates_rads; // current angular rates in rad/s (x=roll, y=pitch, z=yaw) uint32_t _last_current_angle_rad_ms; // system time _current_angle_rad was updated uint32_t _last_req_current_angle_rad_ms; // system time that this driver last requested current angle