aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: added support for a MAVLink snoop function

Browse Source 
this can be used to watch messages for different targets
This commit is contained in:
Andrew Tridgell 2014-12-10 14:31:31 +11:00 committed by Randy Mackay
parent 34be7f808e
commit f198cdcf20
3 changed files with 19 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/GCS_MAVLink/GCS.h
View File

@ -158,6 +158,9 @@ public:
void data_stream_send(void);
void queued_param_send();
void queued_waypoint_send();
void set_snoop(void (*_msg_snoop)(const mavlink_message_t* msg)) {
msg_snoop = _msg_snoop;
}
static const struct AP_Param::GroupInfo var_info[];
@ -190,9 +193,6 @@ public:
// last time we got a non-zero RSSI from RADIO_STATUS
static uint32_t last_radio_status_remrssi_ms;
// mavlink routing object
static MAVLink_routing routing;
// common send functions
void send_meminfo(void);
void send_power_status(void);
@ -318,6 +318,12 @@ private:
// bitmask of what mavlink channels are active
static uint8_t mavlink_active;
// mavlink routing object
static MAVLink_routing routing;
// a vehicle can optionally snoop on messages for other systems
static void (*msg_snoop)(const mavlink_message_t* msg);
// vehicle specific message send function
bool try_send_message(enum ap_message id);

8
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -853,7 +853,11 @@ GCS_MAVLINK::update(void (*run_cli)(AP_HAL::UARTDriver *))
// we exclude radio packets to make it possible to use the
// CLI over the radio
if (msg.msgid != MAVLINK_MSG_ID_RADIO && msg.msgid != MAVLINK_MSG_ID_RADIO_STATUS) {
mavlink_active |= (1U<<chan);
mavlink_active |= (1U<<(chan-MAVLINK_COMM_0));
}
// if a snoop handler has been setup then use it
if (msg_snoop != NULL) {
msg_snoop(&msg);
}
if (!routing.check_and_forward(chan, &msg)) {
handleMessage(&msg);
@ -1099,7 +1103,7 @@ void GCS_MAVLINK::send_statustext_all(const prog_char_t *msg)
{
for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
if ((1U<<i) & mavlink_active) {
mavlink_channel_t chan = (mavlink_channel_t)i;
mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) {
char msg2[50];
strncpy_P(msg2, msg, sizeof(msg2));

4
libraries/GCS_MAVLink/GCS_MAVLink.cpp
View File

@ -46,6 +46,10 @@ static uint8_t mavlink_locked_mask;
// routing table
MAVLink_routing GCS_MAVLINK::routing;
// snoop function for vehicle types that want to see messages for
// other targets
void (*GCS_MAVLINK::msg_snoop)(const mavlink_message_t* msg) = NULL;
/*
lock a channel, preventing use by MAVLink
*/