AP_Mount: added sending of position data to Siyi gimbal

Co-authored-by: Randy Mackay <rmackay9@yahoo.com> this will allow for exif tags of position for photos
rmackay9 · Jul 5, 2024 · 0427c4c · 0427c4c
1 parent 6ce7e17
commit 0427c4c
Show file tree

Hide file tree

Showing 2 changed files with 36 additions and 6 deletions.
diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp
@@ -85,7 +85,7 @@ void AP_Mount_Siyi::update()
     // send attitude to gimbal at 10Hz
     if (now_ms - _last_attitude_send_ms > 100) {
         _last_attitude_send_ms = now_ms;
-        send_attitude();
+        send_attitude_position();
     }
 
     // run zoom control
@@ -1165,9 +1165,9 @@ void AP_Mount_Siyi::check_firmware_version() const
 }
 
 /*
- send ArduPilot attitude to gimbal
+  send ArduPilot attitude and position to gimbal
 */
-void AP_Mount_Siyi::send_attitude(void)
+void AP_Mount_Siyi::send_attitude_position(void)
 {
     const auto &ahrs = AP::ahrs();
     struct {
@@ -1189,6 +1189,35 @@ void AP_Mount_Siyi::send_attitude(void)
     attitude.yawspeed = gyro.z;
 
     send_packet(SiyiCommandId::EXTERNAL_ATTITUDE, (const uint8_t *)&attitude, sizeof(attitude));
+
+    // send location and velocity
+    struct {
+        uint32_t time_boot_ms;
+        int32_t lat, lon;
+        int32_t alt_msl, alt_ellipsoid;
+        Vector3l velocity_ned_int32;
+    } position;
+    Location loc;
+    Vector3f velocity_ned;
+    float undulation = 0;
+    if (!ahrs.get_location(loc) ||
+        !ahrs.get_velocity_NED(velocity_ned)) {
+        return;
+    }
+    AP::gps().get_undulation(undulation);
+
+    position.time_boot_ms = now_ms;
+    position.lat = loc.lat;
+    position.lon = loc.lng;
+    position.alt_msl = loc.alt;
+    position.alt_ellipsoid = position.alt_msl - undulation*100;
+
+    // convert velocity to int32 and scale to mm/s
+    position.velocity_ned_int32.x = velocity_ned.x * 1000;
+    position.velocity_ned_int32.y = velocity_ned.y * 1000;
+    position.velocity_ned_int32.z = velocity_ned.z * 1000;
+
+    send_packet(SiyiCommandId::POSITION_DATA, (const uint8_t *)&position, sizeof(position));
 }
 
 #endif // HAL_MOUNT_SIYI_ENABLED
diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h
@@ -27,7 +27,7 @@
 #include <AP_Math/AP_Math.h>
 #include <AP_Common/AP_Common.h>
 
-#define AP_MOUNT_SIYI_PACKETLEN_MAX     38  // maximum number of bytes in a packet sent to or received from the gimbal
+#define AP_MOUNT_SIYI_PACKETLEN_MAX     42  // maximum number of bytes in a packet sent to or received from the gimbal
 
 class AP_Mount_Siyi : public AP_Mount_Backend_Serial
 {
@@ -120,6 +120,7 @@ class AP_Mount_Siyi : public AP_Mount_Backend_Serial
         READ_RANGEFINDER = 0x15,
         EXTERNAL_ATTITUDE = 0x22,
         SET_TIME = 0x30,
+        POSITION_DATA = 0x3e,
     };
 
     // Function Feedback Info packet info_type values
@@ -333,9 +334,9 @@ class AP_Mount_Siyi : public AP_Mount_Backend_Serial
     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 to gimbal
+    // sending of attitude and position to gimbal
     uint32_t _last_attitude_send_ms;
-    void send_attitude(void);
+    void send_attitude_position(void);
 
     // hardware lookup table indexed by HardwareModel enum values (see above)
     struct HWInfo {