diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 278874c089..a07680ea00 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -65,6 +65,7 @@ set(msg_file_names hil_sensor.msg home_position.msg input_rc.msg + log_message.msg manual_control_setpoint.msg mavlink_log.msg mc_att_ctrl_status.msg diff --git a/msg/log_message.msg b/msg/log_message.msg new file mode 100644 index 0000000000..51fcbbd50d --- /dev/null +++ b/msg/log_message.msg @@ -0,0 +1,5 @@ +# A logging message, output with PX4_{WARN,ERR,INFO} + +uint8 severity # log level (same as in the linux kernel, starting with 0) +uint8[127] text + diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 0883457bee..3d28988d98 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -43,7 +43,7 @@ #include #include #include -#include +#include #include #include #include @@ -584,7 +584,7 @@ void Logger::run() uORB::Subscription vehicle_status_sub(ORB_ID(vehicle_status)); uORB::Subscription parameter_update_sub(ORB_ID(parameter_update)); - uORB::Subscription mavlink_log_sub(ORB_ID(mavlink_log)); + uORB::Subscription log_message_sub(ORB_ID(log_message), 20); int ntopics = add_topics_from_file(PX4_ROOTFSDIR "/fs/microsd/etc/logging/logger_topics.txt"); @@ -735,10 +735,10 @@ void Logger::run() } } - //check for new mavlink log message - if (mavlink_log_sub.check_updated()) { - mavlink_log_sub.update(); - const char *message = (const char *)mavlink_log_sub.get().text; + //check for new logging message(s) + if (log_message_sub.check_updated()) { + log_message_sub.update(); + const char *message = (const char *)log_message_sub.get().text; int message_len = strlen(message); if (message_len > 0) { @@ -747,8 +747,8 @@ void Logger::run() _msg_buffer[0] = (uint8_t)write_msg_size; _msg_buffer[1] = (uint8_t)(write_msg_size >> 8); _msg_buffer[2] = static_cast(ULogMessageType::LOGGING); - _msg_buffer[3] = mavlink_log_sub.get().severity + '0'; - memcpy(_msg_buffer + 4, &mavlink_log_sub.get().timestamp, sizeof(ulog_message_logging_s::timestamp)); + _msg_buffer[3] = log_message_sub.get().severity + '0'; + memcpy(_msg_buffer + 4, &log_message_sub.get().timestamp, sizeof(ulog_message_logging_s::timestamp)); strncpy((char *)(_msg_buffer + 12), message, sizeof(ulog_message_logging_s::message)); write(_msg_buffer, write_msg_size + ULOG_MSG_HEADER_LEN); diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index f07cc4820a..b722eb96cb 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -49,6 +49,7 @@ #include "topics/vehicle_status.h" #include "topics/manual_control_setpoint.h" #include "topics/mavlink_log.h" +#include "topics/log_message.h" #include "topics/vehicle_local_position_setpoint.h" #include "topics/vehicle_local_position.h" #include "topics/vehicle_attitude_setpoint.h" @@ -166,6 +167,7 @@ template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; +template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; diff --git a/src/modules/uORB/uORBMain.cpp b/src/modules/uORB/uORBMain.cpp index 0f0cd0d9af..32730c3fbc 100644 --- a/src/modules/uORB/uORBMain.cpp +++ b/src/modules/uORB/uORBMain.cpp @@ -36,6 +36,7 @@ #include "uORBManager.hpp" #include "uORB.h" #include "uORBCommon.hpp" +#include extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } @@ -78,6 +79,8 @@ uorb_main(int argc, char *argv[]) return -errno; } + px4_log_initialize(); + return OK; } diff --git a/src/platforms/posix/px4_layer/px4_log.c b/src/platforms/posix/px4_layer/px4_log.c index acc7673a7b..fddc46cf14 100644 --- a/src/platforms/posix/px4_layer/px4_log.c +++ b/src/platforms/posix/px4_layer/px4_log.c @@ -3,12 +3,34 @@ #ifdef __PX4_POSIX #include #endif +#include +#include +#include +static orb_advert_t orb_log_message_pub = NULL; __EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1] = { "INFO", "DEBUG", "WARN", "ERROR", "PANIC" }; __EXPORT const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1] = { PX4_ANSI_COLOR_RESET, PX4_ANSI_COLOR_GREEN, PX4_ANSI_COLOR_YELLOW, PX4_ANSI_COLOR_RED, PX4_ANSI_COLOR_RED }; + +void px4_log_initialize(void) +{ + ASSERT(orb_log_message_pub == NULL); + + /* we need to advertise with a valid message */ + struct log_message_s log_message; + log_message.timestamp = hrt_absolute_time(); + log_message.severity = 6; //info + strcpy((char *)log_message.text, "initialized uORB logging"); + + orb_log_message_pub = orb_advertise_queue(ORB_ID(log_message), &log_message, 2); + + if (!orb_log_message_pub) { + PX4_ERR("failed to advertise log_message"); + } +} + void px4_backtrace() { #ifdef __PX4_POSIX @@ -43,5 +65,33 @@ __EXPORT void px4_log_modulename(int level, const char *moduleName, const char * va_end(argptr); PX4_LOG_COLOR_END printf("\n"); + + /* publish an orb log message */ + if (level >= _PX4_LOG_LEVEL_WARN && orb_log_message_pub) { //only publish important messages + + struct log_message_s log_message; + const unsigned max_length = sizeof(log_message.text); + log_message.timestamp = hrt_absolute_time(); + + const uint8_t log_level_table[] = { + 6, /* _PX4_LOG_LEVEL_ALWAYS */ + 7, /* _PX4_LOG_LEVEL_DEBUG */ + 4, /* _PX4_LOG_LEVEL_WARN */ + 3, /* _PX4_LOG_LEVEL_ERROR */ + 0 /* _PX4_LOG_LEVEL_PANIC */ + }; + log_message.severity = log_level_table[level]; + + unsigned pos = 0; + + pos += snprintf((char *)log_message.text + pos, max_length - pos, __px4__log_modulename_pfmt, moduleName); + va_start(argptr, fmt); + pos += vsnprintf((char *)log_message.text + pos, max_length - pos, fmt, argptr); + va_end(argptr); + log_message.text[max_length - 1] = 0; //ensure 0-termination + + orb_publish(ORB_ID(log_message), orb_log_message_pub, &log_message); + + } } diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 0c518d0202..0e393cdb28 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -128,6 +128,11 @@ static inline void do_nothing(int level, ...) __BEGIN_DECLS +/** + * initialize the orb logging. Logging to console still works without or before calling this. + */ +__EXPORT extern void px4_log_initialize(void); + __EXPORT extern const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1]; __EXPORT extern const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1]; __EXPORT extern void px4_backtrace(void);