diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index ca86b2c2e1..cae389529f 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -720,7 +720,7 @@ void Logger::run() // 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 uint16_t write_msg_size = static_cast(msg_size - ULOG_MSG_HEADER_LEN); - const uint16_t write_msg_id = sub.msg_id; + const uint16_t write_msg_id = static_cast(sub.get_orb_id()); //write one byte after another (necessary because of alignment) _msg_buffer[0] = (uint8_t)write_msg_size; @@ -1605,18 +1605,7 @@ void Logger::write_all_add_logged_msg(LogType type) void Logger::write_add_logged_msg(LogType type, LoggerSubscription &subscription) { ulog_message_add_logged_s msg; - - if (subscription.msg_id == MSG_ID_INVALID) { - if (_next_topic_id == MSG_ID_INVALID) { - // if we land here an uint8 is too small -> switch to uint16 - PX4_ERR("limit for _next_topic_id reached"); - return; - } - - subscription.msg_id = _next_topic_id++; - } - - msg.msg_id = subscription.msg_id; + msg.msg_id = static_cast(subscription.get_orb_id()); msg.multi_id = subscription.get_instance(); int message_name_len = strlen(subscription.get_topic()->o_name); diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 120be110c3..ec99c2b80a 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -75,8 +75,6 @@ struct LoggerSubscription : public uORB::SubscriptionInterval { LoggerSubscription(ORB_ID id, uint32_t interval_ms = 0, uint8_t instance = 0) : uORB::SubscriptionInterval(id, interval_ms * 1000, instance) {} - - uint8_t msg_id{MSG_ID_INVALID}; }; class Logger : public ModuleBase, public ModuleParams @@ -335,7 +333,6 @@ private: uint32_t _log_interval{0}; const orb_metadata *_polling_topic_meta{nullptr}; ///< if non-null, poll on this topic instead of sleeping orb_advert_t _mavlink_log_pub{nullptr}; - uint8_t _next_topic_id{0}; ///< id of next subscribed ulog topic char *_replay_file_name{nullptr}; bool _should_stop_file_log{false}; /**< if true _next_load_print is set and file logging will be stopped after load printing (for the full log) */ diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 8e082d2a60..b260e6d3eb 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -159,6 +159,7 @@ public: uint8_t get_instance() const { return _instance; } unsigned get_last_generation() const { return _last_generation; } + ORB_ID get_orb_id() const { return _orb_id; } orb_id_t get_topic() const { return get_orb_meta(_orb_id); } protected: diff --git a/src/modules/uORB/SubscriptionInterval.hpp b/src/modules/uORB/SubscriptionInterval.hpp index 36e629f26d..0bf5d75654 100644 --- a/src/modules/uORB/SubscriptionInterval.hpp +++ b/src/modules/uORB/SubscriptionInterval.hpp @@ -138,6 +138,7 @@ public: uint8_t get_instance() const { return _subscription.get_instance(); } uint32_t get_interval_us() const { return _interval_us; } unsigned get_last_generation() const { return _subscription.get_last_generation(); } + ORB_ID get_orb_id() const { return _subscription.get_orb_id(); } orb_id_t get_topic() const { return _subscription.get_topic(); } /**