diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 84aade3eb5..0352ab8c61 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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 diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 82eefc5184..502c246238 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -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);