From b45daac0e83cfab8c92262bdd4441fdcf46aee55 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 9 Sep 2022 00:34:22 -0400 Subject: [PATCH] commander: fix angular velocity validity - vehicle_angular_velocity timestamp can be newer than saved now --- .../commander/HealthAndArmingChecks/checks/estimatorCheck.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 8eba402998..3ae4692878 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -750,7 +750,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f vehicle_angular_velocity_s angular_velocity{}; _vehicle_angular_velocity_sub.copy(&angular_velocity); const bool condition_angular_velocity_time_valid = angular_velocity.timestamp != 0 - && now - angular_velocity.timestamp < 1_s; + && now < angular_velocity.timestamp + 1_s; const bool condition_angular_velocity_finite = PX4_ISFINITE(angular_velocity.xyz[0]) && PX4_ISFINITE(angular_velocity.xyz[1]) && PX4_ISFINITE(angular_velocity.xyz[2]); const bool angular_velocity_valid = condition_angular_velocity_time_valid