dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

logger: add arming/disarming via AUX1 RC channel logging mode

This commit is contained in:
Beat Küng 2019-04-17 07:07:46 +02:00
parent 721f9f901f
commit ed9d25a75a
4 changed files with 114 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ROMFS/px4fmu_common/init.d/rc.logging
View File

@ -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}

133
src/modules/logger/logger.cpp
View File

@ -50,6 +50,7 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <drivers/drv_hrt.h>
#include <px4_getopt.h>
@ -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;
}

24
src/modules/logger/logger.h
View File

@ -105,8 +105,15 @@ struct LoggerSubscription {
class Logger : public ModuleBase<Logger>
{
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<LoggerSubscription, MAX_TOPICS_NUM> _subscriptions; ///< all subscriptions for full & mission log (in front)

1
src/modules/logger/params.c
View File

@ -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