fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 14:48:54 +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
*/
src/modules/mavlink/mavlink_receiver.h
+3
View File
@@ -68,6 +68,7 @@
#include <uORB/topics/home_position.h>
#include <uORB/topics/input_rc.h>
#include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/log_message.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/obstacle_distance.h>
#include <uORB/topics/offboard_control_mode.h>
@@ -163,6 +164,7 @@ private:
void handle_message_set_mode(mavlink_message_t *msg);
void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
void handle_message_set_position_target_global_int(mavlink_message_t *msg);
void handle_message_statustext(mavlink_message_t *msg);
void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg);
void handle_message_utm_global_position(mavlink_message_t *msg);
void handle_message_vision_position_estimate(mavlink_message_t *msg);
@@ -227,6 +229,7 @@ private:
uORB::Publication<debug_vect_s> _debug_vect_pub{ORB_ID(debug_vect)};
uORB::Publication<follow_target_s> _follow_target_pub{ORB_ID(follow_target)};
uORB::Publication<landing_target_pose_s> _landing_target_pose_pub{ORB_ID(landing_target_pose)};
uORB::Publication<log_message_s> _log_message_pub{ORB_ID(log_message)};
uORB::Publication<obstacle_distance_s> _obstacle_distance_pub{ORB_ID(obstacle_distance)};
uORB::Publication<offboard_control_mode_s> _offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
uORB::Publication<onboard_computer_status_s> _onboard_computer_status_pub{ORB_ID(onboard_computer_status)};