diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index e4eb6f4cf2..eadcd65a1e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -27,6 +27,11 @@ then set LOGGER_ARGS "-f" fi +if param compare SDLOG_MODE 3 +then + set LOGGER_ARGS "-x" +fi + if ! param compare SDLOG_MODE -1 then logger start -b ${LOGGER_BUF} -t ${LOGGER_ARGS} diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 4a9d04e937..877725b85e 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include @@ -198,6 +199,7 @@ $ logger on PRINT_MODULE_USAGE_NAME("logger", "system"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAM_STRING('m', "all", "file|mavlink|all", "Backend mode", true); + PRINT_MODULE_USAGE_PARAM_FLAG('x', "Enable/disable logging via Aux1 RC channel", true); PRINT_MODULE_USAGE_PARAM_FLAG('e', "Enable logging right after start until disarm (otherwise only when armed)", true); PRINT_MODULE_USAGE_PARAM_FLAG('f', "Log until shutdown (implies -e)", true); PRINT_MODULE_USAGE_PARAM_FLAG('t', "Use date/time for naming log directories and files", true); @@ -290,6 +292,7 @@ Logger *Logger::instantiate(int argc, char *argv[]) int log_buffer_size = 12 * 1024; bool log_on_start = false; bool log_until_shutdown = false; + Logger::LogMode log_mode = Logger::LogMode::while_armed; bool error_flag = false; bool log_name_timestamp = false; unsigned int queue_size = 14; //TODO: we might be able to reduce this if mavlink polled on the topic and/or @@ -301,7 +304,7 @@ Logger *Logger::instantiate(int argc, char *argv[]) int ch; const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "r:b:etfm:q:p:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "r:b:etfm:q:p:x", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'r': { unsigned long r = strtoul(myoptarg, nullptr, 10); @@ -314,6 +317,10 @@ Logger *Logger::instantiate(int argc, char *argv[]) } break; + case 'x': + log_mode = Logger::LogMode::rc_aux1; + break; + case 'e': log_on_start = true; break; @@ -379,12 +386,19 @@ Logger *Logger::instantiate(int argc, char *argv[]) } } + if (log_on_start) { + if (log_until_shutdown) { + log_mode = Logger::LogMode::boot_until_shutdown; + } else { + log_mode = Logger::LogMode::boot_until_disarm; + } + } + if (error_flag) { return nullptr; } - Logger *logger = new Logger(backend, log_buffer_size, log_interval, poll_topic, log_on_start, - log_until_shutdown, log_name_timestamp, queue_size); + Logger *logger = new Logger(backend, log_buffer_size, log_interval, poll_topic, log_mode, log_name_timestamp, queue_size); #if defined(DBGPRINT) && defined(__PX4_NUTTX) struct mallinfo alloc_info = mallinfo(); @@ -413,10 +427,8 @@ Logger *Logger::instantiate(int argc, char *argv[]) Logger::Logger(LogWriter::Backend backend, size_t buffer_size, uint32_t log_interval, const char *poll_topic_name, - bool log_on_start, bool log_until_shutdown, bool log_name_timestamp, unsigned int queue_size) : - _arm_override(false), - _log_on_start(log_on_start), - _log_until_shutdown(log_until_shutdown), + LogMode log_mode, bool log_name_timestamp, unsigned int queue_size) : + _log_mode(log_mode), _log_name_timestamp(log_name_timestamp), _writer(backend, buffer_size, queue_size), _log_interval(log_interval) @@ -941,6 +953,11 @@ void Logger::run() } } + int manual_control_sp_sub = -1; + if (_log_mode == LogMode::rc_aux1) { + manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + } + int ntopics = add_topics_from_file(PX4_STORAGEDIR "/etc/logging/logger_topics.txt"); if (ntopics > 0) { @@ -1004,9 +1021,7 @@ void Logger::run() px4_register_shutdown_hook(&Logger::request_stop_static); - // we start logging immediately - // the case where we wait with logging until vehicle is armed is handled below - if (_log_on_start) { + if (_log_mode == LogMode::boot_until_disarm || _log_mode == LogMode::boot_until_shutdown) { start_log_file(LogType::Full); } @@ -1047,8 +1062,8 @@ void Logger::run() while (!should_exit()) { - // Start/stop logging when system arm/disarm - const bool logging_started = check_arming_state(vehicle_status_sub, (MissionLogType)mission_log_mode); + // Start/stop logging (depending on logging mode, by default when arming/disarming) + const bool logging_started = start_stop_logging(vehicle_status_sub, manual_control_sp_sub, (MissionLogType)mission_log_mode); if (logging_started) { #ifdef DBGPRINT timer_start = hrt_absolute_time(); @@ -1272,6 +1287,10 @@ void Logger::run() } } + if (manual_control_sp_sub != -1) { + orb_unsubscribe(manual_control_sp_sub); + } + if (polling_topic_sub >= 0) { orb_unsubscribe(polling_topic_sub); } @@ -1312,44 +1331,76 @@ void Logger::debug_print_buffer(uint32_t &total_bytes, hrt_abstime &timer_start) #endif /* DBGPRINT */ } -bool Logger::check_arming_state(int vehicle_status_sub, MissionLogType mission_log_type) +bool Logger::start_stop_logging(int vehicle_status_sub, int manual_control_sp_sub, MissionLogType mission_log_type) { - bool vehicle_status_updated; - int ret = orb_check(vehicle_status_sub, &vehicle_status_updated); bool bret = false; + bool want_start = false; + bool want_stop = false; - if (ret == 0 && vehicle_status_updated) { - vehicle_status_s vehicle_status; - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); - bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) || _arm_override; + if (_log_mode == LogMode::rc_aux1) { + //aux1-based logging + bool manual_control_setpoint_updated; + int ret = orb_check(manual_control_sp_sub, &manual_control_setpoint_updated); - if (_was_armed != armed && !_log_until_shutdown) { - _was_armed = armed; + if (ret == 0 && manual_control_setpoint_updated) { + manual_control_setpoint_s manual_sp; + orb_copy(ORB_ID(manual_control_setpoint), manual_control_sp_sub, &manual_sp); + bool should_start = manual_sp.aux1 > 0.3f || _manually_logging_override; - if (armed) { + if (_prev_state != should_start) { + _prev_state = should_start; + if (should_start) { + want_start = true; - if (_should_stop_file_log) { // happens on quick arming after disarm - _should_stop_file_log = false; - stop_log_file(LogType::Full); - } - - start_log_file(LogType::Full); - bret = true; - - if (mission_log_type != MissionLogType::Disabled) { - start_log_file(LogType::Mission); - } - - } else { - // delayed stop: we measure the process loads and then stop - initialize_load_output(PrintLoadReason::Postflight); - _should_stop_file_log = true; - - if (mission_log_type != MissionLogType::Disabled) { - stop_log_file(LogType::Mission); + } else { + want_stop = true; } } } + + } else { + // arming-based logging + bool vehicle_status_updated; + int ret = orb_check(vehicle_status_sub, &vehicle_status_updated); + + if (ret == 0 && vehicle_status_updated) { + vehicle_status_s vehicle_status; + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) || _manually_logging_override; + + if (_prev_state != armed && _log_mode != LogMode::boot_until_shutdown) { + _prev_state = armed; + + if (armed) { + want_start = true; + + } else { + want_stop = true; + } + } + } + } + + if (want_start) { + if (_should_stop_file_log) { // happens on quick stop/start toggling + _should_stop_file_log = false; + stop_log_file(LogType::Full); + } + + start_log_file(LogType::Full); + bret = true; + + if (mission_log_type != MissionLogType::Disabled) { + start_log_file(LogType::Mission); + } + } else if (want_stop) { + // delayed stop: we measure the process loads and then stop + initialize_load_output(PrintLoadReason::Postflight); + _should_stop_file_log = true; + + if (mission_log_type != MissionLogType::Disabled) { + stop_log_file(LogType::Mission); + } } return bret; } diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 11d5a6c158..7a9a74f6a2 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -105,8 +105,15 @@ struct LoggerSubscription { class Logger : public ModuleBase { public: + enum class LogMode { + while_armed = 0, + boot_until_disarm, + boot_until_shutdown, + rc_aux1 + }; + Logger(LogWriter::Backend backend, size_t buffer_size, uint32_t log_interval, const char *poll_topic_name, - bool log_on_start, bool log_until_shutdown, bool log_name_timestamp, unsigned int queue_size); + LogMode log_mode, bool log_name_timestamp, unsigned int queue_size); ~Logger(); @@ -160,7 +167,7 @@ public: void print_statistics(LogType type); - void set_arm_override(bool override) { _arm_override = override; } + void set_arm_override(bool override) { _manually_logging_override = override; } private: @@ -336,12 +343,14 @@ private: void add_vision_and_avoidance_topics(); /** - * check current arming state and start/stop logging if state changed and according to configured params. + * check current arming state or aux channel and start/stop logging if state changed and according to + * configured params. * @param vehicle_status_sub + * @param manual_control_sp_sub * @param mission_log_type * @return true if log started */ - bool check_arming_state(int vehicle_status_sub, MissionLogType mission_log_type); + bool start_stop_logging(int vehicle_status_sub, int manual_control_sp_sub, MissionLogType mission_log_type); void handle_vehicle_command_update(int vehicle_command_sub, orb_advert_t &vehicle_command_ack_pub); void ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, vehicle_command_s *cmd, uint32_t result); @@ -369,13 +378,12 @@ private: LogFileName _file_name[(int)LogType::Count]; - bool _was_armed{false}; - bool _arm_override{false}; + bool _prev_state{false}; ///< previous state depending on logging mode (arming or aux1 state) + bool _manually_logging_override{false}; Statistics _statistics[(int)LogType::Count]; - const bool _log_on_start; - const bool _log_until_shutdown; + LogMode _log_mode; const bool _log_name_timestamp; Array _subscriptions; ///< all subscriptions for full & mission log (in front) diff --git a/src/modules/logger/params.c b/src/modules/logger/params.c index d4ec705531..3da2362510 100644 --- a/src/modules/logger/params.c +++ b/src/modules/logger/params.c @@ -59,6 +59,7 @@ PARAM_DEFINE_INT32(SDLOG_UTC_OFFSET, 0); * @value 0 when armed until disarm (default) * @value 1 from boot until disarm * @value 2 from boot until shutdown + * @value 3 depending on AUX1 RC channel * * @reboot_required true * @group SD Logging