diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 5f3dde304b..9b641c75b7 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -825,10 +825,10 @@ static void ahrs_update() ahrs.update(); - if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) + if (should_log(MASK_LOG_ATTITUDE_FAST)) Log_Write_Attitude(); - if (g.log_bitmask & MASK_LOG_IMU) + if (should_log(MASK_LOG_IMU)) Log_Write_IMU(); // calculate a scaled roll limit based on current pitch @@ -876,7 +876,7 @@ static void update_compass(void) if (g.compass_enabled && compass.read()) { ahrs.set_compass(&compass); compass.null_offsets(); - if (g.log_bitmask & MASK_LOG_COMPASS) { + if (should_log(MASK_LOG_COMPASS)) { Log_Write_Compass(); } } else { @@ -907,10 +907,10 @@ static void barometer_accumulate(void) */ static void update_logging1(void) { - if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST)) + if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) Log_Write_Attitude(); - if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_IMU)) + if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_IMU)) Log_Write_IMU(); } @@ -919,13 +919,13 @@ static void update_logging1(void) */ static void update_logging2(void) { - if (g.log_bitmask & MASK_LOG_CTUN) + if (should_log(MASK_LOG_CTUN)) Log_Write_Control_Tuning(); - if (g.log_bitmask & MASK_LOG_NTUN) + if (should_log(MASK_LOG_NTUN)) Log_Write_Nav_Tuning(); - if (g.log_bitmask & MASK_LOG_RC) + if (should_log(MASK_LOG_RC)) Log_Write_RC(); } @@ -968,7 +968,7 @@ static void update_aux(void) static void one_second_loop() { - if (g.log_bitmask & MASK_LOG_CURRENT) + if (should_log(MASK_LOG_CURRENT)) Log_Write_Current(); // send a heartbeat @@ -991,7 +991,7 @@ static void log_perf_info() if (scheduler.debug() != 0) { hal.console->printf_P(PSTR("G_Dt_max=%lu\n"), (unsigned long)G_Dt_max); } - if (g.log_bitmask & MASK_LOG_PM) + if (should_log(MASK_LOG_PM)) Log_Write_Performance(); G_Dt_max = 0; resetPerfData(); @@ -1037,7 +1037,7 @@ static void update_GPS_50Hz(void) g_gps->update(); if (g_gps->last_message_time_ms() != last_gps_reading) { last_gps_reading = g_gps->last_message_time_ms(); - if (g.log_bitmask & MASK_LOG_GPS) { + if (should_log(MASK_LOG_GPS)) { Log_Write_GPS(); } } @@ -1402,7 +1402,7 @@ static void update_alt() takeoff_pitch_cd, throttle_nudge, relative_altitude()); - if (g.log_bitmask & MASK_LOG_TECS) { + if (should_log(MASK_LOG_TECS)) { Log_Write_TECS_Tuning(); } } diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index ad740b204c..e3ad808116 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -245,7 +245,7 @@ static void do_RTL(void) setup_glide_slope(); - if (g.log_bitmask & MASK_LOG_MODE) + if (should_log(MASK_LOG_MODE)) Log_Write_Mode(control_mode); } @@ -574,7 +574,7 @@ static void do_jump() non_nav_command_index = g.command_index; non_nav_command_ID = WAIT_COMMAND; - if (g.log_bitmask & MASK_LOG_CMD) { + if (should_log(MASK_LOG_CMD)) { Log_Write_Cmd(g.command_index, &next_nav_command); } handle_process_nav_cmd(); @@ -668,7 +668,7 @@ static void do_take_picture() { #if CAMERA == ENABLED camera.trigger_pic(); - if (g.log_bitmask & MASK_LOG_CAMERA) { + if (should_log(MASK_LOG_CAMERA)) { Log_Write_Camera(); } #endif diff --git a/ArduPlane/commands_process.pde b/ArduPlane/commands_process.pde index 88f4230139..abe0095f13 100644 --- a/ArduPlane/commands_process.pde +++ b/ArduPlane/commands_process.pde @@ -92,7 +92,7 @@ static void process_next_command() non_nav_command_index = NO_COMMAND; // This will cause the next intervening non-nav command (if any) to be loaded non_nav_command_ID = NO_COMMAND; - if (g.log_bitmask & MASK_LOG_CMD) { + if (should_log(MASK_LOG_CMD)) { Log_Write_Cmd(g.command_index, &next_nav_command); } handle_process_nav_cmd(); @@ -130,7 +130,7 @@ static void process_next_command() gcs_send_text_fmt(PSTR("(2) Non-Nav command ID updated to #%i idx=%u"), (unsigned)non_nav_command_ID, (unsigned)non_nav_command_index); - if (g.log_bitmask & MASK_LOG_CMD) { + if (should_log(MASK_LOG_CMD)) { Log_Write_Cmd(g.command_index, &next_nonnav_command); } diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index e9dcb229d3..3cdf63d37e 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -162,6 +162,7 @@ enum log_messages { #define MASK_LOG_RC (1<<13) #define MASK_LOG_SONAR (1<<14) #define MASK_LOG_ARM_DISARM (1<<15) +#define MASK_LOG_WHEN_DISARMED (1<<30) // Waypoint Modes // ---------------- diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde index 6ac213518c..7cc6db1ede 100644 --- a/ArduPlane/sensors.pde +++ b/ArduPlane/sensors.pde @@ -40,7 +40,7 @@ static void read_sonars(void) return; } - if (g.log_bitmask & MASK_LOG_SONAR) + if (should_log(MASK_LOG_SONAR)) Log_Write_Sonar(); } diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 8d597b46c7..2a1b1706c3 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -205,7 +205,7 @@ static void init_ardupilot() } startup_ground(); - if (g.log_bitmask & MASK_LOG_CMD) + if (should_log(MASK_LOG_CMD)) Log_Write_Startup(TYPE_GROUNDSTART_MSG); // choose the nav controller @@ -359,7 +359,7 @@ static void set_mode(enum FlightMode mode) throttle_suppressed = false; } - if (g.log_bitmask & MASK_LOG_MODE) + if (should_log(MASK_LOG_MODE)) Log_Write_Mode(control_mode); // reset attitude integrators on mode change @@ -607,3 +607,22 @@ static void servo_write(uint8_t ch, uint16_t pwm) hal.rcout->enable_ch(ch); hal.rcout->write(ch, pwm); } + +/* + should we log a message type now? + */ +static bool should_log(uint32_t mask) +{ + if (!(mask & g.log_bitmask)) { + return false; + } + bool armed; + if (arming.arming_required() == AP_Arming::NO) { + // for logging purposes consider us armed if we either don't + // have a safety switch, or we have one and it is disarmed + armed = (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED); + } else { + armed = arming.is_armed(); + } + return armed || (g.log_bitmask & MASK_LOG_WHEN_DISARMED) != 0; +}