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 data_stream_send(void);
void queued_param_send(); void queued_param_send();
void queued_waypoint_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[]; static const struct AP_Param::GroupInfo var_info[];
@ -190,9 +193,6 @@ public:
// last time we got a non-zero RSSI from RADIO_STATUS // last time we got a non-zero RSSI from RADIO_STATUS
static uint32_t last_radio_status_remrssi_ms; static uint32_t last_radio_status_remrssi_ms;
// mavlink routing object
static MAVLink_routing routing;
// common send functions // common send functions
void send_meminfo(void); void send_meminfo(void);
void send_power_status(void); void send_power_status(void);
@ -318,6 +318,12 @@ private:
// bitmask of what mavlink channels are active // bitmask of what mavlink channels are active
static uint8_t mavlink_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 // vehicle specific message send function
bool try_send_message(enum ap_message id); 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 // we exclude radio packets to make it possible to use the
// CLI over the radio // CLI over the radio
if (msg.msgid != MAVLINK_MSG_ID_RADIO && msg.msgid != MAVLINK_MSG_ID_RADIO_STATUS) { 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)) { if (!routing.check_and_forward(chan, &msg)) {
handleMessage(&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++) { for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
if ((1U<<i) & mavlink_active) { 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) { if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) {
char msg2[50]; char msg2[50];
strncpy_P(msg2, msg, sizeof(msg2)); 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 // routing table
MAVLink_routing GCS_MAVLINK::routing; 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 lock a channel, preventing use by MAVLink
*/ */