diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index 06316268ad2d68..bb2ee5ee025fc8 100644 --- 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 index 957c9f8c442bb7..18a7a32f2fbab3 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/libraries/AP_Mount/AP_Mount_Siyi.h @@ -27,7 +27,7 @@ #include #include -#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 {