Browse Source

logger: refactor start_stop_logging

Cleans up function by reducing duplicate code and local variable count

Signed-off-by: Tal Zaitsev <tal@corvus-robotics.com>
sbg
tzai 5 years ago committed by Kabir Mohammed
parent
commit
fb0bf26787
  1. 74
      src/modules/logger/logger.cpp

74
src/modules/logger/logger.cpp

@ -929,76 +929,62 @@ bool Logger::get_disable_boot_logging() @@ -929,76 +929,62 @@ bool Logger::get_disable_boot_logging()
bool Logger::start_stop_logging(MissionLogType mission_log_type)
{
bool bret = false;
bool want_start = false;
bool want_stop = false;
bool updated = false;
bool desired_state = false;
if (_log_mode == LogMode::rc_aux1) {
// aux1-based logging
manual_control_setpoint_s manual_sp;
if (_manual_control_sp_sub.update(&manual_sp)) {
bool should_start = ((manual_sp.aux1 > 0.3f) || _manually_logging_override);
if (_prev_state != should_start) {
_prev_state = should_start;
if (should_start) {
want_start = true;
} else {
want_stop = true;
}
}
desired_state = (manual_sp.aux1 > 0.3f);
updated = true;
}
} else {
} else if (_log_mode != LogMode::boot_until_shutdown) {
// arming-based logging
vehicle_status_s vehicle_status;
if (_vehicle_status_sub.update(&vehicle_status)) {
bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) || _manually_logging_override;
desired_state = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
updated = true;
}
}
if (_prev_state != armed && _log_mode != LogMode::boot_until_shutdown) {
_prev_state = armed;
desired_state = desired_state || _manually_logging_override;
if (armed) {
want_start = true;
// only start/stop if this is a state transition
if (updated && _prev_state != desired_state) {
_prev_state = desired_state;
} else {
want_stop = true;
}
if (desired_state) {
if (_should_stop_file_log) { // happens on quick stop/start toggling
_should_stop_file_log = false;
stop_log_file(LogType::Full);
}
}
}
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);
start_log_file(LogType::Full);
bret = true;
if (mission_log_type != MissionLogType::Disabled) {
start_log_file(LogType::Mission);
}
if (mission_log_type != MissionLogType::Disabled) {
start_log_file(LogType::Mission);
}
return true;
} else if (want_stop) {
// delayed stop: we measure the process loads and then stop
initialize_load_output(PrintLoadReason::Postflight);
_should_stop_file_log = true;
} 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);
if (mission_log_type != MissionLogType::Disabled) {
stop_log_file(LogType::Mission);
}
}
}
return bret;
return false;
}
void Logger::handle_vehicle_command_update()

Loading…
Cancel
Save