commander: fix angular velocity validity

- vehicle_angular_velocity timestamp can be newer than saved now
This commit is contained in:
Daniel Agar 2022-09-09 00:34:22 -04:00
parent 15fece7e14
commit b45daac0e8
1 changed files with 1 additions and 1 deletions

View File

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