Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_Mount: added sending of position and velocity to Siyi #25736

Closed
wants to merge 2 commits into from
Closed

AP_Mount: added sending of position and velocity to Siyi #25736

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c70b1c8
AP_GPS: clarify undulation sign
tridge Dec 8, 2023
79b4805
AP_Mount: added sending of position data to Siyi gimbal
tridge Dec 8, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions libraries/AP_GPS/AP_GPS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -329,6 +329,8 @@ class AP_GPS

// get the difference between WGS84 and AMSL. A positive value means
// the AMSL height is higher than WGS84 ellipsoid height
// to go from ellipsoid height to AMSL height add undulation
// to go from AMSL height to ellipsoid height subtract undulation
bool get_undulation(uint8_t instance, float &undulation) const;

// get the difference between WGS84 and AMSL. A positive value means
Expand Down
28 changes: 25 additions & 3 deletions libraries/AP_Mount/AP_Mount_Siyi.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,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
Expand Down Expand Up @@ -1161,9 +1161,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 {
Expand All @@ -1185,6 +1185,28 @@ void AP_Mount_Siyi::send_attitude(void)
attitude.yawspeed = gyro.z;

send_packet(SiyiCommandId::EXTERNAL_ATTITUDE, (const uint8_t *)&attitude, sizeof(attitude));

struct {
uint32_t time_boot_ms;
int32_t lat, lon;
int32_t alt_msl, alt_ellipsoid;
Vector3f velocity_ned;
} position;
Location loc;
float undulation = 0;
if (!ahrs.get_location(loc) ||
!ahrs.get_velocity_NED(position.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;

send_packet(SiyiCommandId::POSITION_DATA, (const uint8_t *)&position, sizeof(position));
}

#endif // HAL_MOUNT_SIYI_ENABLED
5 changes: 3 additions & 2 deletions libraries/AP_Mount/AP_Mount_Siyi.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -332,9 +333,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 {
Expand Down
Loading