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: move sending of fence_status message up

Browse Source
This commit is contained in:
Peter Barker 2019-01-31 13:10:35 +11:00 committed by Randy Mackay
parent a52a1f710f
commit f1907679e7
3 changed files with 39 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/GCS_MAVLink/GCS.h
View File

@ -183,6 +183,7 @@ public:
// common send functions
void send_heartbeat(void) const;
void send_meminfo(void);
void send_fence_status() const;
void send_power_status(void);
void send_battery_status(const AP_BattMonitor &battery,
const uint8_t instance) const;

5
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -3829,6 +3829,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
send_meminfo();
break;
case MSG_FENCE_STATUS:
CHECK_PAYLOAD_SIZE(FENCE_STATUS);
send_fence_status();
break;
case MSG_RANGEFINDER:
CHECK_PAYLOAD_SIZE(RANGEFINDER);
send_rangefinder();

33
libraries/GCS_MAVLink/GCS_Fence.cpp Normal file
View File

@ -0,0 +1,33 @@
#include "GCS.h"
#include <AC_Fence/AC_Fence.h>
// fence_send_mavlink_status - send fence status to ground station
void GCS_MAVLINK::send_fence_status() const
{
const AC_Fence *fence = AP::fence();
if (fence == nullptr) {
return;
}
if (!fence->enabled()) {
return;
}
// traslate fence library breach types to mavlink breach types
uint8_t mavlink_breach_type = FENCE_BREACH_NONE;
const uint8_t breaches = fence->get_breaches();
if ((breaches & AC_FENCE_TYPE_ALT_MAX) != 0) {
mavlink_breach_type = FENCE_BREACH_MAXALT;
}
if ((breaches & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) != 0) {
mavlink_breach_type = FENCE_BREACH_BOUNDARY;
}
// send status
mavlink_msg_fence_status_send(chan,
static_cast<int8_t>(fence->get_breaches() != 0),
fence->get_breach_count(),
mavlink_breach_type,
fence->get_breach_time());
}