From 8421ad3dfd7ab329a5ce770ed74d7f690471f27f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 22 Apr 2017 08:15:24 +1000 Subject: [PATCH] commander: Changes following code review --- src/modules/commander/PreflightCheck.cpp | 10 +-- src/modules/commander/commander.cpp | 81 ++++++++++++------------ 2 files changed, 47 insertions(+), 44 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 63f3873d58..172c25a861 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -493,7 +493,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_ // If GPS aiding is required, declare fault condition if the EKF is not using GPS if (enforce_gps_required) { - if (!(status.control_mode_flags & 2)) { + if (!(status.control_mode_flags & (1<<2))) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF NOT USING GPS"); } @@ -504,10 +504,10 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_ // If GPS aiding is required, declare fault condition if the required GPS quality checks are failing if (enforce_gps_required) { - if ((status.gps_check_fail_flags & (estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT - | estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP - | estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR - | estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) > 0) { + if ((status.gps_check_fail_flags & ((1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT) + + (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP) + + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR) + + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR))) > 0) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY CHECKS"); } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0497695058..1274389a4c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -165,7 +165,10 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define POSVEL_PROBATION_MAX 100E6 /**< maximum probation duration (usec) */ #define POSVEL_VALID_PROBATION_FACTOR 10 /**< the rate at which the probation duration is increased while checks are failing */ -/* Probation times for position and velocity validity checks to pass if failed */ +/* + * Probation times for position and velocity validity checks to pass if failed + * Signed integers are used because these can become negative values before constraints are applied + */ static int64_t gpos_probation_time_us = POSVEL_PROBATION_TAKEOFF; static int64_t gvel_probation_time_us = POSVEL_PROBATION_TAKEOFF; static int64_t lpos_probation_time_us = POSVEL_PROBATION_TAKEOFF; @@ -195,10 +198,10 @@ static float eph_threshold = 5.0f; // Horizontal position error threshold (m) static float epv_threshold = 10.0f; // Vertivcal position error threshold (m) static float evh_threshold = 1.0f; // Horizontal velocity error threshold (m) -static uint64_t last_lpos_fail_time_us = 0; // Last time that the local position validity recovery check failed (usec) -static uint64_t last_gpos_fail_time_us = 0; // Last time that the global position validity recovery check failed (usec) -static uint64_t last_lvel_fail_time_us = 0; // Last time that the local velocity validity recovery check failed (usec) -static uint64_t last_gvel_fail_time_us = 0; // Last time that the global velocity validity recovery check failed (usec) +static hrt_abstime last_lpos_fail_time_us = 0; // Last time that the local position validity recovery check failed (usec) +static hrt_abstime last_gpos_fail_time_us = 0; // Last time that the global position validity recovery check failed (usec) +static hrt_abstime last_lvel_fail_time_us = 0; // Last time that the local velocity validity recovery check failed (usec) +static hrt_abstime last_gvel_fail_time_us = 0; // Last time that the global velocity validity recovery check failed (usec) /* pre-flight EKF checks */ static float max_ekf_pos_ratio = 0.5f; @@ -2200,7 +2203,7 @@ int commander_thread_main(int argc, char *argv[]) // Set all position and velocity test probation durations to takeoff value // This is a larger value to give the vehicle time to complete a failsafe landing - // if faulty sensors cause loss of navigatio shortly after takeoff. + // if faulty sensors cause loss of navigation shortly after takeoff. gpos_probation_time_us = POSVEL_PROBATION_TAKEOFF; gvel_probation_time_us = POSVEL_PROBATION_TAKEOFF; lpos_probation_time_us = POSVEL_PROBATION_TAKEOFF; @@ -3775,27 +3778,27 @@ check_global_posvel_validity(vehicle_global_position_s *global_position, bool *c { bool global_pos_inaccurate = false; bool global_vel_inaccurate = false; - bool hold_fail_state = false; + hrt_abstime now = hrt_absolute_time(); // Check position accuracy with hysteresis in both test level and time bool pos_status_changed = false; if (status_flags.condition_global_position_valid && global_position->eph > eph_threshold * 2.5f) { global_pos_inaccurate = true; pos_status_changed = true; - last_gpos_fail_time_us = hrt_absolute_time(); + last_gpos_fail_time_us = now; } else if (!status_flags.condition_global_position_valid) { bool level_check_pass = global_position->eph < eph_threshold; - if (!level_check_pass || hold_fail_state) { - gpos_probation_time_us += (hrt_absolute_time() - last_gpos_fail_time_us) * POSVEL_VALID_PROBATION_FACTOR; - last_gpos_fail_time_us = hrt_absolute_time(); - } else if (hrt_absolute_time() - last_gpos_fail_time_us > gpos_probation_time_us) { + if (!level_check_pass) { + gpos_probation_time_us += (now - last_gpos_fail_time_us) * POSVEL_VALID_PROBATION_FACTOR; + last_gpos_fail_time_us = now; + } else if (now - last_gpos_fail_time_us > gpos_probation_time_us) { global_pos_inaccurate = false; pos_status_changed = true; last_gpos_fail_time_us = 0; } } else { - gpos_probation_time_us -= (hrt_absolute_time() - last_gpos_fail_time_us); - last_gpos_fail_time_us = hrt_absolute_time(); + gpos_probation_time_us -= (now - last_gpos_fail_time_us); + last_gpos_fail_time_us = now; } // check global velocity accuracy with hysteresis in both test level and time @@ -3803,29 +3806,29 @@ check_global_posvel_validity(vehicle_global_position_s *global_position, bool *c if (status_flags.condition_global_velocity_valid && global_position->evh > evh_threshold * 2.5f) { global_vel_inaccurate = true; vel_status_changed = true; - last_gvel_fail_time_us = hrt_absolute_time(); + last_gvel_fail_time_us = now; } else if (!status_flags.condition_global_velocity_valid) { bool check_level_pass = global_position->evh < evh_threshold; - if (!check_level_pass || hold_fail_state) { - gvel_probation_time_us += (hrt_absolute_time() - last_gvel_fail_time_us) * POSVEL_VALID_PROBATION_FACTOR; - last_gvel_fail_time_us = hrt_absolute_time(); - } else if (hrt_absolute_time() - last_gvel_fail_time_us > gvel_probation_time_us) { + if (!check_level_pass) { + gvel_probation_time_us += (now - last_gvel_fail_time_us) * POSVEL_VALID_PROBATION_FACTOR; + last_gvel_fail_time_us = now; + } else if (now - last_gvel_fail_time_us > gvel_probation_time_us) { global_vel_inaccurate = false; vel_status_changed = true; last_gvel_fail_time_us = 0; } } else { - gvel_probation_time_us -= (hrt_absolute_time() - last_gvel_fail_time_us); - last_gvel_fail_time_us = hrt_absolute_time(); + gvel_probation_time_us -= (now - last_gvel_fail_time_us); + last_gvel_fail_time_us = now; } - bool global_data_stale = (hrt_absolute_time() - global_position->timestamp > POSITION_TIMEOUT); + bool global_data_stale = (now - global_position->timestamp > POSITION_TIMEOUT); // Set global velocity validity if (vel_status_changed) { if (status_flags.condition_global_velocity_valid && (global_data_stale || global_vel_inaccurate)) { - status_flags.condition_global_position_valid = false; + status_flags.condition_global_velocity_valid = false; *changed = true; } else if (!status_flags.condition_global_velocity_valid && !global_data_stale @@ -3873,27 +3876,27 @@ check_local_posvel_validity(struct vehicle_local_position_s *local_position, boo { bool local_pos_inaccurate = false; bool local_vel_inaccurate = false; - bool hold_fail_state = false; + hrt_abstime now = hrt_absolute_time(); // Check local position accuracy with hysteresis in both test level and time bool pos_status_changed = false; if (status_flags.condition_local_position_valid && local_position->eph > eph_threshold * 2.5f) { local_pos_inaccurate = true; pos_status_changed = true; - last_lpos_fail_time_us = hrt_absolute_time(); + last_lpos_fail_time_us = now; } else if (!status_flags.condition_local_position_valid) { bool level_check_pass = local_position->xy_valid && local_position->eph < eph_threshold; - if (!level_check_pass || hold_fail_state) { - lpos_probation_time_us += (hrt_absolute_time() - last_lpos_fail_time_us) * POSVEL_VALID_PROBATION_FACTOR; - last_lpos_fail_time_us = hrt_absolute_time(); - } else if (hrt_absolute_time() - last_lpos_fail_time_us > lpos_probation_time_us) { + if (!level_check_pass) { + lpos_probation_time_us += (now - last_lpos_fail_time_us) * POSVEL_VALID_PROBATION_FACTOR; + last_lpos_fail_time_us = now; + } else if (now - last_lpos_fail_time_us > lpos_probation_time_us) { local_pos_inaccurate = false; pos_status_changed = true; last_lpos_fail_time_us = 0; } } else { - lpos_probation_time_us -= (hrt_absolute_time() - last_lpos_fail_time_us); - last_lpos_fail_time_us = hrt_absolute_time(); + lpos_probation_time_us -= (now - last_lpos_fail_time_us); + last_lpos_fail_time_us = now; } // Check local velocity accuracy with hysteresis @@ -3901,23 +3904,23 @@ check_local_posvel_validity(struct vehicle_local_position_s *local_position, boo if (status_flags.condition_local_velocity_valid && local_position->evh > evh_threshold * 2.5f) { local_vel_inaccurate = true; vel_status_changed = true; - last_lvel_fail_time_us = hrt_absolute_time(); + last_lvel_fail_time_us = now; } else if (!status_flags.condition_local_velocity_valid) { bool level_check_pass = local_position->v_xy_valid && local_position->evh < evh_threshold; - if (!level_check_pass || hold_fail_state) { - lvel_probation_time_us += (hrt_absolute_time() - last_lvel_fail_time_us) * POSVEL_VALID_PROBATION_FACTOR; - last_lvel_fail_time_us = hrt_absolute_time(); - } else if (hrt_absolute_time() - last_lvel_fail_time_us > lvel_probation_time_us) { + if (!level_check_pass) { + lvel_probation_time_us += (now - last_lvel_fail_time_us) * POSVEL_VALID_PROBATION_FACTOR; + last_lvel_fail_time_us = now; + } else if (now - last_lvel_fail_time_us > lvel_probation_time_us) { local_vel_inaccurate = false; vel_status_changed = true; last_lvel_fail_time_us = 0; } } else { - lvel_probation_time_us -= (hrt_absolute_time() - last_lvel_fail_time_us); - last_lvel_fail_time_us = hrt_absolute_time(); + lvel_probation_time_us -= (now - last_lvel_fail_time_us); + last_lvel_fail_time_us = now; } - bool local_data_stale = (hrt_absolute_time() - local_position->timestamp > POSITION_TIMEOUT); + bool local_data_stale = (now - local_position->timestamp > POSITION_TIMEOUT); // Set local velocity validity if (vel_status_changed) {