From 629ab5540fcb62b4c0ecc49e964f2d3fcc0b8a4d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 14:30:49 +0200 Subject: Fix severity handling of text messages --- src/modules/mavlink/mavlink_main.cpp | 39 ++++++++++++++++++++++++------ src/modules/mavlink/mavlink_main.h | 46 ++++++++++++++++++++++++++++-------- 2 files changed, 68 insertions(+), 17 deletions(-) (limited to 'src/modules/mavlink') 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); } } } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 1f4fb759f..acfc8b90e 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -157,9 +157,9 @@ public: */ int set_hil_enabled(bool hil_enabled); - void send_message(const mavlink_message_t *msg); + void send_message(const mavlink_message_t *msg); - void handle_message(const mavlink_message_t *msg); + void handle_message(const mavlink_message_t *msg); MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); @@ -174,17 +174,43 @@ public: mavlink_channel_t get_channel(); - void configure_stream_threadsafe(const char *stream_name, float rate); + void configure_stream_threadsafe(const char *stream_name, float rate); bool _task_should_exit; /**< if true, mavlink task should exit */ int get_mavlink_fd() { return _mavlink_fd; } - int send_statustext(const char *string); - int send_statustext(enum MAV_SEVERITY severity, const char *string); - MavlinkStream * get_streams() const { return _streams; } + /** + * Send a status text with loglevel INFO + * + * @param string the message to send (will be capped by mavlink max string length) + */ + int send_statustext_info(const char *string); + + /** + * Send a status text with loglevel CRITICAL + * + * @param string the message to send (will be capped by mavlink max string length) + */ + int send_statustext_critical(const char *string); + + /** + * Send a status text with loglevel EMERGENCY + * + * @param string the message to send (will be capped by mavlink max string length) + */ + int send_statustext_emergency(const char *string); + + /** + * Send a status text with loglevel + * + * @param string the message to send (will be capped by mavlink max string length) + * @param severity the log level, one of + */ + int send_statustext(unsigned severity, const char *string); + MavlinkStream * get_streams() const { return _streams; } - float get_rate_mult(); + float get_rate_mult(); /* Functions for waiting to start transmission until message received. */ void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } @@ -195,13 +221,13 @@ public: bool message_buffer_write(const void *ptr, int size); - void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } - void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } + void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } /** * Count a transmision error */ - void count_txerr(); + void count_txerr(); protected: Mavlink *next; -- cgit v1.2.3