Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: add send_ekf_origin

Browse Source
This commit is contained in:
Randy Mackay 2017-09-18 10:35:12 +09:00
parent d66a980be3
commit 9a45940c25
2 changed files with 29 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

@ -158,6 +158,8 @@ public:
void send_vibration(const AP_InertialSensor &ins) const;
void send_home(const Location &home) const;
static void send_home_all(const Location &home);
void send_ekf_origin(const Location &ekf_origin) const;
static void send_ekf_origin_all(const Location &ekf_origin);
void send_heartbeat(uint8_t type, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status);
void send_servo_output_raw(bool hil);
static void send_collision_all(const AP_Avoidance::Obstacle &threat, MAV_COLLISION_ACTION behaviour);

27
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -1416,6 +1416,33 @@ void GCS_MAVLINK::send_home_all(const Location &home)
}
}
void GCS_MAVLINK::send_ekf_origin(const Location &ekf_origin) const
{
if (HAVE_PAYLOAD_SPACE(chan, GPS_GLOBAL_ORIGIN)) {
mavlink_msg_gps_global_origin_send(
chan,
ekf_origin.lat,
ekf_origin.lng,
ekf_origin.alt * 10);
}
}
void GCS_MAVLINK::send_ekf_origin_all(const Location &ekf_origin)
{
for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
if ((1U<<i) & mavlink_active) {
mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
if (HAVE_PAYLOAD_SPACE(chan, GPS_GLOBAL_ORIGIN)) {
mavlink_msg_gps_global_origin_send(
chan,
ekf_origin.lat,
ekf_origin.lng,
ekf_origin.alt * 10);
}
}
}
}
/*
wrapper for sending heartbeat
*/