mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-17 07:27:34 +08:00
commander: remove unused pos vel validity check functions
This commit is contained in:
Paul Riseborough
committed by
Lorenz Meier
Lorenz Meier
parent
625cc4aa83
commit
1a2ef45a4b
@@ -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)
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user