diff --git a/msg/mavlink_log.msg b/msg/mavlink_log.msg index d52004fad6..274596adde 100644 --- a/msg/mavlink_log.msg +++ b/msg/mavlink_log.msg @@ -1,3 +1,3 @@ uint8[50] text -uint8 severity +uint8 severity # log level (same as in the linux kernel, starting with 0) diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index a6a2767ac9..9d1044f6b5 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -41,6 +41,8 @@ #include #include +#include +#include #include #include #include @@ -439,6 +441,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)); add_topic("sensor_gyro", 0); @@ -547,38 +550,37 @@ void Logger::run() //PX4_INFO("topic: %s, size = %zu, out_size = %zu", sub.metadata->o_name, sub.metadata->o_size, msg_size); - if (_writer.write(buffer, msg_size, _dropout_start)) { + if (write(buffer, msg_size)) { #ifdef DBGPRINT total_bytes += msg_size; #endif /* DBGPRINT */ - if (_dropout_start) { - float dropout_duration = (float)(hrt_elapsed_time(&_dropout_start) / 1000) / 1.e3f; - - if (dropout_duration > _max_dropout_duration) { - _max_dropout_duration = dropout_duration; - } - - _dropout_start = 0; - } - data_written = true; } else { - - if (!_dropout_start) { - _dropout_start = hrt_absolute_time(); - ++_write_dropouts; - _high_water = 0; - } - break; // Write buffer overflow, skip this record } } } } + //check for new mavlink log message + if (mavlink_log_sub.check_updated()) { + mavlink_log_sub.update(); + message_logging_s log_msg; + log_msg.log_level = mavlink_log_sub.get().severity + '0'; + log_msg.timestamp = mavlink_log_sub.get().timestamp; + const char *message = (const char *)mavlink_log_sub.get().text; + int message_len = strlen(message); + + if (message_len > 0) { + strncpy(log_msg.message, message, sizeof(log_msg.message)); + log_msg.msg_size = sizeof(log_msg) - sizeof(log_msg.message) - MSG_HEADER_LEN + message_len; + write(&log_msg, log_msg.msg_size + MSG_HEADER_LEN); + } + } + if (!_dropout_start && _writer.get_buffer_fill_count() > _high_water) { _high_water = _writer.get_buffer_fill_count(); } @@ -642,6 +644,32 @@ void Logger::run() } } +bool Logger::write(void *ptr, size_t size) +{ + if (_writer.write(ptr, size, _dropout_start)) { + + if (_dropout_start) { + float dropout_duration = (float)(hrt_elapsed_time(&_dropout_start) / 1000) / 1.e3f; + + if (dropout_duration > _max_dropout_duration) { + _max_dropout_duration = dropout_duration; + } + + _dropout_start = 0; + } + + return true; + } + + if (!_dropout_start) { + _dropout_start = hrt_absolute_time(); + ++_write_dropouts; + _high_water = 0; + } + + return false; +} + int Logger::create_log_dir(tm *tt) { /* create dir on sdcard if needed */ diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 7f8ae5933c..32e1771faf 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -164,6 +164,13 @@ private: */ bool write_wait(void *ptr, size_t size); + /** + * Write data to the logger and handle dropouts. + * Must be called with _writer.lock() held. + * @return true if data written, false otherwise (on overflow) + */ + bool write(void *ptr, size_t size); + /** * Get the time for log file name * @param tt returned time diff --git a/src/modules/logger/messages.h b/src/modules/logger/messages.h index 58fdd56a94..b784a2bd12 100644 --- a/src/modules/logger/messages.h +++ b/src/modules/logger/messages.h @@ -42,6 +42,7 @@ enum class MessageType : uint8_t { REMOVE_LOGGED_MSG = 'R', SYNC = 'S', DROPOUT = 'O', + LOGGING = 'L', }; @@ -108,6 +109,15 @@ struct message_info_header_s { char key[255]; }; +struct message_logging_s { + uint16_t msg_size; //size of message - MSG_HEADER_LEN + uint8_t msg_type = static_cast(MessageType::LOGGING); + + uint8_t log_level; //same levels as in the linux kernel + uint64_t timestamp; + char message[255]; +}; + struct message_parameter_header_s { uint16_t msg_size; uint8_t msg_type = static_cast(MessageType::PARAMETER); diff --git a/src/modules/systemlib/mavlink_log.c b/src/modules/systemlib/mavlink_log.c index 4d4b0062aa..bf897b3057 100644 --- a/src/modules/systemlib/mavlink_log.c +++ b/src/modules/systemlib/mavlink_log.c @@ -64,6 +64,7 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con struct mavlink_log_s log_msg; log_msg.severity = severity; + log_msg.timestamp = hrt_absolute_time(); va_list ap; diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 64067967d1..7ddf234848 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -49,6 +49,7 @@ #include "topics/position_setpoint_triplet.h" #include "topics/vehicle_status.h" #include "topics/manual_control_setpoint.h" +#include "topics/mavlink_log.h" #include "topics/vehicle_local_position_setpoint.h" #include "topics/vehicle_local_position.h" #include "topics/vehicle_attitude_setpoint.h" @@ -165,6 +166,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;