fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-14 19:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactor logger: move subscription initialization into separate class

Browse Source
This commit is contained in:
Beat Küng
2019-12-17 21:22:30 -08:00
committed by Daniel Agar
parent 738ceab0ee
commit e10a4c5002
5 changed files with 588 additions and 476 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/logger/CMakeLists.txt
+1
View File
@@ -38,6 +38,7 @@ px4_add_module(
COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
SRCS
logged_topics.cpp
logger.cpp
log_writer.cpp
log_writer_file.cpp
src/modules/logger/logged_topics.cpp
+389
View File
@@ -0,0 +1,389 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "logged_topics.h"
#include "messages.h"
#include <px4_platform_common/log.h>
#include <px4_platform_common/px4_config.h>
#include <uORB/uORBTopics.h>
#include <string.h>
using namespace px4::logger;
void LoggedTopics::add_default_topics()
{
add_topic("actuator_controls_0", 100);
add_topic("actuator_controls_1", 100);
add_topic("airspeed", 200);
add_topic("airspeed_validated", 200);
add_topic("camera_capture");
add_topic("camera_trigger");
add_topic("camera_trigger_secondary");
add_topic("cellular_status", 200);
add_topic("cpuload");
add_topic("estimator_innovations", 200);
add_topic("estimator_innovation_variances", 200);
add_topic("estimator_innovation_test_ratios", 200);
add_topic("ekf_gps_drift");
add_topic("esc_status", 250);
add_topic("estimator_status", 200);
add_topic("home_position");
add_topic("input_rc", 200);
add_topic("manual_control_setpoint", 200);
add_topic("mission");
add_topic("mission_result");
add_topic("optical_flow", 50);
add_topic("position_controller_status", 500);
add_topic("position_setpoint_triplet", 200);
add_topic("radio_status");
add_topic("rate_ctrl_status", 200);
add_topic("sensor_combined", 100);
add_topic("sensor_preflight", 200);
add_topic("system_power", 500);
add_topic("tecs_status", 200);
add_topic("trajectory_setpoint", 200);
add_topic("vehicle_air_data", 200);
add_topic("vehicle_angular_velocity", 20);
add_topic("vehicle_attitude", 50);
add_topic("vehicle_attitude_setpoint", 100);
add_topic("vehicle_command");
add_topic("vehicle_global_position", 200);
add_topic("vehicle_land_detected");
add_topic("vehicle_local_position", 100);
add_topic("vehicle_local_position_setpoint", 100);
add_topic("vehicle_magnetometer", 200);
add_topic("vehicle_rates_setpoint", 20);
add_topic("vehicle_status", 200);
add_topic("vehicle_status_flags");
add_topic("vtol_vehicle_status", 200);
add_topic_multi("actuator_outputs", 100);
add_topic_multi("battery_status", 500);
add_topic_multi("distance_sensor", 100);
add_topic_multi("sensor_accel_status", 1000);
add_topic_multi("sensor_gyro_status", 1000);
add_topic_multi("telemetry_status");
add_topic_multi("vehicle_gps_position");
add_topic_multi("wind_estimate", 200);
#ifdef CONFIG_ARCH_BOARD_PX4_SITL
add_topic("actuator_controls_virtual_fw");
add_topic("actuator_controls_virtual_mc");
add_topic("fw_virtual_attitude_setpoint");
add_topic("mc_virtual_attitude_setpoint");
add_topic("offboard_control_mode");
add_topic("position_controller_status");
add_topic("time_offset");
add_topic("vehicle_angular_velocity", 10);
add_topic("vehicle_attitude_groundtruth", 10);
add_topic("vehicle_global_position_groundtruth", 100);
add_topic("vehicle_local_position_groundtruth", 100);
add_topic("vehicle_roi");
add_topic_multi("multirotor_motor_limits");
#endif /* CONFIG_ARCH_BOARD_PX4_SITL */
}
void LoggedTopics::add_high_rate_topics()
{
// maximum rate to analyze fast maneuvers (e.g. for racing)
add_topic("actuator_controls_0");
add_topic("actuator_outputs");
add_topic("manual_control_setpoint");
add_topic("rate_ctrl_status");
add_topic("sensor_combined");
add_topic("vehicle_angular_velocity");
add_topic("vehicle_attitude");
add_topic("vehicle_attitude_setpoint");
add_topic("vehicle_rates_setpoint");
}
void LoggedTopics::add_debug_topics()
{
add_topic("debug_array");
add_topic("debug_key_value");
add_topic("debug_value");
add_topic("debug_vect");
}
void LoggedTopics::add_estimator_replay_topics()
{
// for estimator replay (need to be at full rate)
add_topic("ekf2_timestamps");
add_topic("ekf_gps_position");
// current EKF2 subscriptions
add_topic("airspeed");
add_topic("optical_flow");
add_topic("sensor_combined");
add_topic("sensor_selection");
add_topic("vehicle_air_data");
add_topic("vehicle_land_detected");
add_topic("vehicle_magnetometer");
add_topic("vehicle_status");
add_topic("vehicle_visual_odometry");
add_topic("vehicle_visual_odometry_aligned");
add_topic_multi("distance_sensor");
add_topic_multi("vehicle_gps_position");
}
void LoggedTopics::add_thermal_calibration_topics()
{
add_topic_multi("sensor_accel", 100);
add_topic_multi("sensor_baro", 100);
add_topic_multi("sensor_gyro", 100);
}
void LoggedTopics::add_sensor_comparison_topics()
{
add_topic_multi("sensor_accel", 100);
add_topic_multi("sensor_baro", 100);
add_topic_multi("sensor_gyro", 100);
add_topic_multi("sensor_mag", 100);
}
void LoggedTopics::add_vision_and_avoidance_topics()
{
add_topic("collision_constraints");
add_topic("obstacle_distance_fused");
add_topic("onboard_computer_status", 200);
add_topic("vehicle_mocap_odometry", 30);
add_topic("vehicle_trajectory_waypoint", 200);
add_topic("vehicle_trajectory_waypoint_desired", 200);
add_topic("vehicle_visual_odometry", 30);
}
void LoggedTopics::add_system_identification_topics()
{
// for system id need to log imu and controls at full rate
add_topic("actuator_controls_0");
add_topic("actuator_controls_1");
add_topic("sensor_combined");
}
int LoggedTopics::add_topics_from_file(const char *fname)
{
int ntopics = 0;
/* open the topic list file */
FILE *fp = fopen(fname, "r");
if (fp == nullptr) {
return -1;
}
/* call add_topic for each topic line in the file */
for (;;) {
/* get a line, bail on error/EOF */
char line[80];
line[0] = '\0';
if (fgets(line, sizeof(line), fp) == nullptr) {
break;
}
/* skip comment lines */
if ((strlen(line) < 2) || (line[0] == '#')) {
continue;
}
// read line with format: <topic_name>[, <interval>]
char topic_name[80];
uint32_t interval_ms = 0;
int nfields = sscanf(line, "%s %u", topic_name, &interval_ms);
if (nfields > 0) {
int name_len = strlen(topic_name);
if (name_len > 0 && topic_name[name_len - 1] == ',') {
topic_name[name_len - 1] = '\0';
}
/* add topic with specified interval_ms */
if (add_topic(topic_name, interval_ms)) {
ntopics++;
} else {
PX4_ERR("Failed to add topic %s", topic_name);
}
}
}
fclose(fp);
return ntopics;
}
void LoggedTopics::initialize_mission_topics(MissionLogType mission_log_type)
{
if (mission_log_type == MissionLogType::Complete) {
add_mission_topic("camera_capture");
add_mission_topic("mission_result");
add_mission_topic("vehicle_global_position", 1000);
add_mission_topic("vehicle_status", 1000);
} else if (mission_log_type == MissionLogType::Geotagging) {
add_mission_topic("camera_capture");
}
}
void LoggedTopics::add_mission_topic(const char *name, uint16_t interval_ms)
{
if (add_topic(name, interval_ms)) {
++_num_mission_subs;
}
}
bool LoggedTopics::add_topic(const orb_metadata *topic, uint16_t interval_ms, uint8_t instance)
{
size_t fields_len = strlen(topic->o_fields) + strlen(topic->o_name) + 1; //1 for ':'
if (fields_len > sizeof(ulog_message_format_s::format)) {
PX4_WARN("skip topic %s, format string is too large: %zu (max is %zu)", topic->o_name, fields_len,
sizeof(ulog_message_format_s::format));
return false;
}
if (_subscriptions.count >= MAX_TOPICS_NUM) {
PX4_WARN("Too many subscriptions, failed to add: %s %d", topic->o_name, instance);
return false;
}
RequestedSubscription &sub = _subscriptions.sub[_subscriptions.count++];
sub.topic = topic;
sub.interval_ms = interval_ms;
sub.instance = instance;
return true;
}
bool LoggedTopics::add_topic(const char *name, uint16_t interval_ms, uint8_t instance)
{
const orb_metadata *const *topics = orb_get_topics();
bool success = false;
for (size_t i = 0; i < orb_topics_count(); i++) {
if (strcmp(name, topics[i]->o_name) == 0) {
bool already_added = false;
// check if already added: if so, only update the interval
for (int j = 0; j < _subscriptions.count; ++j) {
if (_subscriptions.sub[j].topic == topics[i] &&
_subscriptions.sub[j].instance == instance) {
PX4_DEBUG("logging topic %s(%d), interval: %i, already added, only setting interval",
topics[i]->o_name, instance, interval_ms);
_subscriptions.sub[j].interval_ms = interval_ms;
success = true;
already_added = true;
break;
}
}
if (!already_added) {
success = add_topic(topics[i], interval_ms, instance);
PX4_DEBUG("logging topic: %s(%d), interval: %i", topics[i]->o_name, instance, interval_ms);
break;
}
}
}
return success;
}
bool LoggedTopics::add_topic_multi(const char *name, uint16_t interval_ms)
{
// add all possible instances
for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) {
add_topic(name, interval_ms, instance);
}
return true;
}
bool LoggedTopics::initialize_logged_topics(SDLogProfileMask profile)
{
int ntopics = add_topics_from_file(PX4_STORAGEDIR "/etc/logging/logger_topics.txt");
if (ntopics > 0) {
PX4_INFO("logging %d topics from logger_topics.txt", ntopics);
} else {
initialize_configured_topics(profile);
}
return true;
}
void LoggedTopics::initialize_configured_topics(SDLogProfileMask profile)
{
// load appropriate topics for profile
// the order matters: if several profiles add the same topic, the logging rate of the last one will be used
if (profile & SDLogProfileMask::DEFAULT) {
add_default_topics();
}
if (profile & SDLogProfileMask::ESTIMATOR_REPLAY) {
add_estimator_replay_topics();
}
if (profile & SDLogProfileMask::THERMAL_CALIBRATION) {
add_thermal_calibration_topics();
}
if (profile & SDLogProfileMask::SYSTEM_IDENTIFICATION) {
add_system_identification_topics();
}
if (profile & SDLogProfileMask::HIGH_RATE) {
add_high_rate_topics();
}
if (profile & SDLogProfileMask::DEBUG_TOPICS) {
add_debug_topics();
}
if (profile & SDLogProfileMask::SENSOR_COMPARISON) {
add_sensor_comparison_topics();
}
if (profile & SDLogProfileMask::VISION_AND_AVOIDANCE) {
add_vision_and_avoidance_topics();
}
}
src/modules/logger/logged_topics.h
+152
View File
@@ -0,0 +1,152 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <stdint.h>
#include <uORB/uORB.h>
namespace px4
{
namespace logger
{
enum class SDLogProfileMask : int32_t {
DEFAULT = 1 << 0,
ESTIMATOR_REPLAY = 1 << 1,
THERMAL_CALIBRATION = 1 << 2,
SYSTEM_IDENTIFICATION = 1 << 3,
HIGH_RATE = 1 << 4,
DEBUG_TOPICS = 1 << 5,
SENSOR_COMPARISON = 1 << 6,
VISION_AND_AVOIDANCE = 1 << 7
};
enum class MissionLogType : int32_t {
Disabled = 0,
Complete = 1,
Geotagging = 2
};
inline bool operator&(SDLogProfileMask a, SDLogProfileMask b)
{
return static_cast<int32_t>(a) & static_cast<int32_t>(b);
}
/**
* @class LoggedTopics
* Contains the list of configured topics
*/
class LoggedTopics
{
public:
static constexpr int MAX_TOPICS_NUM = 200; /**< Maximum number of logged topics */
struct RequestedSubscription {
const orb_metadata *topic;
uint16_t interval_ms;
uint8_t instance;
};
struct RequestedSubscriptionArray {
RequestedSubscription sub[MAX_TOPICS_NUM];
int count{0};
};
LoggedTopics() = default;
~LoggedTopics() = default;
/**
* Add topic subscriptions based on the configured mission log type.
* Must be initialized first
*/
void initialize_mission_topics(MissionLogType mission_log_type);
bool initialize_logged_topics(SDLogProfileMask profile);
const RequestedSubscriptionArray &subscriptions() const { return _subscriptions; }
int numMissionSubscriptions() const { return _num_mission_subs; }
private:
/**
* Add a topic to be logged.
* @param name topic name
* @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
*/
bool add_topic(const char *name, uint16_t interval_ms = 0, uint8_t instance = 0);
bool add_topic_multi(const char *name, uint16_t interval_ms = 0);
/**
* Parse a file containing a list of uORB topics to log, calling add_topic for each
* @param fname name of file
* @return number of topics added
*/
int add_topics_from_file(const char *fname);
/**
* Add a topic to be logged for the mission log (it's also added to the full log).
* The interval is expected to be 0 or large (in the order of 0.1 seconds or higher).
* Must be called before all other topics are added.
* @param name topic name
* @param interval limit rate if >0 [ms], otherwise log as fast as the topic is updated.
*/
void add_mission_topic(const char *name, uint16_t interval_ms = 0);
/**
* Add topic subscriptions based on the profile configuration
*/
void initialize_configured_topics(SDLogProfileMask profile);
void add_default_topics();
void add_estimator_replay_topics();
void add_thermal_calibration_topics();
void add_system_identification_topics();
void add_high_rate_topics();
void add_debug_topics();
void add_sensor_comparison_topics();
void add_vision_and_avoidance_topics();
/**
* add a logged topic (called by add_topic() above).
* @return true on success
*/
bool add_topic(const orb_metadata *topic, uint16_t interval_ms = 0, uint8_t instance = 0);
RequestedSubscriptionArray _subscriptions;
int _num_mission_subs{0};
};
} //namespace logger
} //namespace px4
src/modules/logger/logger.cpp
+44 -388
View File
@@ -33,6 +33,7 @@
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/console_buffer.h>
#include "logged_topics.h"
#include "logger.h"
#include "messages.h"
#include "watchdog.h"
@@ -410,84 +411,6 @@ bool Logger::request_stop_static()
return true;
}
bool Logger::add_topic(const orb_metadata *topic, uint16_t interval_ms, uint8_t instance)
{
size_t fields_len = strlen(topic->o_fields) + strlen(topic->o_name) + 1; //1 for ':'
if (fields_len > sizeof(ulog_message_format_s::format)) {
PX4_WARN("skip topic %s, format string is too large: %zu (max is %zu)", topic->o_name, fields_len,
sizeof(ulog_message_format_s::format));
return false;
}
if (_requested_subscriptions->count >= MAX_TOPICS_NUM) {
PX4_WARN("Too many subscriptions, failed to add: %s %d", topic->o_name, instance);
return false;
}
RequestedSubscription &sub = _requested_subscriptions->sub[_requested_subscriptions->count++];
sub.topic = topic;
sub.interval_ms = interval_ms;
sub.instance = instance;
return true;
}
bool Logger::add_topic(const char *name, uint16_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;
}
if (!_requested_subscriptions) {
PX4_ERR("logic error"); // sanity-check: it is a bug if we land here
return false;
}
const orb_metadata *const *topics = orb_get_topics();
bool success = false;
for (size_t i = 0; i < orb_topics_count(); i++) {
if (strcmp(name, topics[i]->o_name) == 0) {
bool already_added = false;
// check if already added: if so, only update the interval
for (int j = 0; j < _requested_subscriptions->count; ++j) {
if (_requested_subscriptions->sub[j].topic == topics[i] &&
_requested_subscriptions->sub[j].instance == instance) {
PX4_DEBUG("logging topic %s(%d), interval: %i, already added, only setting interval",
topics[i]->o_name, instance, interval_ms);
_requested_subscriptions->sub[j].interval_ms = interval_ms;
success = true;
already_added = true;
break;
}
}
if (!already_added) {
success = add_topic(topics[i], interval_ms, instance);
PX4_DEBUG("logging topic: %s(%d), interval: %i", topics[i]->o_name, instance, interval_ms);
break;
}
}
}
return success;
}
bool Logger::add_topic_multi(const char *name, uint16_t interval_ms)
{
// add all possible instances
for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) {
add_topic(name, interval_ms, instance);
}
return true;
}
bool Logger::copy_if_updated(int sub_idx, void *buffer, bool try_to_subscribe)
{
LoggerSubscription &sub = _subscriptions[sub_idx];
@@ -513,211 +436,6 @@ bool Logger::copy_if_updated(int sub_idx, void *buffer, bool try_to_subscribe)
return updated;
}
void Logger::add_default_topics()
{
add_topic("actuator_controls_0", 100);
add_topic("actuator_controls_1", 100);
add_topic("airspeed", 200);
add_topic("airspeed_validated", 200);
add_topic("camera_capture");
add_topic("camera_trigger");
add_topic("camera_trigger_secondary");
add_topic("cellular_status", 200);
add_topic("cpuload");
add_topic("estimator_innovations", 200);
add_topic("estimator_innovation_variances", 200);
add_topic("estimator_innovation_test_ratios", 200);
add_topic("ekf_gps_drift");
add_topic("esc_status", 250);
add_topic("estimator_status", 200);
add_topic("home_position");
add_topic("input_rc", 200);
add_topic("manual_control_setpoint", 200);
add_topic("mission");
add_topic("mission_result");
add_topic("optical_flow", 50);
add_topic("position_controller_status", 500);
add_topic("position_setpoint_triplet", 200);
add_topic("radio_status");
add_topic("rate_ctrl_status", 200);
add_topic("sensor_combined", 100);
add_topic("sensor_preflight", 200);
add_topic("system_power", 500);
add_topic("tecs_status", 200);
add_topic("trajectory_setpoint", 200);
add_topic("vehicle_air_data", 200);
add_topic("vehicle_angular_velocity", 20);
add_topic("vehicle_attitude", 50);
add_topic("vehicle_attitude_setpoint", 100);
add_topic("vehicle_command");
add_topic("vehicle_global_position", 200);
add_topic("vehicle_land_detected");
add_topic("vehicle_local_position", 100);
add_topic("vehicle_local_position_setpoint", 100);
add_topic("vehicle_magnetometer", 200);
add_topic("vehicle_rates_setpoint", 20);
add_topic("vehicle_status", 200);
add_topic("vehicle_status_flags");
add_topic("vtol_vehicle_status", 200);
add_topic_multi("actuator_outputs", 100);
add_topic_multi("battery_status", 500);
add_topic_multi("distance_sensor", 100);
add_topic_multi("sensor_accel_status", 1000);
add_topic_multi("sensor_gyro_status", 1000);
add_topic_multi("telemetry_status");
add_topic_multi("vehicle_gps_position");
add_topic_multi("wind_estimate", 200);
#ifdef CONFIG_ARCH_BOARD_PX4_SITL
add_topic("actuator_controls_virtual_fw");
add_topic("actuator_controls_virtual_mc");
add_topic("fw_virtual_attitude_setpoint");
add_topic("mc_virtual_attitude_setpoint");
add_topic("offboard_control_mode");
add_topic("position_controller_status");
add_topic("time_offset");
add_topic("vehicle_angular_velocity", 10);
add_topic("vehicle_attitude_groundtruth", 10);
add_topic("vehicle_global_position_groundtruth", 100);
add_topic("vehicle_local_position_groundtruth", 100);
add_topic("vehicle_roi");
add_topic_multi("multirotor_motor_limits");
#endif /* CONFIG_ARCH_BOARD_PX4_SITL */
}
void Logger::add_high_rate_topics()
{
// maximum rate to analyze fast maneuvers (e.g. for racing)
add_topic("actuator_controls_0");
add_topic("actuator_outputs");
add_topic("manual_control_setpoint");
add_topic("rate_ctrl_status");
add_topic("sensor_combined");
add_topic("vehicle_angular_velocity");
add_topic("vehicle_attitude");
add_topic("vehicle_attitude_setpoint");
add_topic("vehicle_rates_setpoint");
}
void Logger::add_debug_topics()
{
add_topic("debug_array");
add_topic("debug_key_value");
add_topic("debug_value");
add_topic("debug_vect");
}
void Logger::add_estimator_replay_topics()
{
// for estimator replay (need to be at full rate)
add_topic("ekf2_timestamps");
add_topic("ekf_gps_position");
// current EKF2 subscriptions
add_topic("airspeed");
add_topic("optical_flow");
add_topic("sensor_combined");
add_topic("sensor_selection");
add_topic("vehicle_air_data");
add_topic("vehicle_land_detected");
add_topic("vehicle_magnetometer");
add_topic("vehicle_status");
add_topic("vehicle_visual_odometry");
add_topic("vehicle_visual_odometry_aligned");
add_topic_multi("distance_sensor");
add_topic_multi("vehicle_gps_position");
}
void Logger::add_thermal_calibration_topics()
{
add_topic_multi("sensor_accel", 100);
add_topic_multi("sensor_baro", 100);
add_topic_multi("sensor_gyro", 100);
}
void Logger::add_sensor_comparison_topics()
{
add_topic_multi("sensor_accel", 100);
add_topic_multi("sensor_baro", 100);
add_topic_multi("sensor_gyro", 100);
add_topic_multi("sensor_mag", 100);
}
void Logger::add_vision_and_avoidance_topics()
{
add_topic("collision_constraints");
add_topic("obstacle_distance_fused");
add_topic("onboard_computer_status", 200);
add_topic("vehicle_mocap_odometry", 30);
add_topic("vehicle_trajectory_waypoint", 200);
add_topic("vehicle_trajectory_waypoint_desired", 200);
add_topic("vehicle_visual_odometry", 30);
}
void Logger::add_system_identification_topics()
{
// for system id need to log imu and controls at full rate
add_topic("actuator_controls_0");
add_topic("actuator_controls_1");
add_topic("sensor_combined");
}
int Logger::add_topics_from_file(const char *fname)
{
int ntopics = 0;
/* open the topic list file */
FILE *fp = fopen(fname, "r");
if (fp == nullptr) {
return -1;
}
/* call add_topic for each topic line in the file */
for (;;) {
/* get a line, bail on error/EOF */
char line[80];
line[0] = '\0';
if (fgets(line, sizeof(line), fp) == nullptr) {
break;
}
/* skip comment lines */
if ((strlen(line) < 2) || (line[0] == '#')) {
continue;
}
// read line with format: <topic_name>[, <interval>]
char topic_name[80];
uint32_t interval_ms = 0;
int nfields = sscanf(line, "%s %u", topic_name, &interval_ms);
if (nfields > 0) {
int name_len = strlen(topic_name);
if (name_len > 0 && topic_name[name_len - 1] == ',') {
topic_name[name_len - 1] = '\0';
}
/* add topic with specified interval_ms */
if (add_topic(topic_name, interval_ms)) {
ntopics++;
} else {
PX4_ERR("Failed to add topic %s", topic_name);
}
}
}
fclose(fp);
return ntopics;
}
const char *Logger::configured_backend_mode() const
{
switch (_writer.backend()) {
@@ -731,70 +449,7 @@ const char *Logger::configured_backend_mode() const
}
}
void Logger::initialize_mission_topics(MissionLogType type)
{
if (type == MissionLogType::Complete) {
add_mission_topic("camera_capture");
add_mission_topic("mission_result");
add_mission_topic("vehicle_global_position", 1000);
add_mission_topic("vehicle_status", 1000);
} else if (type == MissionLogType::Geotagging) {
add_mission_topic("camera_capture");
}
}
bool Logger::initialize_topics(MissionLogType mission_log_mode)
{
RequestedSubscriptionArray requested_subscriptions;
_requested_subscriptions = &requested_subscriptions;
// mission log topics if enabled (must be added first)
if (mission_log_mode != MissionLogType::Disabled) {
initialize_mission_topics((MissionLogType)mission_log_mode);
if (_num_mission_subs > 0) {
int mkdir_ret = mkdir(LOG_ROOT[(int)LogType::Mission], S_IRWXU | S_IRWXG | S_IRWXO);
if (mkdir_ret != 0 && errno != EEXIST) {
PX4_ERR("failed creating log root dir: %s (%i)", LOG_ROOT[(int)LogType::Mission], errno);
}
}
}
int ntopics = add_topics_from_file(PX4_STORAGEDIR "/etc/logging/logger_topics.txt");
if (ntopics > 0) {
PX4_INFO("logging %d topics from logger_topics.txt", ntopics);
} else {
initialize_configured_topics();
}
delete[](_subscriptions);
_subscriptions = nullptr;
// now that we know how many subscriptions we need, move _requested_subscriptions into _subscriptions
if (requested_subscriptions.count > 0) {
_subscriptions = new LoggerSubscription[requested_subscriptions.count];
if (!_subscriptions) {
PX4_ERR("alloc failed");
return false;
}
for (int i = 0; i < requested_subscriptions.count; ++i) {
const RequestedSubscription &sub = requested_subscriptions.sub[i];
_subscriptions[i] = LoggerSubscription(sub.topic, sub.interval_ms, sub.instance);
}
}
_num_subscriptions = requested_subscriptions.count;
_requested_subscriptions = nullptr;
return true;
}
void Logger::initialize_configured_topics()
{
// get the logging profile
SDLogProfileMask sdlog_profile = SDLogProfileMask::DEFAULT;
@@ -808,54 +463,55 @@ void Logger::initialize_configured_topics()
sdlog_profile = SDLogProfileMask::DEFAULT;
}
// load appropriate topics for profile
// the order matters: if several profiles add the same topic, the logging rate of the last one will be used
if (sdlog_profile & SDLogProfileMask::DEFAULT) {
add_default_topics();
LoggedTopics logged_topics;
// initialize mission topics
logged_topics.initialize_mission_topics(mission_log_mode);
_num_mission_subs = logged_topics.numMissionSubscriptions();
if (_num_mission_subs > 0) {
if (_num_mission_subs >= MAX_MISSION_TOPICS_NUM) {
PX4_ERR("Max num mission topics exceeded (%i)", _num_mission_subs);
_num_mission_subs = MAX_MISSION_TOPICS_NUM;
}
for (int i = 0; i < _num_mission_subs; ++i) {
_mission_subscriptions[i].min_delta_ms = logged_topics.subscriptions().sub[i].interval_ms;
_mission_subscriptions[i].next_write_time = 0;
}
int mkdir_ret = mkdir(LOG_ROOT[(int)LogType::Mission], S_IRWXU | S_IRWXG | S_IRWXO);
if (mkdir_ret != 0 && errno != EEXIST) {
PX4_ERR("failed creating log root dir: %s (%i)", LOG_ROOT[(int)LogType::Mission], errno);
}
}
if (sdlog_profile & SDLogProfileMask::ESTIMATOR_REPLAY) {
add_estimator_replay_topics();
if (!logged_topics.initialize_logged_topics(sdlog_profile)) {
return false;
}
if (sdlog_profile & SDLogProfileMask::THERMAL_CALIBRATION) {
add_thermal_calibration_topics();
delete[](_subscriptions);
_subscriptions = nullptr;
if (logged_topics.subscriptions().count > 0) {
_subscriptions = new LoggerSubscription[logged_topics.subscriptions().count];
if (!_subscriptions) {
PX4_ERR("alloc failed");
return false;
}
for (int i = 0; i < logged_topics.subscriptions().count; ++i) {
const LoggedTopics::RequestedSubscription &sub = logged_topics.subscriptions().sub[i];
// if we poll on a topic, we don't use the interval and let the polled topic define the maximum interval
uint16_t interval_ms = _polling_topic_meta ? 0 : sub.interval_ms;
_subscriptions[i] = LoggerSubscription(sub.topic, interval_ms, sub.instance);
}
}
if (sdlog_profile & SDLogProfileMask::SYSTEM_IDENTIFICATION) {
add_system_identification_topics();
}
if (sdlog_profile & SDLogProfileMask::HIGH_RATE) {
add_high_rate_topics();
}
if (sdlog_profile & SDLogProfileMask::DEBUG_TOPICS) {
add_debug_topics();
}
if (sdlog_profile & SDLogProfileMask::SENSOR_COMPARISON) {
add_sensor_comparison_topics();
}
if (sdlog_profile & SDLogProfileMask::VISION_AND_AVOIDANCE) {
add_vision_and_avoidance_topics();
}
}
void Logger::add_mission_topic(const char *name, uint32_t interval_ms)
{
if (_num_mission_subs >= MAX_MISSION_TOPICS_NUM) {
PX4_ERR("Max num mission topics exceeded");
return;
}
if (add_topic(name, interval_ms)) {
_mission_subscriptions[_num_mission_subs].min_delta_ms = interval_ms;
_mission_subscriptions[_num_mission_subs].next_write_time = 0;
++_num_mission_subs;
}
_num_subscriptions = logged_topics.subscriptions().count;
return true;
}
void Logger::run()
src/modules/logger/logger.h
+2 -88
View File
@@ -61,28 +61,6 @@ namespace px4
namespace logger
{
enum class SDLogProfileMask : int32_t {
DEFAULT = 1 << 0,
ESTIMATOR_REPLAY = 1 << 1,
THERMAL_CALIBRATION = 1 << 2,
SYSTEM_IDENTIFICATION = 1 << 3,
HIGH_RATE = 1 << 4,
DEBUG_TOPICS = 1 << 5,
SENSOR_COMPARISON = 1 << 6,
VISION_AND_AVOIDANCE = 1 << 7
};
enum class MissionLogType : int32_t {
Disabled = 0,
Complete = 1,
Geotagging = 2
};
inline bool operator&(SDLogProfileMask a, SDLogProfileMask b)
{
return static_cast<int32_t>(a) & static_cast<int32_t>(b);
}
static constexpr uint8_t MSG_ID_INVALID = UINT8_MAX;
struct LoggerSubscription : public uORB::SubscriptionInterval {
@@ -136,24 +114,6 @@ public:
*/
void setReplayFile(const char *file_name);
/**
* Add a topic to be logged. This must be called before start_log()
* (because it does not write an ADD_LOGGED_MSG message).
* @param name topic name
* @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
*/
bool add_topic(const char *name, uint16_t interval_ms = 0, uint8_t instance = 0);
bool add_topic_multi(const char *name, uint16_t interval_ms = 0);
/**
* add a logged topic (called by add_topic() above).
* In addition, it subscribes to the first instance of the topic, if it's advertised,
* @return true on success
*/
bool add_topic(const orb_metadata *topic, uint16_t interval_ms = 0, uint8_t instance = 0);
/**
* request the logger thread to stop (this method does not block).
* @return true if the logger is stopped, false if (still) running
@@ -172,7 +132,6 @@ private:
Watchdog
};
static constexpr int MAX_TOPICS_NUM = 250; /**< Maximum number of logged 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 const char *LOG_ROOT[(int)LogType::Count] = {
@@ -200,16 +159,6 @@ private:
unsigned next_write_time{0}; ///< next time to write in 0.1 seconds
};
struct RequestedSubscription {
const orb_metadata *topic;
uint16_t interval_ms;
uint8_t instance;
};
struct RequestedSubscriptionArray {
RequestedSubscription sub[MAX_TOPICS_NUM];
int count;
};
/**
* Write an ADD_LOGGED_MSG to the log for a all current subscriptions and instances
*/
@@ -312,46 +261,12 @@ private:
bool write_message(LogType type, void *ptr, size_t size);
/**
* Parse a file containing a list of uORB topics to log, calling add_topic for each
* @param fname name of file
* @return number of topics added
*/
int add_topics_from_file(const char *fname);
/**
* Add topic subscriptions based on the configured mission log type
*/
void initialize_mission_topics(MissionLogType type);
/**
* Add a topic to be logged for the mission log (it's also added to the full log).
* The interval is expected to be 0 or large (in the order of 0.1 seconds or higher).
* Must be called before all other topics are added.
* @param name topic name
* @param interval limit rate if >0 [ms], otherwise log as fast as the topic is updated.
*/
void add_mission_topic(const char *name, uint32_t interval_ms = 0);
/**
* Add topic subscriptions from SD file if it exists, otherwise call initialize_configured_topics()
* Add topic subscriptions from SD file if it exists, otherwise add topics based on the configured profile.
* This must be called before start_log() (because it does not write an ADD_LOGGED_MSG message).
* @return true on success
*/
bool initialize_topics(MissionLogType mission_log_mode);
/**
* Add topic subscriptions based on the _sdlog_profile_handle parameter
*/
void initialize_configured_topics();
void add_default_topics();
void add_estimator_replay_topics();
void add_thermal_calibration_topics();
void add_system_identification_topics();
void add_high_rate_topics();
void add_debug_topics();
void add_sensor_comparison_topics();
void add_vision_and_avoidance_topics();
/**
* check current arming state or aux channel and start/stop logging if state changed and according to
* configured params.
@@ -397,7 +312,6 @@ private:
LogMode _log_mode;
const bool _log_name_timestamp;
RequestedSubscriptionArray *_requested_subscriptions{nullptr}; ///< used during initialize_topics(), then translated to _subscriptions
LoggerSubscription *_subscriptions{nullptr}; ///< all subscriptions for full & mission log (in front)
int _num_subscriptions{0};
MissionSubscription _mission_subscriptions[MAX_MISSION_TOPICS_NUM] {}; ///< additional data for mission subscriptions