fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-11 03:21:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink_receiver: write STATUSTEXT messages from the same system to log

Browse Source
This commit is contained in:
Oleg Kalachev
2019-11-04 15:33:22 -05:00
committed by Daniel Agar
parent 2416d0fc63
commit 7b83da708a
2 changed files with 51 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+48
View File
@@ -46,6 +46,7 @@
#include <drivers/drv_rc_input.h>
#include <drivers/drv_tone_alarm.h>
#include <ecl/geo/geo.h>
#include <systemlib/px4_macros.h>
#ifdef CONFIG_NET
#include <net/if.h>
@@ -249,6 +250,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_onboard_computer_status(msg);
break;
case MAVLINK_MSG_ID_STATUSTEXT:
handle_message_statustext(msg);
break;
default:
break;
}
@@ -2571,6 +2576,49 @@ MavlinkReceiver::handle_message_onboard_computer_status(mavlink_message_t *msg)
_onboard_computer_status_pub.publish(onboard_computer_status_topic);
}
void MavlinkReceiver::handle_message_statustext(mavlink_message_t *msg)
{
if (msg->sysid == mavlink_system.sysid) {
// log message from the same system
mavlink_statustext_t statustext;
mavlink_msg_statustext_decode(msg, &statustext);
log_message_s log_message{};
switch (statustext.severity) {
case MAV_SEVERITY_EMERGENCY:
case MAV_SEVERITY_ALERT:
case MAV_SEVERITY_CRITICAL:
log_message.severity = 0;
break;
case MAV_SEVERITY_ERROR:
log_message.severity = 3;
break;
case MAV_SEVERITY_WARNING:
log_message.severity = 4;
break;
case MAV_SEVERITY_NOTICE:
case MAV_SEVERITY_INFO:
log_message.severity = 6;
break;
default:
return;
}
log_message.timestamp = hrt_absolute_time();
snprintf(log_message.text, sizeof(log_message.text),
"[mavlink: component %d] %." STRINGIFY(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN) "s", msg->compid, statustext.text);
_log_message_pub.publish(log_message);
}
}
/**
* Receive data from UART/UDP
*/