zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

commander: remove unused pos vel validity check functions

sbg
Paul Riseborough 8 years ago committed by Lorenz Meier
parent
625cc4aa83
commit
1a2ef45a4b
1 changed files with 0 additions and 200 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 200
      src/modules/commander/commander.cpp

200
src/modules/commander/commander.cpp
Unescape View File

@ -299,10 +299,6 @@ transition_result_t set_main_state_rc(struct vehicle_status_s *status, vehicle_g @@ -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_ @@ -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)
{

Write Preview
Loading…
Cancel
Save