mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-02 14:08:45 -04:00
GCS_MAVLink: remove ahrs3
This commit is contained in:
parent
dbfbe50737
commit
6f8176c71e
@ -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;
|
||||
|
@ -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();
|
||||
|
@ -41,7 +41,6 @@ enum ap_message : uint8_t {
|
||||
MSG_AHRS,
|
||||
MSG_SIMSTATE,
|
||||
MSG_AHRS2,
|
||||
MSG_AHRS3,
|
||||
MSG_HWSTATUS,
|
||||
MSG_WIND,
|
||||
MSG_RANGEFINDER,
|
||||
|
Loading…
Reference in New Issue
Block a user