Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: add send_to_active_channels method

This commit is contained in:
Peter Barker 2018-11-07 13:11:08 +11:00 committed by Andrew Tridgell
parent 4d46c801f2
commit 264a757095
2 changed files with 49 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/GCS_MAVLink/GCS.cpp
View File

@ -40,22 +40,38 @@ void GCS::send_text(MAV_SEVERITY severity, const char *fmt, ...)
va_end(arg_list);
}
#define FOR_EACH_ACTIVE_CHANNEL(methodcall) \
do { \
for (uint8_t i=0; i<num_gcs(); i++) { \
if (!chan(i).initialised) { \
continue; \
} \
if (!(GCS_MAVLINK::active_channel_mask() & (1 << (chan(i).get_chan()-MAVLINK_COMM_0)))) { \
continue; \
} \
chan(i).methodcall; \
} \
} while (0)
void GCS::send_to_active_channels(uint32_t msgid, const char *pkt)
{
const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(msgid);
if (entry == nullptr) {
return;
}
for (uint8_t i=0; i<num_gcs(); i++) {
GCS_MAVLINK &c = chan(i);
if (!c.initialised) {
continue;
}
if (!c.is_active()) {
continue;
}
if (entry->max_msg_len + c.packet_overhead() > c.get_uart()->txspace()) {
// no room on this channel
continue;
}
c.send_message(pkt, entry);
}
}
void GCS::send_named_float(const char *name, float value) const
{
FOR_EACH_ACTIVE_CHANNEL(send_named_float(name, value));
mavlink_named_value_float_t packet;
packet.time_boot_ms = AP_HAL::millis();
packet.value = value;
memcpy(packet.name, name, MIN(strlen(name), (uint8_t)MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN));
gcs().send_to_active_channels(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT,
(const char *)&packet);
}
/*

20
libraries/GCS_MAVLink/GCS.h
View File

@ -324,6 +324,24 @@ public:
virtual void packetReceived(const mavlink_status_t &status,
mavlink_message_t &msg);
// send a mavlink_message_t out this GCS_MAVLINK connection.
// Caller is responsible for ensuring space.
void send_message(uint32_t msgid, const char *pkt) const {
const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(msgid);
if (entry == nullptr) {
return;
}
send_message(pkt, entry);
}
void send_message(const char *pkt, const mavlink_msg_entry_t *entry) const {
_mav_finalize_message_chan_send(chan,
entry->msgid,
pkt,
entry->min_msg_len,
entry->max_msg_len,
entry->crc_extra);
}
// accessor for uart
AP_HAL::UARTDriver *get_uart() { return _port; }
@ -926,6 +944,8 @@ public:
virtual MAV_TYPE frame_type() const = 0;
virtual const char* frame_string() const { return nullptr; }
void send_to_active_channels(uint32_t msgid, const char *pkt);
void send_text(MAV_SEVERITY severity, const char *fmt, ...);
void send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list);
virtual void send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text);