GCS_MAVLink: remove ahrs3

This commit is contained in:
Randy Mackay 2020-04-22 10:26:45 +09:00
parent dbfbe50737
commit 6f8176c71e
3 changed files with 0 additions and 31 deletions

View File

@ -225,7 +225,6 @@ public:
virtual void send_nav_controller_output() const = 0;
virtual void send_pid_tuning() = 0;
void send_ahrs2();
void send_ahrs3();
void send_system_time();
void send_rc_channels() const;
void send_rc_channels_raw() const;

View File

@ -409,29 +409,6 @@ void GCS_MAVLINK::send_ahrs2()
#endif
}
void GCS_MAVLINK::send_ahrs3()
{
#if AP_AHRS_NAVEKF_AVAILABLE && HAL_NAVEKF2_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,
euler.y,
euler.z,
loc.alt*1.0e-2f,
loc.lat,
loc.lng,
0, 0, 0, 0);
}
#endif
}
MissionItemProtocol *GCS::get_prot_for_mission_type(const MAV_MISSION_TYPE mission_type) const
{
switch (mission_type) {
@ -759,7 +736,6 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
{ 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},
@ -4560,11 +4536,6 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
send_ahrs2();
break;
case MSG_AHRS3:
CHECK_PAYLOAD_SIZE(AHRS3);
send_ahrs3();
break;
case MSG_PID_TUNING:
CHECK_PAYLOAD_SIZE(PID_TUNING);
send_pid_tuning();

View File

@ -41,7 +41,6 @@ enum ap_message : uint8_t {
MSG_AHRS,
MSG_SIMSTATE,
MSG_AHRS2,
MSG_AHRS3,
MSG_HWSTATUS,
MSG_WIND,
MSG_RANGEFINDER,