fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-09 03:48:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

fixes

Browse Source 
Signed-off-by: Silvan <silvan@auterion.com>
This commit is contained in:
Silvan
2025-10-24 13:25:36 +02:00
committed by Pernilla
parent 7c85d756f9
commit f9257147cb
4 changed files with 13 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
+10 -20
View File
@@ -656,30 +656,20 @@ void EstimatorChecks::deadReckoningTimeout(const Context &context, Report &repor
_vehicle_land_detected_sub.copy(&vehicle_land_detected);
_in_air_valid = (now < vehicle_land_detected.timestamp + 2_s) && !vehicle_land_detected.landed;
bool dead_reckoning_valid_but_timouted = false;
bool dead_reckoning_changed = _was_dead_reckoning != lpos.dead_reckoning;
if (!lpos.dead_reckoning || !_in_air_valid) {
_last_not_dead_reckoning_time_us = now;
if (dead_reckoning_changed && _in_air_valid) {
if (lpos.dead_reckoning) {
_last_initiated_dead_reckoning_time_us = now;
} else {
reporter.failsafeFlags().mode_req_dead_reckoning_invalid = true;
reporter.failsafeFlags().dead_reckoning_invalid = false;
_last_initiated_dead_reckoning_time_us = 0;
}
}
bool dead_reckoning_valid_but_timouted = false;
if (reporter.failsafeFlags().mode_req_dead_reckoning_invalid
&& !reporter.failsafeFlags().dead_reckoning_invalid) {
if ((reporter.failsafeFlags().mode_req_global_position && !reporter.failsafeFlags().global_position_invalid) ||
(reporter.failsafeFlags().mode_req_global_position_relaxed
&& !reporter.failsafeFlags().global_position_invalid_relaxed) ||
(reporter.failsafeFlags().mode_req_local_position && !reporter.failsafeFlags().local_position_invalid)) {
dead_reckoning_valid_but_timouted = (_param_com_dead_reckoning_tout_t.get() > FLT_EPSILON
&& lpos.dead_reckoning
&& now > _last_initiated_dead_reckoning_time_us + _param_com_dead_reckoning_tout_t.get() * 1_s);
reporter.failsafeFlags().dead_reckoning_invalid = dead_reckoning_valid_but_timouted;
reporter.failsafeFlags().mode_req_dead_reckoning_invalid = !dead_reckoning_valid_but_timouted;
dead_reckoning_valid_but_timouted = (_last_not_dead_reckoning_time_us != 0
&& now > _last_not_dead_reckoning_time_us + _param_com_dead_reckoning_tout_t.get() * 1_s);
}
if (dead_reckoning_valid_but_timouted && _param_com_dead_reckoning_tout_act.get()) {
@@ -703,7 +693,7 @@ void EstimatorChecks::deadReckoningTimeout(const Context &context, Report &repor
}
}
_was_dead_reckoning = lpos.dead_reckoning;
reporter.failsafeFlags().dead_reckoning_invalid = dead_reckoning_valid_but_timouted;
}
void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading,
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
+1 -2
View File
@@ -103,12 +103,11 @@ private:
hrt_abstime _last_lpos_fail_time_us{0}; ///< Last time that the local position validity recovery check failed (usec)
hrt_abstime _last_lpos_relaxed_fail_time_us{0}; ///< Last time that the relaxed local position validity recovery check failed (usec)
hrt_abstime _last_lvel_fail_time_us{0}; ///< Last time that the local velocity validity recovery check failed (usec)
hrt_abstime _last_initiated_dead_reckoning_time_us{0}; ///< Last time that the dead reckoning initiated (usec)
hrt_abstime _last_not_dead_reckoning_time_us{0}; ///< Last time that the dead reckoning initiated (usec)
bool _gps_was_fused{false};
bool _gnss_spoofed{false};
bool _in_air_valid{false};
bool _was_dead_reckoning{false};
bool _nav_failure_imminent_warned{false};
src/modules/commander/ModeUtil/mode_requirements.cpp
-1
View File
@@ -98,7 +98,6 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_global_position_relaxed);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_position_relaxed);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_dead_reckoning_invalid);
} else {
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_global_position);
src/modules/commander/failsafe/failsafe.cpp
+2 -1
View File
@@ -550,7 +550,8 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
}
// trigger dead reckoning Timeout Failsafe (only in auto mission)
if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) {
CHECK_FAILSAFE(status_flags, dead_reckoning_invalid, fromPosLowActParam(_param_com_dead_reckoning_tout_act.get()));
}