Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: added sending of position data to Siyi gimbal

Browse Source 
Co-authored-by: Randy Mackay <rmackay9@yahoo.com>
this will allow for exif tags of position for photos
This commit is contained in:
Andrew Tridgell 2023-12-09 08:00:37 +11:00
parent 99fb5a994e
commit b19186980a
2 changed files with 36 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AP_Mount/AP_Mount_Siyi.cpp
View File

@ -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

7
libraries/AP_Mount/AP_Mount_Siyi.h
View File

@ -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 @@ private:
READ_RANGEFINDER = 0x15,
EXTERNAL_ATTITUDE = 0x22,
SET_TIME = 0x30,
POSITION_DATA = 0x3e,
};
// Function Feedback Info packet info_type values
@ -333,9 +334,9 @@ private:
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 {