Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 20:48:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: add support for ATTITUDE_QUATERNION

Browse Source
This commit is contained in:
Peter Barker 2021-03-03 17:42:44 +11:00 committed by Peter Barker
parent 0414929310
commit 5123b05b2b
3 changed files with 31 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/GCS_MAVLink/GCS.h
View File

@ -247,6 +247,7 @@ public:
void send_battery2();
void send_opticalflow();
virtual void send_attitude() const;
virtual void send_attitude_quaternion() const;
void send_autopilot_version() const;
void send_extended_sys_state() const;
void send_local_position() const;

29
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -790,6 +790,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
} map[] {
{ MAVLINK_MSG_ID_HEARTBEAT, MSG_HEARTBEAT},
{ MAVLINK_MSG_ID_ATTITUDE, MSG_ATTITUDE},
{ MAVLINK_MSG_ID_ATTITUDE_QUATERNION, MSG_ATTITUDE_QUATERNION},
{ MAVLINK_MSG_ID_GLOBAL_POSITION_INT, MSG_LOCATION},
{ MAVLINK_MSG_ID_HOME_POSITION, MSG_HOME},
{ MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, MSG_ORIGIN},
@ -4541,6 +4542,29 @@ void GCS_MAVLINK::send_attitude() const
omega.z);
}
void GCS_MAVLINK::send_attitude_quaternion() const
{
const AP_AHRS &ahrs = AP::ahrs();
Quaternion quat;
if (!ahrs.get_secondary_quaternion(quat)) {
return;
}
const Vector3f omega = ahrs.get_gyro();
const float repr_offseq_q[] {0,0,0,0}; // unused, but probably should correspond to the AHRS view?
mavlink_msg_attitude_quaternion_send(
chan,
AP_HAL::millis(),
quat.q1,
quat.q2,
quat.q3,
quat.q4,
omega.x, // rollspeed
omega.y, // pitchspeed
omega.z, // yawspeed
repr_offseq_q
);
}
int32_t GCS_MAVLINK::global_position_int_alt() const {
return global_position_current_loc.alt * 10UL;
}
@ -4646,6 +4670,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
send_attitude();
break;
case MSG_ATTITUDE_QUATERNION:
CHECK_PAYLOAD_SIZE(ATTITUDE_QUATERNION);
send_attitude_quaternion();
break;
case MSG_NEXT_PARAM:
CHECK_PAYLOAD_SIZE(PARAM_VALUE);
queued_param_send();

1
libraries/GCS_MAVLink/ap_message.h
View File

@ -9,6 +9,7 @@
enum ap_message : uint8_t {
MSG_HEARTBEAT,
MSG_ATTITUDE,
MSG_ATTITUDE_QUATERNION,
MSG_LOCATION,
MSG_SYS_STATUS,
MSG_POWER_STATUS,