From c9f7c20d4667549d7d4d1b5943060ddc042f5044 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 19 Jan 2022 10:43:23 +0100 Subject: [PATCH] MulticoperPositionController: time failsafe messages together The following output instead of printing the action "stop and wait" just once: WARN [mc_pos_control] invalid setpoints WARN [mc_pos_control] invalid setpoints WARN [mc_pos_control] Failsafe: stop and wait WARN [mc_pos_control] invalid setpoints WARN [mc_pos_control] Failsafe: stop and wait WARN [mc_pos_control] invalid setpoints WARN [mc_pos_control] Failsafe: stop and wait --- src/modules/mc_pos_control/MulticopterPositionControl.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 69266c46fc..fc898efe28 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -321,8 +321,6 @@ void MulticopterPositionControl::Run() // set _dt in controllib Block for BlockDerivative setDt(dt); - - const bool was_in_failsafe = _in_failsafe; _in_failsafe = false; _vehicle_control_mode_sub.update(&_vehicle_control_mode); @@ -478,14 +476,16 @@ void MulticopterPositionControl::Run() } else { // Failsafe - if ((time_stamp_now - _last_warn) > 2_s) { + const bool warn_failsafe = (time_stamp_now - _last_warn) > 2_s; + + if (warn_failsafe) { PX4_WARN("invalid setpoints"); _last_warn = time_stamp_now; } vehicle_local_position_setpoint_s failsafe_setpoint{}; - failsafe(time_stamp_now, failsafe_setpoint, states, !was_in_failsafe); + failsafe(time_stamp_now, failsafe_setpoint, states, warn_failsafe); // reset constraints _vehicle_constraints = {0, NAN, NAN, false, {}};