aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: split SIMSTATE, AHRS2 and AHRS3 onto their own ap_message ids

Browse Source
This commit is contained in:
Peter Barker 2018-12-18 22:55:24 +11:00 committed by Peter Barker
parent 979e5261d5
commit aa7844e4a7
2 changed files with 24 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/GCS_MAVLink/GCS.h
View File

@ -68,6 +68,8 @@ enum ap_message : uint8_t {
MSG_FENCE_STATUS,
MSG_AHRS,
MSG_SIMSTATE,
MSG_AHRS2,
MSG_AHRS3,
MSG_HWSTATUS,
MSG_WIND,
MSG_RANGEFINDER,
@ -190,6 +192,7 @@ public:
virtual void send_rangefinder() const;
void send_proximity() const;
void send_ahrs2();
void send_ahrs3();
void send_system_time();
void send_radio_in();
void send_raw_imu();

24
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -387,7 +387,8 @@ void GCS_MAVLINK::send_ahrs2()
const AP_AHRS &ahrs = AP::ahrs();
Vector3f euler;
struct Location loc {};
if (ahrs.get_secondary_attitude(euler)) {
if (ahrs.get_secondary_attitude(euler) ||
ahrs.get_secondary_position(loc)) {
mavlink_msg_ahrs2_send(chan,
euler.x,
euler.y,
@ -396,11 +397,18 @@ void GCS_MAVLINK::send_ahrs2()
loc.lat,
loc.lng);
}
const AP_AHRS_NavEKF &_ahrs = reinterpret_cast<const AP_AHRS_NavEKF&>(ahrs);
const NavEKF2 &ekf2 = _ahrs.get_NavEKF2_const();
#endif
}
void GCS_MAVLINK::send_ahrs3()
{
#if AP_AHRS_NAVEKF_AVAILABLE
const NavEKF2 &ekf2 = AP::ahrs_navekf().get_NavEKF2_const();
if (ekf2.activeCores() > 0 &&
HAVE_PAYLOAD_SPACE(chan, AHRS3)) {
struct Location loc {};
ekf2.getLLH(loc);
Vector3f euler;
ekf2.getEulerAngles(-1,euler);
mavlink_msg_ahrs3_send(chan,
euler.x,
@ -899,6 +907,8 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
{ MAVLINK_MSG_ID_FENCE_STATUS, MSG_FENCE_STATUS},
{ MAVLINK_MSG_ID_AHRS, MSG_AHRS},
{ MAVLINK_MSG_ID_SIMSTATE, MSG_SIMSTATE},
{ MAVLINK_MSG_ID_AHRS2, MSG_AHRS2},
{ MAVLINK_MSG_ID_AHRS3, MSG_AHRS3},
{ MAVLINK_MSG_ID_HWSTATUS, MSG_HWSTATUS},
{ MAVLINK_MSG_ID_WIND, MSG_WIND},
{ MAVLINK_MSG_ID_RANGEFINDER, MSG_RANGEFINDER},
@ -3835,10 +3845,18 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
case MSG_SIMSTATE:
CHECK_PAYLOAD_SIZE(SIMSTATE);
send_simstate();
break;
case MSG_AHRS2:
CHECK_PAYLOAD_SIZE(AHRS2);
send_ahrs2();
break;
case MSG_AHRS3:
CHECK_PAYLOAD_SIZE(AHRS3);
send_ahrs3();
break;
case MSG_AHRS:
CHECK_PAYLOAD_SIZE(AHRS);
send_ahrs();