Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

Commander: execute pre arm check with preflight checks

Browse Source
This commit is contained in:
Matthias Grob
2022-06-13 13:33:44 +02:00
committed by Daniel Agar
parent aa575d6af0
commit 3b3d8b9942
5 changed files with 55 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp
+6 -3
View File
@@ -68,9 +68,12 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
&& !(status.hil_state == vehicle_status_s::HIL_STATE_ON)
&& (_arm_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE)) {
if (!PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, control_mode, true, true, time_since_boot)
|| !PreFlightCheck::preArmCheck(mavlink_log_pub, status_flags, control_mode, safety_button_available, safety_off,
arm_requirements, status)) {
if (!PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, control_mode,
true, // report_failures
true, // prearm
time_since_boot,
safety_button_available, safety_off,
arm_requirements)) {
feedback_provided = true; // Preflight checks report error messages
valid_transition = false;
}
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
+5 -1
View File
@@ -52,7 +52,9 @@ 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 bool prearm, const hrt_abstime &time_since_boot)
bool report_failures, const bool prearm, const hrt_abstime &time_since_boot,
const bool safety_button_available, const bool safety_off,
const arm_requirements_t &arm_requirements)
{
report_failures = (report_failures && !status_flags.calibration_enabled);
@@ -226,6 +228,8 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
failed = failed || !modeCheck(mavlink_log_pub, report_failures, status);
failed = failed || !cpuResourceCheck(mavlink_log_pub, report_failures);
failed = failed || !parachuteCheck(mavlink_log_pub, report_failures, status_flags);
failed = failed || !preArmCheck(mavlink_log_pub, status_flags, control_mode,
safety_button_available, safety_off, arm_requirements, status, report_failures);
/* Report status */
return !failed;
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp
+15 -14
View File
@@ -56,6 +56,14 @@ public:
PreFlightCheck() = default;
~PreFlightCheck() = default;
struct arm_requirements_t {
bool arm_authorization = false;
bool esc_check = false;
bool global_position = false;
bool mission = false;
bool geofence = false;
};
/**
* Runs a preflight check on all sensors to see if they are properly calibrated and healthy
*
@@ -83,20 +91,9 @@ 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 bool prearm, const hrt_abstime &time_since_boot);
struct arm_requirements_t {
bool arm_authorization = false;
bool esc_check = false;
bool global_position = false;
bool mission = false;
bool geofence = false;
};
static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off,
const arm_requirements_t &arm_requirements, vehicle_status_s &status,
bool report_fail = true);
bool reportFailures, const bool prearm, const hrt_abstime &time_since_boot,
const bool safety_button_available, const bool safety_off,
const arm_requirements_t &arm_requirements);
private:
static bool sensorAvailabilityCheck(const bool report_failure,
@@ -138,4 +135,8 @@ private:
static bool sdcardCheck(orb_advert_t *mavlink_log_pub, bool &sd_card_detected_once, const bool report_fail);
static bool parachuteCheck(orb_advert_t *mavlink_log_pub, const bool report_fail,
const vehicle_status_flags_s &status_flags);
static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off,
const arm_requirements_t &arm_requirements, vehicle_status_s &status,
const bool report_fail);
};
src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp
+1 -2
View File
@@ -40,7 +40,7 @@
bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off,
const arm_requirements_t &arm_requirements, vehicle_status_s &status, bool report_fail)
const arm_requirements_t &arm_requirements, vehicle_status_s &status, const bool report_fail)
{
bool prearm_ok = true;
@@ -227,6 +227,5 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
}
}
return prearm_ok;
}
src/modules/commander/Commander.cpp
+28 -19
View File
@@ -298,16 +298,14 @@ int Commander::custom_command(int argc, char *argv[])
bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, vehicle_status, vehicle_status_flags,
vehicle_control_mode,
true, true, 30_s);
true, // report_failures
true, // prearm
30_s,
false, // safety_buttton_available not known
false, // safety_off not known
PreFlightCheck::arm_requirements_t{});
PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED");
bool dummy_safety_button{false};
bool dummy_safety_off{false};
bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, vehicle_status_flags, vehicle_control_mode,
dummy_safety_button, dummy_safety_off,
PreFlightCheck::arm_requirements_t{}, vehicle_status);
PX4_INFO("Prearm check: %s", prearm_check_res ? "OK" : "FAILED");
print_health_flags(vehicle_status);
return 0;
@@ -856,7 +854,12 @@ 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, true, hrt_elapsed_time(&_boot_timestamp));
false, // report_failures
true, // prearm
hrt_elapsed_time(&_boot_timestamp),
false, // safety_buttton_available not known
false, // safety_off not known,
PreFlightCheck::arm_requirements_t{});
}
Commander::~Commander()
@@ -3004,19 +3007,19 @@ Commander::run()
// Evaluate current prearm status (skip during arm -> disarm transition)
if (!actuator_armed_prev.armed && !_arm_state_machine.isArmed() && !_vehicle_status_flags.calibration_enabled) {
perf_begin(_preflight_check_perf);
bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _vehicle_status, _vehicle_status_flags,
_vehicle_control_mode,
false, true, hrt_elapsed_time(&_boot_timestamp));
perf_end(_preflight_check_perf);
// skip arm authorization check until actual arming attempt
PreFlightCheck::arm_requirements_t arm_req = _arm_requirements;
arm_req.arm_authorization = false;
bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, _vehicle_status_flags, _vehicle_control_mode,
_safety.isButtonAvailable(), _safety.isSafetyOff(),
arm_req, _vehicle_status, false);
_vehicle_status_flags.pre_flight_checks_pass = PreFlightCheck::preflightCheck(nullptr, _vehicle_status,
_vehicle_status_flags,
_vehicle_control_mode,
false, // report_failures
true, // prearm
hrt_elapsed_time(&_boot_timestamp),
_safety.isButtonAvailable(), _safety.isSafetyOff(),
arm_req);
perf_end(_preflight_check_perf);
_vehicle_status_flags.pre_flight_checks_pass = preflight_check_res && prearm_check_res;
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true,
_vehicle_status_flags.pre_flight_checks_pass, _vehicle_status);
}
@@ -3638,8 +3641,14 @@ void Commander::data_link_check()
if (!_arm_state_machine.isArmed() && !_vehicle_status_flags.calibration_enabled) {
// make sure to report preflight check failures to a connecting GCS
// skip arm authorization check until actual arming attempt
PreFlightCheck::arm_requirements_t arm_req = _arm_requirements;
arm_req.arm_authorization = false;
PreFlightCheck::preflightCheck(&_mavlink_log_pub, _vehicle_status, _vehicle_status_flags, _vehicle_control_mode,
true, false, hrt_elapsed_time(&_boot_timestamp));
true, // report_failures
false, // prearm
hrt_elapsed_time(&_boot_timestamp),
_safety.isButtonAvailable(), _safety.isSafetyOff(), arm_req);
}
}