zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

logger move to uORB::SubscriptionInterval (#12123)

sbg
Daniel Agar 6 years ago committed by GitHub
parent
e91614c791
commit
83e532d339
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 132 additions and 225 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 310
      src/modules/logger/logger.cpp
  2. 47
      src/modules/logger/logger.h

310
src/modules/logger/logger.cpp
Unescape View File

@ -409,7 +409,7 @@ bool Logger::request_stop_static()
return true; return true;
} }
LoggerSubscription *Logger::add_topic(const orb_metadata *topic) LoggerSubscription *Logger::add_topic(const orb_metadata *topic, uint32_t interval_ms, uint8_t instance)
{ {
LoggerSubscription *subscription = nullptr; LoggerSubscription *subscription = nullptr;
size_t fields_len = strlen(topic->o_fields) + strlen(topic->o_name) + 1; //1 for ':' size_t fields_len = strlen(topic->o_fields) + strlen(topic->o_name) + 1; //1 for ':'
@ -421,38 +421,23 @@ LoggerSubscription *Logger::add_topic(const orb_metadata *topic)
return nullptr; return nullptr;
} }
int fd = -1; if (_subscriptions.push_back(LoggerSubscription{topic, interval_ms, instance})) {
// Only subscribe to the topic now if it's published. If published later on, we'll dynamically
// add the subscription then
if (orb_exists(topic, 0) == 0) {
fd = orb_subscribe(topic);
if (fd < 0) {
PX4_WARN("logger: %s subscribe failed (%i)", topic->o_name, errno);
return nullptr;
}
} else {
PX4_DEBUG("Topic %s does not exist. Not subscribing (yet)", topic->o_name);
}
if (_subscriptions.push_back(LoggerSubscription(fd, topic))) {
subscription = &_subscriptions[_subscriptions.size() - 1]; subscription = &_subscriptions[_subscriptions.size() - 1];
} else { } else {
PX4_WARN("logger: failed to add topic. Too many subscriptions"); PX4_WARN("Too many subscriptions, failed to add: %s %d", topic->o_name, instance);
if (fd >= 0) {
orb_unsubscribe(fd);
}
} }
return subscription; return subscription;
} }
bool Logger::add_topic(const char *name, unsigned interval) bool Logger::add_topic(const char *name, uint32_t interval_ms, uint8_t instance)
{ {
// if we poll on a topic, we don't use the interval and let the polled topic define the maximum interval
if (_polling_topic_meta) {
interval_ms = 0;
}
const orb_metadata *const *topics = orb_get_topics(); const orb_metadata *const *topics = orb_get_topics();
LoggerSubscription *subscription = nullptr; LoggerSubscription *subscription = nullptr;
@ -462,9 +447,13 @@ bool Logger::add_topic(const char *name, unsigned interval)
// check if already added: if so, only update the interval // check if already added: if so, only update the interval
for (size_t j = 0; j < _subscriptions.size(); ++j) { for (size_t j = 0; j < _subscriptions.size(); ++j) {
if (_subscriptions[j].metadata == topics[i]) { if ((_subscriptions[j].get_topic() == topics[i]) && (_subscriptions[j].get_instance() == instance)) {
PX4_DEBUG("logging topic %s, interval: %i, already added, only setting interval",
topics[i]->o_name, interval); PX4_DEBUG("logging topic %s(%d), interval: %i, already added, only setting interval",
topics[i]->o_name, instance, interval_ms);
_subscriptions[j].set_interval_ms(interval_ms);
subscription = &_subscriptions[j]; subscription = &_subscriptions[j];
already_added = true; already_added = true;
break; break;
@ -472,134 +461,78 @@ bool Logger::add_topic(const char *name, unsigned interval)
} }
if (!already_added) { if (!already_added) {
subscription = add_topic(topics[i]); subscription = add_topic(topics[i], interval_ms, instance);
PX4_DEBUG("logging topic: %s, interval: %i", topics[i]->o_name, interval); PX4_DEBUG("logging topic: %s(%d), interval: %i", topics[i]->o_name, instance, interval_ms);
break; break;
} }
} }
} }
// if we poll on a topic, we don't use the interval and let the polled topic define the maximum interval return (subscription != nullptr);
if (_polling_topic_meta) { }
interval = 0;
}
if (subscription) {
if (subscription->fd[0] >= 0) {
orb_set_interval(subscription->fd[0], interval);
} else { bool Logger::add_topic_multi(const char *name, uint32_t interval_ms)
// store the interval: use a value < 0 to know it's not a valid fd {
subscription->fd[0] = -interval - 1; // add all possible instances
} for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) {
add_topic(name, interval_ms, instance);
} }
return subscription; return true;
} }
bool Logger::copy_if_updated_multi(int sub_idx, int multi_instance, void *buffer, bool try_to_subscribe) bool Logger::copy_if_updated(int sub_idx, void *buffer, bool try_to_subscribe)
{ {
bool updated = false;
LoggerSubscription &sub = _subscriptions[sub_idx]; LoggerSubscription &sub = _subscriptions[sub_idx];
int &handle = sub.fd[multi_instance];
if (handle < 0 && try_to_subscribe) { bool updated = false;
if (try_to_subscribe_topic(sub, multi_instance)) { if (sub.valid()) {
updated = sub.update(buffer);
write_add_logged_msg(LogType::Full, sub, multi_instance); } else if (try_to_subscribe) {
if (sub.subscribe()) {
write_add_logged_msg(LogType::Full, sub);
if (sub_idx < _num_mission_subs) { if (sub_idx < _num_mission_subs) {
write_add_logged_msg(LogType::Mission, sub, multi_instance); write_add_logged_msg(LogType::Mission, sub);
}
/* copy first data */
if (orb_copy(sub.metadata, handle, buffer) == PX4_OK) {
updated = true;
} }
}
} else if (handle >= 0) {
orb_check(handle, &updated);
if (updated) { // copy first data
orb_copy(sub.metadata, handle, buffer); updated = sub.copy(buffer);
} }
} }
return updated; return updated;
} }
bool Logger::try_to_subscribe_topic(LoggerSubscription &sub, int multi_instance)
{
bool ret = false;
if (OK == orb_exists(sub.metadata, multi_instance)) {
unsigned int interval;
if (multi_instance == 0) {
// the first instance and no subscription yet: this means we stored the negative interval as fd
interval = (unsigned int)(-(sub.fd[0] + 1));
} else {
// set to the same interval as the first instance
if (orb_get_interval(sub.fd[0], &interval) != 0) {
interval = 0;
}
}
int &handle = sub.fd[multi_instance];
handle = orb_subscribe_multi(sub.metadata, multi_instance);
if (handle >= 0) {
PX4_DEBUG("subscribed to instance %d of topic %s", multi_instance, sub.metadata->o_name);
if (interval > 0) {
orb_set_interval(handle, interval);
}
ret = true;
} else {
PX4_ERR("orb_subscribe_multi %s failed (%i)", sub.metadata->o_name, errno);
}
}
return ret;
}
void Logger::add_default_topics() void Logger::add_default_topics()
{ {
// Note: try to avoid setting the interval where possible, as it increases RAM usage
add_topic("actuator_controls_0", 100); add_topic("actuator_controls_0", 100);
add_topic("actuator_controls_1", 100); add_topic("actuator_controls_1", 100);
add_topic("actuator_outputs", 100);
add_topic("airspeed", 200); add_topic("airspeed", 200);
add_topic("battery_status", 500);
add_topic("camera_capture"); add_topic("camera_capture");
add_topic("camera_trigger"); add_topic("camera_trigger");
add_topic("camera_trigger_secondary"); add_topic("camera_trigger_secondary");
add_topic("cpuload"); add_topic("cpuload");
add_topic("distance_sensor", 100);
add_topic("ekf2_innovations", 200); add_topic("ekf2_innovations", 200);
//add_topic("ekf_gps_drift"); add_topic("ekf_gps_drift");
add_topic("esc_status", 250); add_topic("esc_status", 250);
add_topic("estimator_status", 200); add_topic("estimator_status", 200);
add_topic("home_position"); add_topic("home_position");
add_topic("input_rc", 200); add_topic("input_rc", 200);
add_topic("manual_control_setpoint", 200); add_topic("manual_control_setpoint", 200);
//add_topic("mission"); add_topic("mission");
//add_topic("mission_result"); add_topic("mission_result");
add_topic("optical_flow", 50); add_topic("optical_flow", 50);
add_topic("position_controller_status", 500);
add_topic("position_setpoint_triplet", 200); add_topic("position_setpoint_triplet", 200);
//add_topic("radio_status"); add_topic("radio_status");
add_topic("rate_ctrl_status", 200); add_topic("rate_ctrl_status", 200);
add_topic("sensor_combined", 100); add_topic("sensor_combined", 100);
add_topic("sensor_preflight", 200); add_topic("sensor_preflight", 200);
add_topic("system_power", 500); add_topic("system_power", 500);
add_topic("tecs_status", 200); add_topic("tecs_status", 200);
add_topic("telemetry_status");
add_topic("trajectory_setpoint", 200); add_topic("trajectory_setpoint", 200);
add_topic("vehicle_air_data", 200); add_topic("vehicle_air_data", 200);
add_topic("vehicle_angular_velocity", 20); add_topic("vehicle_angular_velocity", 20);
@ -607,7 +540,6 @@ void Logger::add_default_topics()
add_topic("vehicle_attitude_setpoint", 100); add_topic("vehicle_attitude_setpoint", 100);
add_topic("vehicle_command"); add_topic("vehicle_command");
add_topic("vehicle_global_position", 200); add_topic("vehicle_global_position", 200);
add_topic("vehicle_gps_position");
add_topic("vehicle_land_detected"); add_topic("vehicle_land_detected");
add_topic("vehicle_local_position", 100); add_topic("vehicle_local_position", 100);
add_topic("vehicle_local_position_setpoint", 100); add_topic("vehicle_local_position_setpoint", 100);
@ -618,12 +550,18 @@ void Logger::add_default_topics()
add_topic("vtol_vehicle_status", 200); add_topic("vtol_vehicle_status", 200);
add_topic("wind_estimate", 200); add_topic("wind_estimate", 200);
add_topic_multi("actuator_outputs", 100);
add_topic_multi("battery_status", 500);
add_topic_multi("distance_sensor", 100);
add_topic_multi("telemetry_status");
add_topic_multi("vehicle_gps_position");
#ifdef CONFIG_ARCH_BOARD_PX4_SITL #ifdef CONFIG_ARCH_BOARD_PX4_SITL
add_topic("actuator_controls_virtual_fw"); add_topic("actuator_controls_virtual_fw");
add_topic("actuator_controls_virtual_mc"); add_topic("actuator_controls_virtual_mc");
add_topic("fw_virtual_attitude_setpoint"); add_topic("fw_virtual_attitude_setpoint");
add_topic("mc_virtual_attitude_setpoint"); add_topic("mc_virtual_attitude_setpoint");
add_topic("multirotor_motor_limits");
add_topic("offboard_control_mode"); add_topic("offboard_control_mode");
add_topic("position_controller_status"); add_topic("position_controller_status");
add_topic("time_offset"); add_topic("time_offset");
@ -632,6 +570,9 @@ void Logger::add_default_topics()
add_topic("vehicle_global_position_groundtruth", 100); add_topic("vehicle_global_position_groundtruth", 100);
add_topic("vehicle_local_position_groundtruth", 100); add_topic("vehicle_local_position_groundtruth", 100);
add_topic("vehicle_roi"); add_topic("vehicle_roi");
add_topic_multi("multirotor_motor_limits");
#endif /* CONFIG_ARCH_BOARD_PX4_SITL */ #endif /* CONFIG_ARCH_BOARD_PX4_SITL */
} }
@ -665,31 +606,32 @@ void Logger::add_estimator_replay_topics()
// current EKF2 subscriptions // current EKF2 subscriptions
add_topic("airspeed"); add_topic("airspeed");
add_topic("distance_sensor");
add_topic("optical_flow"); add_topic("optical_flow");
add_topic("sensor_combined"); add_topic("sensor_combined");
add_topic("sensor_selection"); add_topic("sensor_selection");
add_topic("vehicle_air_data"); add_topic("vehicle_air_data");
add_topic("vehicle_gps_position");
add_topic("vehicle_land_detected"); add_topic("vehicle_land_detected");
add_topic("vehicle_magnetometer"); add_topic("vehicle_magnetometer");
add_topic("vehicle_status"); add_topic("vehicle_status");
add_topic("vehicle_visual_odometry"); add_topic("vehicle_visual_odometry");
add_topic_multi("distance_sensor");
add_topic_multi("vehicle_gps_position");
} }
void Logger::add_thermal_calibration_topics() void Logger::add_thermal_calibration_topics()
{ {
add_topic("sensor_accel", 100); add_topic_multi("sensor_accel", 100);
add_topic("sensor_baro", 100); add_topic_multi("sensor_baro", 100);
add_topic("sensor_gyro", 100); add_topic_multi("sensor_gyro", 100);
} }
void Logger::add_sensor_comparison_topics() void Logger::add_sensor_comparison_topics()
{ {
add_topic("sensor_accel", 100); add_topic_multi("sensor_accel", 100);
add_topic("sensor_baro", 100); add_topic_multi("sensor_baro", 100);
add_topic("sensor_gyro", 100); add_topic_multi("sensor_gyro", 100);
add_topic("sensor_mag", 100); add_topic_multi("sensor_mag", 100);
} }
void Logger::add_vision_and_avoidance_topics() void Logger::add_vision_and_avoidance_topics()
@ -712,14 +654,10 @@ void Logger::add_system_identification_topics()
int Logger::add_topics_from_file(const char *fname) int Logger::add_topics_from_file(const char *fname)
{ {
FILE *fp; int ntopics = 0;
char line[80];
char topic_name[80];
unsigned interval;
int ntopics = 0;
/* open the topic list file */ /* open the topic list file */
fp = fopen(fname, "r"); FILE *fp = fopen(fname, "r");
if (fp == nullptr) { if (fp == nullptr) {
return -1; return -1;
@ -727,8 +665,8 @@ int Logger::add_topics_from_file(const char *fname)
/* call add_topic for each topic line in the file */ /* call add_topic for each topic line in the file */
for (;;) { for (;;) {
/* get a line, bail on error/EOF */ /* get a line, bail on error/EOF */
char line[80];
line[0] = '\0'; line[0] = '\0';
if (fgets(line, sizeof(line), fp) == nullptr) { if (fgets(line, sizeof(line), fp) == nullptr) {
@ -741,8 +679,9 @@ int Logger::add_topics_from_file(const char *fname)
} }
// read line with format: <topic_name>[, <interval>] // read line with format: <topic_name>[, <interval>]
interval = 0; char topic_name[80];
int nfields = sscanf(line, "%s %u", topic_name, &interval); uint32_t interval_ms = 0;
int nfields = sscanf(line, "%s %u", topic_name, &interval_ms);
if (nfields > 0) { if (nfields > 0) {
int name_len = strlen(topic_name); int name_len = strlen(topic_name);
@ -751,8 +690,8 @@ int Logger::add_topics_from_file(const char *fname)
topic_name[name_len - 1] = '\0'; topic_name[name_len - 1] = '\0';
} }
/* add topic with specified interval */ /* add topic with specified interval_ms */
if (add_topic(topic_name, interval)) { if (add_topic(topic_name, interval_ms)) {
ntopics++; ntopics++;
} else { } else {
@ -841,15 +780,15 @@ void Logger::initialize_configured_topics()
} }
void Logger::add_mission_topic(const char *name, unsigned interval) void Logger::add_mission_topic(const char *name, uint32_t interval_ms)
{ {
if (_num_mission_subs >= MAX_MISSION_TOPICS_NUM) { if (_num_mission_subs >= MAX_MISSION_TOPICS_NUM) {
PX4_ERR("Max num mission topics exceeded"); PX4_ERR("Max num mission topics exceeded");
return; return;
} }
if (add_topic(name, interval)) { if (add_topic(name, interval_ms)) {
_mission_subscriptions[_num_mission_subs].min_delta_ms = interval; _mission_subscriptions[_num_mission_subs].min_delta_ms = interval_ms;
_mission_subscriptions[_num_mission_subs].next_write_time = 0; _mission_subscriptions[_num_mission_subs].next_write_time = 0;
++_num_mission_subs; ++_num_mission_subs;
} }
@ -917,8 +856,8 @@ void Logger::run()
for (const auto &subscription : _subscriptions) { for (const auto &subscription : _subscriptions) {
//use o_size, because that's what orb_copy will use //use o_size, because that's what orb_copy will use
if (subscription.metadata->o_size > max_msg_size) { if (subscription.get_topic()->o_size > max_msg_size) {
max_msg_size = subscription.metadata->o_size; max_msg_size = subscription.get_topic()->o_size;
} }
} }
@ -1051,51 +990,48 @@ void Logger::run()
int sub_idx = 0; int sub_idx = 0;
for (LoggerSubscription &sub : _subscriptions) { for (LoggerSubscription &sub : _subscriptions) {
/* each message consists of a header followed by an orb data object
*/
size_t msg_size = sizeof(ulog_message_data_header_s) + sub.metadata->o_size_no_padding;
/* if this topic has been updated, copy the new data into the message buffer /* if this topic has been updated, copy the new data into the message buffer
* and write a message to the log * and write a message to the log
*/ */
for (int instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) { const bool try_to_subscribe = (sub_idx == next_subscribe_topic_index);
if (copy_if_updated_multi(sub_idx, instance, _msg_buffer + sizeof(ulog_message_data_header_s),
sub_idx == next_subscribe_topic_index)) { if (copy_if_updated(sub_idx, _msg_buffer + sizeof(ulog_message_data_header_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 uint16_t write_msg_size = static_cast<uint16_t>(msg_size - ULOG_MSG_HEADER_LEN);
const uint16_t write_msg_id = sub.msg_id;
uint16_t write_msg_size = static_cast<uint16_t>(msg_size - ULOG_MSG_HEADER_LEN); //write one byte after another (necessary because of alignment)
//write one byte after another (necessary because of alignment) _msg_buffer[0] = (uint8_t)write_msg_size;
_msg_buffer[0] = (uint8_t)write_msg_size; _msg_buffer[1] = (uint8_t)(write_msg_size >> 8);
_msg_buffer[1] = (uint8_t)(write_msg_size >> 8); _msg_buffer[2] = static_cast<uint8_t>(ULogMessageType::DATA);
_msg_buffer[2] = static_cast<uint8_t>(ULogMessageType::DATA); _msg_buffer[3] = (uint8_t)write_msg_id;
uint16_t write_msg_id = sub.msg_ids[instance]; _msg_buffer[4] = (uint8_t)(write_msg_id >> 8);
_msg_buffer[3] = (uint8_t)write_msg_id;
_msg_buffer[4] = (uint8_t)(write_msg_id >> 8);
//PX4_INFO("topic: %s, size = %zu, out_size = %zu", sub.metadata->o_name, sub.metadata->o_size, msg_size); // PX4_INFO("topic: %s, size = %zu, out_size = %zu", sub.get_topic()->o_name, sub.get_topic()->o_size, msg_size);
// full log // full log
if (write_message(LogType::Full, _msg_buffer, msg_size)) { if (write_message(LogType::Full, _msg_buffer, msg_size)) {
#ifdef DBGPRINT #ifdef DBGPRINT
total_bytes += msg_size; total_bytes += msg_size;
#endif /* DBGPRINT */ #endif /* DBGPRINT */
data_written = true; data_written = true;
} }
// mission log // mission log
if (sub_idx < _num_mission_subs) { if (sub_idx < _num_mission_subs) {
if (_writer.is_started(LogType::Mission)) { if (_writer.is_started(LogType::Mission)) {
if (_mission_subscriptions[sub_idx].next_write_time < (loop_time / 100000)) { if (_mission_subscriptions[sub_idx].next_write_time < (loop_time / 100000)) {
unsigned delta_time = _mission_subscriptions[sub_idx].min_delta_ms; unsigned delta_time = _mission_subscriptions[sub_idx].min_delta_ms;
if (delta_time > 0) { if (delta_time > 0) {
_mission_subscriptions[sub_idx].next_write_time = (loop_time / 100000) + delta_time / 100; _mission_subscriptions[sub_idx].next_write_time = (loop_time / 100000) + delta_time / 100;
} }
if (write_message(LogType::Mission, _msg_buffer, msg_size)) { if (write_message(LogType::Mission, _msg_buffer, msg_size)) {
data_written = true; data_written = true;
}
} }
} }
} }
@ -1181,10 +1117,8 @@ void Logger::run()
// - we avoid subscribing to many topics at once, when logging starts // - we avoid subscribing to many topics at once, when logging starts
// - we'll get the data immediately once we start logging (no need to wait for the next subscribe timeout) // - we'll get the data immediately once we start logging (no need to wait for the next subscribe timeout)
if (next_subscribe_topic_index != -1) { if (next_subscribe_topic_index != -1) {
for (int instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) { if (!_subscriptions[next_subscribe_topic_index].valid()) {
if (_subscriptions[next_subscribe_topic_index].fd[instance] < 0) { _subscriptions[next_subscribe_topic_index].subscribe();
try_to_subscribe_topic(_subscriptions[next_subscribe_topic_index], instance);
}
} }
if (++next_subscribe_topic_index >= (int)_subscriptions.size()) { if (++next_subscribe_topic_index >= (int)_subscriptions.size()) {
@ -1235,16 +1169,6 @@ void Logger::run()
// stop the writer thread // stop the writer thread
_writer.thread_stop(); _writer.thread_stop();
//unsubscribe
for (LoggerSubscription &sub : _subscriptions) {
for (int instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) {
if (sub.fd[instance] >= 0) {
orb_unsubscribe(sub.fd[instance]);
sub.fd[instance] = -1;
}
}
}
if (polling_topic_sub >= 0) { if (polling_topic_sub >= 0) {
orb_unsubscribe(polling_topic_sub); orb_unsubscribe(polling_topic_sub);
} }
@ -1887,7 +1811,7 @@ void Logger::write_formats(LogType type)
for (size_t i = 0; i < sub_count; ++i) { for (size_t i = 0; i < sub_count; ++i) {
const LoggerSubscription &sub = _subscriptions[i]; const LoggerSubscription &sub = _subscriptions[i];
write_format(type, *sub.metadata, written_formats, msg); write_format(type, *sub.get_topic(), written_formats, msg);
} }
_writer.unlock(); _writer.unlock();
@ -1906,36 +1830,34 @@ void Logger::write_all_add_logged_msg(LogType type)
for (size_t i = 0; i < sub_count; ++i) { for (size_t i = 0; i < sub_count; ++i) {
LoggerSubscription &sub = _subscriptions[i]; LoggerSubscription &sub = _subscriptions[i];
for (int instance = 0; instance < ORB_MULTI_MAX_INSTANCES; ++instance) { if (sub.valid()) {
if (sub.fd[instance] >= 0) { write_add_logged_msg(type, sub);
write_add_logged_msg(type, sub, instance);
}
} }
} }
_writer.unlock(); _writer.unlock();
} }
void Logger::write_add_logged_msg(LogType type, LoggerSubscription &subscription, int instance) void Logger::write_add_logged_msg(LogType type, LoggerSubscription &subscription)
{ {
ulog_message_add_logged_s msg; ulog_message_add_logged_s msg;
if (subscription.msg_ids[instance] == (uint8_t) - 1) { if (subscription.msg_id == MSG_ID_INVALID) {
if (_next_topic_id == (uint8_t) - 1) { if (_next_topic_id == MSG_ID_INVALID) {
// if we land here an uint8 is too small -> switch to uint16 // if we land here an uint8 is too small -> switch to uint16
PX4_ERR("limit for _next_topic_id reached"); PX4_ERR("limit for _next_topic_id reached");
return; return;
} }
subscription.msg_ids[instance] = _next_topic_id++; subscription.msg_id = _next_topic_id++;
} }
msg.msg_id = subscription.msg_ids[instance]; msg.msg_id = subscription.msg_id;
msg.multi_id = instance; msg.multi_id = subscription.get_instance();
int message_name_len = strlen(subscription.metadata->o_name); int message_name_len = strlen(subscription.get_topic()->o_name);
memcpy(msg.message_name, subscription.metadata->o_name, message_name_len); memcpy(msg.message_name, subscription.get_topic()->o_name, message_name_len);
size_t msg_size = sizeof(msg) - sizeof(msg.message_name) + message_name_len; size_t msg_size = sizeof(msg) - sizeof(msg.message_name) + message_name_len;
msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN;

47
src/modules/logger/logger.h
Unescape View File

@ -83,27 +83,17 @@ inline bool operator&(SDLogProfileMask a, SDLogProfileMask b)
return static_cast<int32_t>(a) & static_cast<int32_t>(b); return static_cast<int32_t>(a) & static_cast<int32_t>(b);
} }
struct LoggerSubscription { static constexpr uint8_t MSG_ID_INVALID = UINT8_MAX;
int fd[ORB_MULTI_MAX_INSTANCES]; ///< uorb subscription. The first fd is also used to store the interval if
/// not subscribed yet (-interval - 1)
const orb_metadata *metadata = nullptr;
uint8_t msg_ids[ORB_MULTI_MAX_INSTANCES];
LoggerSubscription() {} struct LoggerSubscription : public uORB::SubscriptionInterval {
LoggerSubscription(int fd_, const orb_metadata *metadata_) : uint8_t msg_id{MSG_ID_INVALID};
metadata(metadata_)
{
fd[0] = fd_;
for (int i = 1; i < ORB_MULTI_MAX_INSTANCES; i++) { LoggerSubscription() = default;
fd[i] = -1;
}
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { LoggerSubscription(const orb_metadata *meta, uint32_t interval_ms = 0, uint8_t instance = 0) :
msg_ids[i] = (uint8_t) - 1; uORB::SubscriptionInterval(meta, interval_ms * 1000, instance)
} {}
}
}; };
class Logger : public ModuleBase<Logger> class Logger : public ModuleBase<Logger>
@ -150,18 +140,19 @@ public:
* Add a topic to be logged. This must be called before start_log() * Add a topic to be logged. This must be called before start_log()
* (because it does not write an ADD_LOGGED_MSG message). * (because it does not write an ADD_LOGGED_MSG message).
* @param name topic name * @param name topic name
* @param interval limit rate if >0, otherwise log as fast as the topic is updated. * @param interval limit in milliseconds if >0, otherwise log as fast as the topic is updated.
* @param instance orb topic instance
* @return true on success * @return true on success
*/ */
bool add_topic(const char *name, unsigned interval = 0); bool add_topic(const char *name, uint32_t interval_ms = 0, uint8_t instance = 0);
bool add_topic_multi(const char *name, uint32_t interval_ms = 0);
/** /**
* add a logged topic (called by add_topic() above). * add a logged topic (called by add_topic() above).
* In addition, it subscribes to the first instance of the topic, if it's advertised, * In addition, it subscribes to the first instance of the topic, if it's advertised,
* and sets the file descriptor of LoggerSubscription accordingly
* @return the newly added subscription on success, nullptr otherwise * @return the newly added subscription on success, nullptr otherwise
*/ */
LoggerSubscription *add_topic(const orb_metadata *topic); LoggerSubscription *add_topic(const orb_metadata *topic, uint32_t interval_ms = 0, uint8_t instance = 0);
/** /**
* request the logger thread to stop (this method does not block). * request the logger thread to stop (this method does not block).
@ -181,7 +172,7 @@ private:
Watchdog Watchdog
}; };
static constexpr size_t MAX_TOPICS_NUM = 64; /**< Maximum number of logged topics */ static constexpr size_t MAX_TOPICS_NUM = 90; /**< Maximum number of logged topics */
static constexpr int MAX_MISSION_TOPICS_NUM = 5; /**< Maximum number of mission topics */ static constexpr int MAX_MISSION_TOPICS_NUM = 5; /**< Maximum number of mission topics */
static constexpr unsigned MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ static constexpr unsigned MAX_NO_LOGFILE = 999; /**< Maximum number of log files */
static constexpr const char *LOG_ROOT[(int)LogType::Count] = { static constexpr const char *LOG_ROOT[(int)LogType::Count] = {
@ -218,7 +209,7 @@ private:
* Write an ADD_LOGGED_MSG to the log for a given subscription and instance. * Write an ADD_LOGGED_MSG to the log for a given subscription and instance.
* _writer.lock() must be held when calling this. * _writer.lock() must be held when calling this.
*/ */
void write_add_logged_msg(LogType type, LoggerSubscription &subscription, int instance); void write_add_logged_msg(LogType type, LoggerSubscription &subscription);
/** /**
* Create logging directory * Create logging directory
@ -301,13 +292,7 @@ private:
void write_changed_parameters(LogType type); void write_changed_parameters(LogType type);
inline bool copy_if_updated_multi(int sub_idx, int multi_instance, void *buffer, bool try_to_subscribe); inline bool copy_if_updated(int sub_idx, void *buffer, bool try_to_subscribe);
/**
* Check if a topic instance exists and subscribe to it
* @return true when topic exists and subscription successful
*/
bool try_to_subscribe_topic(LoggerSubscription &sub, int multi_instance);
/** /**
* Write exactly one ulog message to the logger and handle dropouts. * Write exactly one ulog message to the logger and handle dropouts.
@ -335,7 +320,7 @@ private:
* @param name topic name * @param name topic name
* @param interval limit rate if >0 [ms], otherwise log as fast as the topic is updated. * @param interval limit rate if >0 [ms], otherwise log as fast as the topic is updated.
*/ */
void add_mission_topic(const char *name, unsigned interval = 0); void add_mission_topic(const char *name, uint32_t interval_ms = 0);
/** /**
* Add topic subscriptions based on the _sdlog_profile_handle parameter * Add topic subscriptions based on the _sdlog_profile_handle parameter

Write Preview
Loading…
Cancel
Save