mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-22 07:44:03 -04:00
Copter: move send_attitude up to GCS_MAVLINK
This commit is contained in:
parent
2b68ab4d87
commit
da1b2579c4
@ -758,7 +758,6 @@ private:
|
||||
// GCS_Mavlink.cpp
|
||||
void gcs_send_heartbeat(void);
|
||||
void gcs_send_deferred(void);
|
||||
void send_attitude(mavlink_channel_t chan);
|
||||
void send_fence_status(mavlink_channel_t chan);
|
||||
void send_extended_status1(mavlink_channel_t chan);
|
||||
void send_location(mavlink_channel_t chan);
|
||||
|
@ -98,20 +98,6 @@ MAV_STATE GCS_MAVLINK_Copter::system_status() const
|
||||
}
|
||||
|
||||
|
||||
NOINLINE void Copter::send_attitude(mavlink_channel_t chan)
|
||||
{
|
||||
const Vector3f &gyro = ins.get_gyro();
|
||||
mavlink_msg_attitude_send(
|
||||
chan,
|
||||
millis(),
|
||||
ahrs.roll,
|
||||
ahrs.pitch,
|
||||
ahrs.yaw,
|
||||
gyro.x,
|
||||
gyro.y,
|
||||
gyro.z);
|
||||
}
|
||||
|
||||
#if AC_FENCE == ENABLED
|
||||
NOINLINE void Copter::send_fence_status(mavlink_channel_t chan)
|
||||
{
|
||||
@ -319,11 +305,6 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
|
||||
}
|
||||
break;
|
||||
|
||||
case MSG_ATTITUDE:
|
||||
CHECK_PAYLOAD_SIZE(ATTITUDE);
|
||||
copter.send_attitude(chan);
|
||||
break;
|
||||
|
||||
case MSG_LOCATION:
|
||||
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
|
||||
copter.send_location(chan);
|
||||
|
Loading…
Reference in New Issue
Block a user