From 6238befef9f7df2b61dfcc548987159a625d4964 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 12 Sep 2023 06:50:31 +1000
Subject: [PATCH] AP_Mount: added sending of attitude and velocity for SIYI

will be used by SIYI for improved gimbal control
---
 libraries/AP_Mount/AP_Mount_Siyi.cpp | 51 ++++++++++++++++++++++++++++
 libraries/AP_Mount/AP_Mount_Siyi.h   |  6 ++++
 2 files changed, 57 insertions(+)

diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp
index 37caa578c9a259..bca98bc4713112 100644
--- a/libraries/AP_Mount/AP_Mount_Siyi.cpp
+++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp
@@ -83,6 +83,12 @@ void AP_Mount_Siyi::update()
         _last_rangefinder_req_ms = now_ms;
     }
 
+    // send attitude to gimbal at 10Hz
+    if (now_ms - _last_attitude_send_ms > 100) {
+        _last_attitude_send_ms = now_ms;
+        send_attitude_velocity();
+    }
+    
     // run zoom control
     update_zoom_control();
 
@@ -1021,4 +1027,49 @@ void AP_Mount_Siyi::check_firmware_version() const
     }
 }
 
+/*
+ send ArduPilot attitude and velocity to gimbal
+*/
+void AP_Mount_Siyi::send_attitude_velocity(void)
+{
+    const auto &ahrs = AP::ahrs();
+    struct {
+        uint32_t time_boot_ms;
+        float roll, pitch, yaw;
+        float rollspeed, pitchspeed, yawspeed;
+    } attitude;
+
+    // get attitude as euler 312
+    const auto &m = ahrs.get_rotation_body_to_ned();
+    const auto att = m.to_euler312();
+    const auto &gyro = ahrs.get_gyro();
+    const uint32_t now_ms = AP_HAL::millis();
+
+    attitude.time_boot_ms = now_ms;
+    attitude.roll = att.x;
+    attitude.pitch = att.y;
+    attitude.yaw = att.z;
+    attitude.rollspeed = gyro.x;
+    attitude.pitchspeed = gyro.y;
+    attitude.yawspeed = gyro.z;
+
+    send_packet(SiyiCommandId::EXTERNAL_ATTITUDE, (const uint8_t *)&attitude, sizeof(attitude));
+
+    // send velocity NED
+    struct {
+        uint32_t time_boot_ms;
+        float vn, ve, vd;
+    } velocity;
+    Vector3f vel_ned;
+    if (!ahrs.get_velocity_NED(vel_ned)) {
+        return;
+    }
+    velocity.time_boot_ms = now_ms;
+    velocity.vn = vel_ned.x;
+    velocity.ve = vel_ned.y;
+    velocity.vd = vel_ned.z;
+
+    send_packet(SiyiCommandId::EXTERNAL_VELOCITY, (const uint8_t *)&velocity, sizeof(velocity));
+}
+
 #endif // HAL_MOUNT_SIYI_ENABLED
diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h
index a0d4a13df91420..3e604d06a3b8d0 100644
--- a/libraries/AP_Mount/AP_Mount_Siyi.h
+++ b/libraries/AP_Mount/AP_Mount_Siyi.h
@@ -111,6 +111,8 @@ class AP_Mount_Siyi : public AP_Mount_Backend
         ABSOLUTE_ZOOM = 0x0F,
         SET_CAMERA_IMAGE_TYPE = 0x11,
         READ_RANGEFINDER = 0x15,
+        EXTERNAL_ATTITUDE = 0x22,
+        EXTERNAL_VELOCITY = 0x26,
     };
 
     // Function Feedback Info packet info_type values
@@ -290,6 +292,10 @@ class AP_Mount_Siyi : public AP_Mount_Backend
     uint32_t _last_rangefinder_dist_ms;             // system time of last successful read of rangefinder distance
     float _rangefinder_dist_m;                      // distance received from rangefinder
 
+    // sending of attitude and velocity to gimbal
+    uint32_t _last_attitude_send_ms;
+    void send_attitude_velocity(void);
+
     // hardware lookup table indexed by HardwareModel enum values (see above)
     struct HWInfo {
         uint8_t hwid[2];