forked from Archive/PX4-Autopilot
Commander: run ekf checks without grace period after boot
This commit is contained in:
parent
8deebd07b8
commit
a3288ff732
|
@ -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
|
||||
|
|
|
@ -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; }
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -299,7 +299,6 @@ int Commander::custom_command(int argc, char *argv[])
|
|||
bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, vehicle_status, vehicle_status_flags,
|
||||
vehicle_control_mode,
|
||||
true, // report_failures
|
||||
30_s,
|
||||
false, // safety_buttton_available not known
|
||||
false); // safety_off not known
|
||||
PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED");
|
||||
|
@ -496,7 +495,7 @@ bool Commander::shutdown_if_allowed()
|
|||
_safety.isButtonAvailable(), _safety.isSafetyOff(),
|
||||
vehicle_status_s::ARMING_STATE_SHUTDOWN,
|
||||
_actuator_armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags,
|
||||
hrt_elapsed_time(&_boot_timestamp), arm_disarm_reason_t::shutdown);
|
||||
arm_disarm_reason_t::shutdown);
|
||||
}
|
||||
|
||||
static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason)
|
||||
|
@ -748,7 +747,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
|
|||
transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode,
|
||||
_safety.isButtonAvailable(), _safety.isSafetyOff(),
|
||||
vehicle_status_s::ARMING_STATE_ARMED, _actuator_armed, run_preflight_checks,
|
||||
&_mavlink_log_pub, _vehicle_status_flags, hrt_elapsed_time(&_boot_timestamp),
|
||||
&_mavlink_log_pub, _vehicle_status_flags,
|
||||
calling_reason);
|
||||
|
||||
if (arming_res == TRANSITION_CHANGED) {
|
||||
|
@ -793,7 +792,7 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
|
|||
_safety.isButtonAvailable(), _safety.isSafetyOff(),
|
||||
vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, false,
|
||||
&_mavlink_log_pub, _vehicle_status_flags,
|
||||
hrt_elapsed_time(&_boot_timestamp), calling_reason);
|
||||
calling_reason);
|
||||
|
||||
if (arming_res == TRANSITION_CHANGED) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s\t", arm_disarm_reason_str(calling_reason));
|
||||
|
@ -853,7 +852,6 @@ Commander::Commander() :
|
|||
// run preflight immediately to find all relevant parameters, but don't report
|
||||
PreFlightCheck::preflightCheck(&_mavlink_log_pub, _vehicle_status, _vehicle_status_flags, _vehicle_control_mode,
|
||||
false, // report_failures
|
||||
hrt_elapsed_time(&_boot_timestamp),
|
||||
false, // safety_buttton_available not known
|
||||
false); // safety_off not known
|
||||
}
|
||||
|
@ -1408,7 +1406,6 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
|||
_safety.isButtonAvailable(), _safety.isSafetyOff(),
|
||||
vehicle_status_s::ARMING_STATE_INIT, _actuator_armed,
|
||||
false /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags,
|
||||
30_s, // time since boot not relevant for switching to ARMING_STATE_INIT
|
||||
(cmd.from_external ? arm_disarm_reason_t::command_external : arm_disarm_reason_t::command_internal))
|
||||
) {
|
||||
|
||||
|
@ -2459,7 +2456,6 @@ Commander::run()
|
|||
_safety.isButtonAvailable(), _safety.isSafetyOff(),
|
||||
vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed,
|
||||
true /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags,
|
||||
hrt_elapsed_time(&_boot_timestamp),
|
||||
arm_disarm_reason_t::transition_to_standby);
|
||||
}
|
||||
|
||||
|
@ -3000,7 +2996,6 @@ Commander::run()
|
|||
_vehicle_status_flags,
|
||||
_vehicle_control_mode,
|
||||
false, // report_failures
|
||||
hrt_elapsed_time(&_boot_timestamp),
|
||||
_safety.isButtonAvailable(), _safety.isSafetyOff());
|
||||
perf_end(_preflight_check_perf);
|
||||
|
||||
|
@ -3630,7 +3625,6 @@ void Commander::data_link_check()
|
|||
// make sure to report preflight check failures to a connecting GCS
|
||||
PreFlightCheck::preflightCheck(&_mavlink_log_pub, _vehicle_status, _vehicle_status_flags, _vehicle_control_mode,
|
||||
true, // report_failures
|
||||
hrt_elapsed_time(&_boot_timestamp),
|
||||
_safety.isButtonAvailable(), _safety.isSafetyOff());
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue