Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-09 09:24:01 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: added send_local_position()

Browse Source
This commit is contained in:
Andrew Tridgell 2015-04-05 08:24:05 -07:00
parent baf292def1
commit 6e52ef2a74
2 changed files with 25 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/GCS_MAVLink/GCS.h
View File

@ -57,6 +57,7 @@ enum ap_message {
MSG_OPTICAL_FLOW,
MSG_GIMBAL_REPORT,
MSG_EKF_STATUS_REPORT,
MSG_LOCAL_POSITION,
MSG_RETRY_DEFERRED // this must be last
};
@ -135,6 +136,7 @@ public:
void send_opticalflow(AP_AHRS_NavEKF &ahrs, const OpticalFlow &optflow);
#endif
void send_autopilot_version(void) const;
void send_local_position(const AP_AHRS &ahrs) const;
// return a bitmap of active channels. Used by libraries to loop
// over active channels to send to all active channels

23
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -1303,3 +1303,26 @@ void GCS_MAVLINK::send_autopilot_version(void) const
);
}
/*
send LOCAL_POSITION_NED message
*/
void GCS_MAVLINK::send_local_position(const AP_AHRS &ahrs) const
{
Vector3f local_position, velocity;
if (!ahrs.get_relative_position_NED(local_position) ||
!ahrs.get_velocity_NED(velocity)) {
// we don't know the position and velocity
return;
}
mavlink_msg_local_position_ned_send(
chan,
hal.scheduler->millis(),
local_position.x,
local_position.y,
local_position.z,
velocity.x,
velocity.y,
velocity.z);
}