From 309465858a8e80a786a5094582110939ad290e94 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 19 Oct 2022 20:21:36 -0400 Subject: [PATCH] commander: elapsed time checks avoid subtracting unsigned integers - avoid the possibility of unsigned underflow from subtracting two HRT timestamps (uint64_t) - most of these aren't problematic, but people tend to replicate the pattern, so it's better to be safe - likely wasn't a problem when people were using hrt_absolute_time() in place, but if using an existing timestamp there's the possibility it's older than a more recent topic update --- src/modules/commander/Commander.cpp | 4 +-- .../HealthAndArmingChecks/Common.cpp | 2 +- .../HealthAndArmingChecks.cpp | 4 +-- .../checks/estimatorCheck.cpp | 26 ++++++++----------- 4 files changed, 16 insertions(+), 20 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f6e2d29ae2..e3abcffd8f 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1699,7 +1699,7 @@ void Commander::run() const bool nav_state_or_failsafe_changed = handleModeIntentionAndFailsafe(); // Run arming checks @ 10Hz - if (now - _last_health_and_arming_check >= 100_ms || _status_changed || nav_state_or_failsafe_changed) { + if ((now >= _last_health_and_arming_check + 100_ms) || _status_changed || nav_state_or_failsafe_changed) { _last_health_and_arming_check = now; perf_begin(_preflight_check_perf); @@ -1767,7 +1767,7 @@ void Commander::run() _actuator_armed.prearmed = getPrearmState(); // publish states (armed, control_mode, vehicle_status, failure_detector_status) at 2 Hz or immediately when changed - if (now - _vehicle_status.timestamp >= 500_ms || _status_changed || nav_state_or_failsafe_changed + if ((now >= _vehicle_status.timestamp + 500_ms) || _status_changed || nav_state_or_failsafe_changed || !(_actuator_armed == actuator_armed_prev)) { // publish actuator_armed first (used by output modules) diff --git a/src/modules/commander/HealthAndArmingChecks/Common.cpp b/src/modules/commander/HealthAndArmingChecks/Common.cpp index 84e2b6b856..ad9943e21f 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.cpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.cpp @@ -200,7 +200,7 @@ bool Report::report(bool is_armed, bool force) const hrt_abstime now = hrt_absolute_time(); const bool has_difference = _had_unreported_difference || _results_changed; - if (now - _last_report < _min_reporting_interval && !force) { + if ((now < _last_report + _min_reporting_interval) && !force) { if (has_difference) { _had_unreported_difference = true; } diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp index 038673d82a..9c4ec57950 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp @@ -102,8 +102,8 @@ bool HealthAndArmingChecks::update(bool force_reporting) // Check if we need to publish the failsafe flags const hrt_abstime now = hrt_absolute_time(); - if (now - _failsafe_flags.timestamp > 500_ms || results_changed) { - _failsafe_flags.timestamp = now; + if ((now > _failsafe_flags.timestamp + 500_ms) || results_changed) { + _failsafe_flags.timestamp = hrt_absolute_time(); _failsafe_flags_pub.publish(_failsafe_flags); } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 7eddcbb870..496206f210 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -606,12 +606,11 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report & _time_last_innov_pass = now; // if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed - const bool sufficient_time = context.status().takeoff_time != 0 - && now - context.status().takeoff_time > 30_s; + const bool sufficient_time = (context.status().takeoff_time != 0) && (now > context.status().takeoff_time + 30_s); const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f); // Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds - if (now - _time_last_innov_fail > 10_s && (sufficient_time || sufficient_speed)) { + if ((now > _time_last_innov_fail + 10_s) && (sufficient_time || sufficient_speed)) { _nav_test_passed = true; _nav_test_failed = false; } @@ -619,7 +618,7 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report & } else if (innovation_fail) { _time_last_innov_fail = now; - if (now - _time_last_innov_pass > 2_s) { + if (now > _time_last_innov_pass + 2_s) { // if the innovation test has failed continuously, declare the nav as failed _nav_test_failed = true; /* EVENT @@ -675,15 +674,14 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags) { // The following flags correspond to mode requirements, and are reported in the corresponding mode checks - - const hrt_abstime now = hrt_absolute_time(); - vehicle_global_position_s gpos; if (!_vehicle_global_position_sub.copy(&gpos)) { gpos = {}; } + const hrt_abstime now = hrt_absolute_time(); + // run position and velocity accuracy checks // Check if quality checking of position accuracy and consistency is to be performed float lpos_eph_threshold_relaxed = _param_com_pos_fs_eph.get(); @@ -726,8 +724,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f // altitude - failsafe_flags.local_altitude_invalid = !lpos.z_valid - || (now - lpos.timestamp > (_param_com_pos_fs_delay.get() * 1_s)); + failsafe_flags.local_altitude_invalid = !lpos.z_valid || (now > lpos.timestamp + (_param_com_pos_fs_delay.get() * 1_s)); // attitude @@ -742,7 +739,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f && (fabsf(q(3)) <= 1.f + eps); const bool norm_in_tolerance = fabsf(1.f - q.norm()) <= eps; - failsafe_flags.attitude_invalid = now > attitude.timestamp + 1_s || !norm_in_tolerance + failsafe_flags.attitude_invalid = (now > attitude.timestamp + 1_s) || !norm_in_tolerance || !no_element_larger_than_one; } else { @@ -753,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 = matrix::Vector3f(angular_velocity.xyz).isAllFinite(); const bool angular_velocity_invalid = !condition_angular_velocity_time_valid || !condition_angular_velocity_finite; @@ -774,7 +771,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f // gps if (vehicle_gps_position.timestamp != 0) { - bool time = now - vehicle_gps_position.timestamp < 1_s; + bool time = (now < vehicle_gps_position.timestamp + 1_s); bool fix = vehicle_gps_position.fix_type >= 2; bool eph = vehicle_gps_position.eph < _param_com_pos_fs_eph.get(); @@ -796,8 +793,7 @@ bool EstimatorChecks::checkPosVelValidity(const hrt_abstime &now, const bool dat const bool was_valid) const { bool valid = was_valid; - const bool data_stale = (now - data_timestamp_us > _param_com_pos_fs_delay.get() * 1_s) - || (data_timestamp_us == 0); + const bool data_stale = (now > data_timestamp_us + _param_com_pos_fs_delay.get() * 1_s) || (data_timestamp_us == 0); const float req_accuracy = (was_valid ? required_accuracy * 2.5f : required_accuracy); const bool level_check_pass = data_valid && !data_stale && (data_accuracy < req_accuracy); @@ -805,7 +801,7 @@ bool EstimatorChecks::checkPosVelValidity(const hrt_abstime &now, const bool dat if (level_check_pass) { if (!was_valid) { // check if probation period has elapsed - if (now - last_fail_time_us > 1_s) { + if (now > last_fail_time_us + 1_s) { valid = true; } }