Browse Source

Improve uLog message struct definitions

1. Rename *_header_s structs to *_s, since the _header postfix is not
helpful
2. Rename the "key" string variables in the message structs to
"key_value_str" as the string actually contains not just the key but the
key and value pair information
3. Add comments on other uLog messages to clarify use (need more
improvement / can be even more simplified)
main
Junwoo Hwang 3 years ago committed by Beat Küng
parent
commit
1ddd1573be
  1. 48
      src/modules/logger/logger.cpp
  2. 75
      src/modules/logger/messages.h

48
src/modules/logger/logger.cpp

@ -601,7 +601,7 @@ void Logger::run() @@ -601,7 +601,7 @@ void Logger::run()
max_msg_size = _event_subscription.get_topic()->o_size;
}
max_msg_size += sizeof(ulog_message_data_header_s);
max_msg_size += sizeof(ulog_message_data_s);
if (sizeof(ulog_message_logging_s) > (size_t)max_msg_size) {
max_msg_size = sizeof(ulog_message_logging_s);
@ -745,9 +745,9 @@ void Logger::run() @@ -745,9 +745,9 @@ void Logger::run()
*/
const bool try_to_subscribe = (sub_idx == next_subscribe_topic_index);
if (copy_if_updated(sub_idx, _msg_buffer + sizeof(ulog_message_data_header_s), try_to_subscribe)) {
if (copy_if_updated(sub_idx, _msg_buffer + sizeof(ulog_message_data_s), try_to_subscribe)) {
// each message consists of a header followed by an orb data object
const size_t msg_size = sizeof(ulog_message_data_header_s) + sub.get_topic()->o_size_no_padding;
const size_t msg_size = sizeof(ulog_message_data_s) + sub.get_topic()->o_size_no_padding;
const uint16_t write_msg_size = static_cast<uint16_t>(msg_size - ULOG_MSG_HEADER_LEN);
const uint16_t write_msg_id = sub.msg_id;
@ -964,7 +964,7 @@ bool Logger::handle_event_updates(uint32_t &total_bytes) @@ -964,7 +964,7 @@ bool Logger::handle_event_updates(uint32_t &total_bytes)
bool data_written = false;
while (_event_subscription.updated()) {
event_s *orb_event = (event_s *)(_msg_buffer + sizeof(ulog_message_data_header_s));
event_s *orb_event = (event_s *)(_msg_buffer + sizeof(ulog_message_data_s));
_event_subscription.copy(orb_event);
// Important: we can only access single-byte values in orb_event (it's not necessarily aligned)
@ -978,7 +978,7 @@ bool Logger::handle_event_updates(uint32_t &total_bytes) @@ -978,7 +978,7 @@ bool Logger::handle_event_updates(uint32_t &total_bytes)
updated_sequence -= _event_sequence_offset;
memcpy(&orb_event->event_sequence, &updated_sequence, sizeof(updated_sequence));
size_t msg_size = sizeof(ulog_message_data_header_s) + _event_subscription.get_topic()->o_size_no_padding;
size_t msg_size = sizeof(ulog_message_data_s) + _event_subscription.get_topic()->o_size_no_padding;
uint16_t write_msg_size = static_cast<uint16_t>(msg_size - ULOG_MSG_HEADER_LEN);
uint16_t write_msg_id = _event_subscription.msg_id;
//write one byte after another (because of alignment)
@ -1834,14 +1834,14 @@ void Logger::write_add_logged_msg(LogType type, LoggerSubscription &subscription @@ -1834,14 +1834,14 @@ void Logger::write_add_logged_msg(LogType type, LoggerSubscription &subscription
void Logger::write_info(LogType type, const char *name, const char *value)
{
_writer.lock();
ulog_message_info_header_s msg = {};
ulog_message_info_s msg = {};
uint8_t *buffer = reinterpret_cast<uint8_t *>(&msg);
msg.msg_type = static_cast<uint8_t>(ULogMessageType::INFO);
/* construct format key (type and name) */
size_t vlen = strlen(value);
msg.key_len = snprintf(msg.key, sizeof(msg.key), "char[%zu] %s", vlen, name);
size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len;
msg.key_len = snprintf(msg.key_value_str, sizeof(msg.key_value_str), "char[%zu] %s", vlen, name);
size_t msg_size = sizeof(msg) - sizeof(msg.key_value_str) + msg.key_len;
/* copy string value directly to buffer */
if (vlen < (sizeof(msg) - msg_size)) {
@ -1859,15 +1859,15 @@ void Logger::write_info(LogType type, const char *name, const char *value) @@ -1859,15 +1859,15 @@ void Logger::write_info(LogType type, const char *name, const char *value)
void Logger::write_info_multiple(LogType type, const char *name, const char *value, bool is_continued)
{
_writer.lock();
ulog_message_info_multiple_header_s msg;
ulog_message_info_multiple_s msg;
uint8_t *buffer = reinterpret_cast<uint8_t *>(&msg);
msg.msg_type = static_cast<uint8_t>(ULogMessageType::INFO_MULTIPLE);
msg.is_continued = is_continued;
/* construct format key (type and name) */
size_t vlen = strlen(value);
msg.key_len = snprintf(msg.key, sizeof(msg.key), "char[%zu] %s", vlen, name);
size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len;
msg.key_len = snprintf(msg.key_value_str, sizeof(msg.key_value_str), "char[%zu] %s", vlen, name);
size_t msg_size = sizeof(msg) - sizeof(msg.key_value_str) + msg.key_len;
/* copy string value directly to buffer */
if (vlen < (sizeof(msg) - msg_size)) {
@ -1879,7 +1879,7 @@ void Logger::write_info_multiple(LogType type, const char *name, const char *val @@ -1879,7 +1879,7 @@ void Logger::write_info_multiple(LogType type, const char *name, const char *val
write_message(type, buffer, msg_size);
} else {
PX4_ERR("info_multiple str too long (%" PRIu8 "), key=%s", msg.key_len, msg.key);
PX4_ERR("info_multiple str too long (%" PRIu8 "), key=%s", msg.key_len, msg.key_value_str);
}
_writer.unlock();
@ -1900,13 +1900,13 @@ template<typename T> @@ -1900,13 +1900,13 @@ template<typename T>
void Logger::write_info_template(LogType type, const char *name, T value, const char *type_str)
{
_writer.lock();
ulog_message_info_header_s msg = {};
ulog_message_info_s msg = {};
uint8_t *buffer = reinterpret_cast<uint8_t *>(&msg);
msg.msg_type = static_cast<uint8_t>(ULogMessageType::INFO);
/* construct format key (type and name) */
msg.key_len = snprintf(msg.key, sizeof(msg.key), "%s %s", type_str, name);
size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len;
msg.key_len = snprintf(msg.key_value_str, sizeof(msg.key_value_str), "%s %s", type_str, name);
size_t msg_size = sizeof(msg) - sizeof(msg.key_value_str) + msg.key_len;
/* copy string value directly to buffer */
memcpy(&buffer[msg_size], &value, sizeof(T));
@ -2044,7 +2044,7 @@ void Logger::write_version(LogType type) @@ -2044,7 +2044,7 @@ void Logger::write_version(LogType type)
void Logger::write_parameter_defaults(LogType type)
{
_writer.lock();
ulog_message_parameter_default_header_s msg = {};
ulog_message_parameter_default_s msg = {};
uint8_t *buffer = reinterpret_cast<uint8_t *>(&msg);
msg.msg_type = static_cast<uint8_t>(ULogMessageType::PARAMETER_DEFAULT);
@ -2092,8 +2092,8 @@ void Logger::write_parameter_defaults(LogType type) @@ -2092,8 +2092,8 @@ void Logger::write_parameter_defaults(LogType type)
}
// format parameter key (type and name)
msg.key_len = snprintf(msg.key, sizeof(msg.key), "%s %s", type_str, param_name(param));
size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len;
msg.key_len = snprintf(msg.key_value_str, sizeof(msg.key_value_str), "%s %s", type_str, param_name(param));
size_t msg_size = sizeof(msg) - sizeof(msg.key_value_str) + msg.key_len;
if (param_get_default_value(param, &default_value) != 0) {
continue;
@ -2138,7 +2138,7 @@ void Logger::write_parameter_defaults(LogType type) @@ -2138,7 +2138,7 @@ void Logger::write_parameter_defaults(LogType type)
void Logger::write_parameters(LogType type)
{
_writer.lock();
ulog_message_parameter_header_s msg = {};
ulog_message_parameter_s msg = {};
uint8_t *buffer = reinterpret_cast<uint8_t *>(&msg);
msg.msg_type = static_cast<uint8_t>(ULogMessageType::PARAMETER);
@ -2175,8 +2175,8 @@ void Logger::write_parameters(LogType type) @@ -2175,8 +2175,8 @@ void Logger::write_parameters(LogType type)
}
// format parameter key (type and name)
msg.key_len = snprintf(msg.key, sizeof(msg.key), "%s %s", type_str, param_name(param));
size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len;
msg.key_len = snprintf(msg.key_value_str, sizeof(msg.key_value_str), "%s %s", type_str, param_name(param));
size_t msg_size = sizeof(msg) - sizeof(msg.key_value_str) + msg.key_len;
// copy parameter value directly to buffer
switch (ptype) {
@ -2207,7 +2207,7 @@ void Logger::write_parameters(LogType type) @@ -2207,7 +2207,7 @@ void Logger::write_parameters(LogType type)
void Logger::write_changed_parameters(LogType type)
{
_writer.lock();
ulog_message_parameter_header_s msg = {};
ulog_message_parameter_s msg = {};
uint8_t *buffer = reinterpret_cast<uint8_t *>(&msg);
msg.msg_type = static_cast<uint8_t>(ULogMessageType::PARAMETER);
@ -2245,8 +2245,8 @@ void Logger::write_changed_parameters(LogType type) @@ -2245,8 +2245,8 @@ void Logger::write_changed_parameters(LogType type)
}
// format parameter key (type and name)
msg.key_len = snprintf(msg.key, sizeof(msg.key), "%s %s", type_str, param_name(param));
size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len;
msg.key_len = snprintf(msg.key_value_str, sizeof(msg.key_value_str), "%s %s", type_str, param_name(param));
size_t msg_size = sizeof(msg) - sizeof(msg.key_value_str) + msg.key_len;
// copy parameter value directly to buffer
switch (ptype) {

75
src/modules/logger/messages.h

@ -88,12 +88,29 @@ struct ulog_key_header_s { @@ -88,12 +88,29 @@ struct ulog_key_header_s {
uint8_t data[0];
};
#define ULOG_MSG_HEADER_LEN 3 //accounts for msg_size and msg_type
/**
* @brief Message Header for the uLog
*
* This header components that is in the beginning of every uLog messages that gets written into
* Definitions section as well as the Data section of the uLog file.
*/
struct ulog_message_header_s {
uint16_t msg_size;
uint8_t msg_type;
};
#define ULOG_MSG_HEADER_LEN 3 // Length of the header in bytes: accounts for msg_size (2 bytes) and msg_type (1 byte)
/**
* @brief Format Message
*
* This message describes a single uLog topic's name and it's inner fields. The inner fields can have the type
* as defined in the uORB message file (e.g. msg/action_request.msg). Including other uORB topics, which is the
* nested type case.
*
* @param format Format of the uORB topic in the format: "message_name:field0;field1;"
* Example: "action_request:uint64_t timestamp;uint8_t action;uint8_t source;uint8_t mode;uint8_t[5] _padding0;"
*/
struct ulog_message_format_s {
uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::FORMAT);
@ -101,6 +118,11 @@ struct ulog_message_format_s { @@ -101,6 +118,11 @@ struct ulog_message_format_s {
char format[1500];
};
/**
* @brief Subscribe Message
*
* This message describes which uORB topic the logger has subscribed to.
*/
struct ulog_message_add_logged_s {
uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::ADD_LOGGED_MSG);
@ -110,6 +132,11 @@ struct ulog_message_add_logged_s { @@ -110,6 +132,11 @@ struct ulog_message_add_logged_s {
char message_name[255];
};
/**
* @brief Unsubscribe Message
*
* This message describes which uORB topic the logger has unsubscribed from.
*/
struct ulog_message_remove_logged_s {
uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::REMOVE_LOGGED_MSG);
@ -117,6 +144,9 @@ struct ulog_message_remove_logged_s { @@ -117,6 +144,9 @@ struct ulog_message_remove_logged_s {
uint16_t msg_id;
};
/**
* @brief Sync Message
*/
struct ulog_message_sync_s {
uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::SYNC);
@ -131,28 +161,40 @@ struct ulog_message_dropout_s { @@ -131,28 +161,40 @@ struct ulog_message_dropout_s {
uint16_t duration; //in ms
};
struct ulog_message_data_header_s {
struct ulog_message_data_s {
uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::DATA);
uint16_t msg_id;
};
struct ulog_message_info_header_s {
/**
* @brief Information Message
*
* Writes the dictionary type key:value relationship of any kind of information.
* Example: key_value_str[] = "char[5] sys_toolchain_ver9.4.0"
*/
struct ulog_message_info_s {
uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::INFO);
uint8_t key_len;
char key[255];
uint8_t key_len; // Length of the 'key'
char key_value_str[255]; // String with the key and value information
};
struct ulog_message_info_multiple_header_s {
/**
* @brief Multiple Information Message
*
* Writes the dictionary type key:value relationship of any kind of information, but
* for the ones which has a long value that can't be contained in a single Information Message.
*/
struct ulog_message_info_multiple_s {
uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::INFO_MULTIPLE);
uint8_t is_continued; ///< can be used for arrays: set to 1, if this message is part of the previous with the same key
uint8_t key_len;
char key[255];
uint8_t is_continued; // Can be used for arrays: set to 1, if this message is part of the previous with the same key
uint8_t key_len; // Length of the 'key'
char key_value_str[255]; // String with the key and value information
};
struct ulog_message_logging_s {
@ -174,17 +216,22 @@ struct ulog_message_logging_tagged_s { @@ -174,17 +216,22 @@ struct ulog_message_logging_tagged_s {
char message[128]; //defines the maximum length of a logged message string
};
struct ulog_message_parameter_header_s {
/**
* @brief Parameter Message
*
* Includes a parameter value information in the format "<TYPE> <PARAMETER_NAME> <VALUE>"
*/
struct ulog_message_parameter_s {
uint16_t msg_size;
uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::PARAMETER);
uint8_t key_len;
char key[255];
char key_value_str[255]; // String with the key and value information
};
enum class ulog_parameter_default_type_t : uint8_t {
system = (1 << 0),
current_setup = (1 << 1) //airframe default
current_setup = (1 << 1) // Airframe default set by "param set-default <PARAM> <VALUE>" command in the startup script
};
inline ulog_parameter_default_type_t operator|(ulog_parameter_default_type_t a, ulog_parameter_default_type_t b)
@ -192,13 +239,13 @@ inline ulog_parameter_default_type_t operator|(ulog_parameter_default_type_t a, @@ -192,13 +239,13 @@ inline ulog_parameter_default_type_t operator|(ulog_parameter_default_type_t a,
return static_cast<ulog_parameter_default_type_t>(static_cast<uint8_t>(a) | static_cast<uint8_t>(b));
}
struct ulog_message_parameter_default_header_s {
struct ulog_message_parameter_default_s {
uint16_t msg_size;
uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::PARAMETER_DEFAULT);
ulog_parameter_default_type_t default_types;
uint8_t key_len;
char key[255];
char key_value_str[255]; // String with the key and value information
};

Loading…
Cancel
Save