dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fix commander: check estimator type & avoid timestamp wrap-around 

Before it was possible that the publication timestamp was never than 'now',
leading to wrap-around.
This commit is contained in:
Beat Küng 2022-09-03 10:35:01 +02:00 committed by Daniel Agar
parent 27f8298bb9
commit 82911e48be
2 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
View File

@ -102,7 +102,7 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
}
}
if (missing_data) {
if (missing_data && _param_sys_mc_est_group.get() == 2) {
/* EVENT
*/
reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
@ -740,7 +740,8 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
&& (fabsf(q(3)) <= 1.f + eps);
const bool norm_in_tolerance = fabsf(1.f - q.norm()) <= eps;
failsafe_flags.attitude_valid = now - attitude.timestamp < 1_s && norm_in_tolerance && no_element_larger_than_one;
failsafe_flags.attitude_valid = now < attitude.timestamp + 1_s && norm_in_tolerance
&& no_element_larger_than_one;
} else {
failsafe_flags.attitude_valid = false;

1
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
View File

@ -105,6 +105,7 @@ private:
bool _position_reliant_on_optical_flow{false};
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
(ParamInt<px4::params::SYS_MC_EST_GROUP>) _param_sys_mc_est_group,
(ParamInt<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode,
(ParamInt<px4::params::COM_ARM_MAG_STR>) _param_com_arm_mag_str,
(ParamFloat<px4::params::COM_ARM_EKF_HGT>) _param_com_arm_ekf_hgt,