diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d42b96b4c2..45b1305667 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -299,10 +299,6 @@ transition_result_t set_main_state_rc(struct vehicle_status_s *status, vehicle_g void reset_posvel_validity(vehicle_global_position_s *global_position, vehicle_local_position_s *local_position, bool *changed); -void check_global_posvel_validity(vehicle_global_position_s *global_position, bool *changed); - -void check_local_posvel_validity(vehicle_local_position_s *local_position, bool *changed); - void check_posvel_validity(bool data_valid, float data_accuracy, float required_accuracy, uint64_t data_timestamp_us, hrt_abstime *last_fail_time_us, int64_t *probation_time_us, bool *valid_state, bool *validity_changed); void set_control_mode(); @@ -3779,202 +3775,6 @@ reset_posvel_validity(vehicle_global_position_s *global_position, vehicle_local_ check_posvel_validity(local_position->v_xy_valid, local_position->evh, evh_threshold, local_position->timestamp, &last_lvel_fail_time_us, &lvel_probation_time_us, &status_flags.condition_local_velocity_valid, changed); } -void -check_global_posvel_validity(vehicle_global_position_s *global_position, bool *changed) -{ - bool global_pos_inaccurate = false; - bool global_vel_inaccurate = 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 = now; - } else if (!status_flags.condition_global_position_valid) { - bool level_check_pass = global_position->eph < eph_threshold; - 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 -= (now - last_gpos_fail_time_us); - last_gpos_fail_time_us = now; - } - - // check global velocity accuracy with hysteresis in both test level and time - bool vel_status_changed = false; - 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 = now; - } else if (!status_flags.condition_global_velocity_valid) { - bool check_level_pass = global_position->evh < evh_threshold; - 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 -= (now - last_gvel_fail_time_us); - last_gvel_fail_time_us = now; - } - - 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_velocity_valid = false; - *changed = true; - } else if (!status_flags.condition_global_velocity_valid - && !global_data_stale - && !global_vel_inaccurate) { - status_flags.condition_global_velocity_valid = true; - *changed = true; - } - } - - // Set global position validity - if (pos_status_changed) { - if (status_flags.condition_global_position_valid - && (global_data_stale || global_pos_inaccurate)) { - status_flags.condition_global_position_valid = false; - set_tune_override(TONE_GPS_WARNING_TUNE); - *changed = true; - } else if (!status_flags.condition_global_position_valid - && !global_data_stale - && !global_pos_inaccurate) { - status_flags.condition_global_position_valid = true; - *changed = true; - } - } - - // constrain probation times - if (land_detector.landed) { - gpos_probation_time_us = POSVEL_PROBATION_MIN; - gvel_probation_time_us = POSVEL_PROBATION_MIN; - } else { - if (gpos_probation_time_us < POSVEL_PROBATION_MIN) { - gpos_probation_time_us = POSVEL_PROBATION_MIN; - } else if (gpos_probation_time_us > POSVEL_PROBATION_MAX) { - gpos_probation_time_us = POSVEL_PROBATION_MAX; - } - if (gvel_probation_time_us < POSVEL_PROBATION_MIN) { - gvel_probation_time_us = POSVEL_PROBATION_MIN; - } else if (gvel_probation_time_us > POSVEL_PROBATION_MAX) { - gvel_probation_time_us = POSVEL_PROBATION_MAX; - } - } -} - -void -check_local_posvel_validity(struct vehicle_local_position_s *local_position, bool *changed) -{ - bool local_pos_inaccurate = false; - bool local_vel_inaccurate = 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 = 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) { - 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 -= (now - last_lpos_fail_time_us); - last_lpos_fail_time_us = now; - } - - // Check local velocity accuracy with hysteresis - bool vel_status_changed = false; - 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 = 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) { - 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 -= (now - last_lvel_fail_time_us); - last_lvel_fail_time_us = now; - } - - bool local_data_stale = (now - local_position->timestamp > POSITION_TIMEOUT); - - // Set local velocity validity - if (vel_status_changed) { - if (status_flags.condition_local_velocity_valid - && (local_data_stale || local_vel_inaccurate)) { - status_flags.condition_local_velocity_valid = false; - *changed = true; - } else if (!status_flags.condition_local_velocity_valid - && !local_data_stale - && !local_vel_inaccurate) { - status_flags.condition_local_velocity_valid = true; - *changed = true; - } - } - - // Set local position validity - if (pos_status_changed) { - if (status_flags.condition_local_position_valid - && (local_data_stale || !local_position->xy_valid || local_pos_inaccurate)) { - status_flags.condition_local_position_valid = false; - *changed = true; - } else if (!status_flags.condition_local_position_valid - && !local_data_stale - && !local_pos_inaccurate - && local_position->xy_valid) { - status_flags.condition_local_position_valid = true; - *changed = true; - } - } - - // constrain probation times - if (land_detector.landed) { - lpos_probation_time_us = POSVEL_PROBATION_MIN; - lvel_probation_time_us = POSVEL_PROBATION_MIN; - } else { - if (lpos_probation_time_us < POSVEL_PROBATION_MIN) { - lpos_probation_time_us = POSVEL_PROBATION_MIN; - } else if (lpos_probation_time_us > POSVEL_PROBATION_MAX) { - lpos_probation_time_us = POSVEL_PROBATION_MAX; - } - if (lvel_probation_time_us < POSVEL_PROBATION_MIN) { - lvel_probation_time_us = POSVEL_PROBATION_MIN; - } else if (lvel_probation_time_us > POSVEL_PROBATION_MAX) { - lvel_probation_time_us = POSVEL_PROBATION_MAX; - } - } -} - void check_posvel_validity(bool data_valid, float data_accuracy, float required_accuracy, uint64_t data_timestamp_us, hrt_abstime *last_fail_time_us, int64_t *probation_time_us, bool *valid_state, bool *validity_changed) {