Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

Commander: check_posvel_validity use return value

Browse Source
This commit is contained in:
Matthias Grob
2021-03-30 21:11:46 +02:00
committed by Daniel Agar
parent 29e1e0905c
commit b7600f4e4d
2 changed files with 12 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+11 -12
View File
@@ -3093,9 +3093,9 @@ Commander::reset_posvel_validity()
bool
Commander::check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy,
const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us, bool *valid_state)
const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us,
const bool was_valid)
{
const bool was_valid = *valid_state;
bool valid = was_valid;
// constrain probation times
@@ -3141,7 +3141,6 @@ Commander::check_posvel_validity(const bool data_valid, const float data_accurac
if (was_valid != valid) {
_status_changed = true;
*valid_state = valid;
}
return valid;
@@ -3847,17 +3846,17 @@ void Commander::UpdateEstimateValidity()
}
}
// condition_global_position_valid
check_posvel_validity(lpos.xy_valid && !_nav_test_failed, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
&_last_gpos_fail_time_us, &_gpos_probation_time_us, &_status_flags.condition_global_position_valid);
_status_flags.condition_global_position_valid =
check_posvel_validity(lpos.xy_valid && !_nav_test_failed, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
&_last_gpos_fail_time_us, &_gpos_probation_time_us, _status_flags.condition_global_position_valid);
// condition_local_position_valid
check_posvel_validity(lpos.xy_valid && !_nav_test_failed, lpos.eph, lpos_eph_threshold_adj, lpos.timestamp,
&_last_lpos_fail_time_us, &_lpos_probation_time_us, &_status_flags.condition_local_position_valid);
_status_flags.condition_local_position_valid =
check_posvel_validity(lpos.xy_valid && !_nav_test_failed, lpos.eph, lpos_eph_threshold_adj, lpos.timestamp,
&_last_lpos_fail_time_us, &_lpos_probation_time_us, _status_flags.condition_local_position_valid);
// condition_local_velocity_valid
check_posvel_validity(lpos.v_xy_valid && !_nav_test_failed, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
&_last_lvel_fail_time_us, &_lvel_probation_time_us, &_status_flags.condition_local_velocity_valid);
_status_flags.condition_local_velocity_valid =
check_posvel_validity(lpos.v_xy_valid && !_nav_test_failed, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
&_last_lvel_fail_time_us, &_lvel_probation_time_us, _status_flags.condition_local_velocity_valid);
}
void
src/modules/commander/Commander.hpp
+1 -1
View File
@@ -132,7 +132,7 @@ private:
bool check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy,
const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us,
bool *valid_state);
const bool was_valid);
void control_status_leds(bool changed, const uint8_t battery_warning);