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

Commander: run ekf checks without grace period after boot

Browse Source
This commit is contained in:
Matthias Grob
2022-07-05 10:35:07 -04:00
committed by Daniel Agar
parent 8deebd07b8
commit a3288ff732
7 changed files with 12 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp
+1 -2
View File
@@ -42,7 +42,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off,
const arming_state_t new_arming_state, actuator_armed_s &armed, const bool fRunPreArmChecks,
orb_advert_t *mavlink_log_pub, vehicle_status_flags_s &status_flags,
const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason)
arm_disarm_reason_t calling_reason)
{
// Double check that our static arrays are still valid
static_assert(vehicle_status_s::ARMING_STATE_INIT == 0, "ARMING_STATE_INIT == 0");
@@ -69,7 +69,6 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
if (!PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, control_mode,
true, // report_failures
time_since_boot,
safety_button_available, safety_off,
true)) { // is_arm_attempt
feedback_provided = true; // Preflight checks report error messages
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp
+1 -1
View File
@@ -58,7 +58,7 @@ public:
const bool safety_button_available, const bool safety_off, const arming_state_t new_arming_state,
actuator_armed_s &armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub,
vehicle_status_flags_s &status_flags,
const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason);
arm_disarm_reason_t calling_reason);
// Getters
uint8_t getArmState() const { return _arm_state; }
src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp
-1
View File
@@ -269,7 +269,6 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
true /* enable pre-arm checks */,
nullptr /* no mavlink_log_pub */,
status_flags,
2e6, /* 2 seconds after boot, everything should be checked */
arm_disarm_reason_t::unit_test);
// Validate result of transition
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
+4 -17
View File
@@ -52,7 +52,7 @@ static constexpr unsigned max_mandatory_baro_count = 1;
bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode,
bool report_failures, const hrt_abstime &time_since_boot,
bool report_failures,
const bool safety_button_available, const bool safety_off,
const bool is_arm_attempt)
{
@@ -193,23 +193,10 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
}
if (estimator_type == 2) {
const bool ekf_healthy = ekf2Check(mavlink_log_pub, status, report_failures) &&
ekf2CheckSensorBias(mavlink_log_pub, report_failures);
const bool in_grace_period = time_since_boot < 10_s;
const bool do_report_ekf2_failures = report_failures && (!in_grace_period);
const bool ekf_healthy = ekf2Check(mavlink_log_pub, status, false, do_report_ekf2_failures) &&
ekf2CheckSensorBias(mavlink_log_pub, do_report_ekf2_failures);
// For the first 10 seconds the ekf2 can be unhealthy, and we just mark it
// as not present.
// After that or if we're forced to report, we'll set the flags as is.
if (!ekf_healthy && !do_report_ekf2_failures) {
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, false, false, status);
} else {
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, true, ekf_healthy, status);
}
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, true, ekf_healthy, status);
if (control_mode.flag_control_attitude_enabled
|| control_mode.flag_control_velocity_enabled
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp
+2 -3
View File
@@ -64,7 +64,7 @@ public:
**/
static bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode,
bool reportFailures, const hrt_abstime &time_since_boot,
bool reportFailures,
const bool safety_button_available, const bool safety_off,
const bool is_arm_attempt = false);
@@ -93,8 +93,7 @@ private:
const float arming_max_airspeed_allowed);
static int rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail);
static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail);
static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
const bool report_fail);
static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool report_fail);
static bool ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bool report_fail);
static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail);
static bool manualControlCheck(orb_advert_t *mavlink_log_pub, const bool report_fail);
src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp
+1 -2
View File
@@ -44,8 +44,7 @@
using namespace time_literals;
bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
const bool report_fail)
bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool report_fail)
{
bool success = true; // start with a pass and change to a fail if any test fails