diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-12 14:30:49 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-12 14:30:49 +0200 |
commit | 629ab5540fcb62b4c0ecc49e964f2d3fcc0b8a4d (patch) | |
tree | 3b9a47e5e76eb1506a5b3e20aaee8cbeeab74d92 /src/modules/mavlink/mavlink_main.cpp | |
parent | 5616a07bf306b3fa2bc70078e9c8c26a086065dc (diff) | |
download | px4-firmware-629ab5540fcb62b4c0ecc49e964f2d3fcc0b8a4d.tar.gz px4-firmware-629ab5540fcb62b4c0ecc49e964f2d3fcc0b8a4d.tar.bz2 px4-firmware-629ab5540fcb62b4c0ecc49e964f2d3fcc0b8a4d.zip |
Fix severity handling of text messages
Diffstat (limited to 'src/modules/mavlink/mavlink_main.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 39 |
1 files changed, 32 insertions, 7 deletions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0d685c3d8..cd0581af4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -492,6 +492,7 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) // printf("logmsg: %s\n", txt); struct mavlink_logmessage msg; strncpy(msg.text, txt, sizeof(msg.text)); + msg.severity = (unsigned char)cmd; Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { @@ -840,7 +841,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { /* Start sending parameters */ mavlink_pm_start_queued_send(); - send_statustext("[mavlink pm] sending list"); + send_statustext_info("[pm] sending list"); } } break; @@ -864,7 +865,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; sprintf(buf, "[pm] unknown: %s", name); - send_statustext(buf); + send_statustext_info(buf); } else { /* set and send parameter */ @@ -901,17 +902,28 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav } int -Mavlink::send_statustext(const char *string) +Mavlink::send_statustext_info(const char *string) { - return send_statustext(MAV_SEVERITY_INFO, string); + return send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string); } int -Mavlink::send_statustext(enum MAV_SEVERITY severity, const char *string) +Mavlink::send_statustext_critical(const char *string) +{ + return send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string); +} + +int +Mavlink::send_statustext_emergency(const char *string) +{ + return send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string); +} + +int +Mavlink::send_statustext(unsigned severity, const char *string) { const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; mavlink_statustext_t statustext; - statustext.severity = severity; int i = 0; @@ -929,6 +941,19 @@ Mavlink::send_statustext(enum MAV_SEVERITY severity, const char *string) /* Enforce null termination */ statustext.text[i] = '\0'; + /* Map severity */ + switch (severity) { + case MAVLINK_IOC_SEND_TEXT_INFO: + statustext.severity = MAV_SEVERITY_INFO; + break; + case MAVLINK_IOC_SEND_TEXT_CRITICAL: + statustext.severity = MAV_SEVERITY_CRITICAL; + break; + case MAVLINK_IOC_SEND_TEXT_EMERGENCY: + statustext.severity = MAV_SEVERITY_EMERGENCY; + break; + } + mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text); return OK; @@ -1449,7 +1474,7 @@ Mavlink::task_main(int argc, char *argv[]) int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); if (lb_ret == OK) { - send_statustext(msg.text); + send_statustext(msg.severity, msg.text); } } } |