diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index c36e569384..b87f3f027b 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -83,6 +83,9 @@ public: void enable_hil(); + // TODO: only temporarily static until low priority thread is removed + static bool preflight_check(bool report); + private: bool handle_command(vehicle_status_s *status_local, const vehicle_command_s &cmd, actuator_armed_s *armed_local, home_position_s *home, const vehicle_global_position_s &global_pos, diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index b29a81286e..fea703871e 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -67,6 +67,7 @@ #include #include #include +#include #include #include "PreflightCheck.h" @@ -448,6 +449,54 @@ out: return success; } +static bool powerCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool prearm) +{ + bool success = true; + + if (!prearm) { + // Ignore power check after arming. + return true; + + } else { + int system_power_sub = orb_subscribe(ORB_ID(system_power)); + + system_power_s system_power; + + if (orb_copy(ORB_ID(system_power), system_power_sub, &system_power) == PX4_OK) { + + if (hrt_elapsed_time(&system_power.timestamp) < 200000) { + + /* copy avionics voltage */ + int avionics_power_rail_voltage = system_power.voltage5V_v; + + // avionics rail + // Check avionics rail voltages + if (avionics_power_rail_voltage < 4.5f) { + success = false; + + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: Avionics power low: %6.2f Volt", (double)avionics_power_rail_voltage); + } + + } else if (avionics_power_rail_voltage < 4.9f) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics power low: %6.2f Volt", (double)avionics_power_rail_voltage); + } + + } else if (avionics_power_rail_voltage > 5.4f) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics power high: %6.2f Volt", (double)avionics_power_rail_voltage); + } + } + } + } + + orb_unsubscribe(system_power_sub); + } + + return success; +} + static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool enforce_gps_required) { bool success = true; // start with a pass and change to a fail if any test fails @@ -461,7 +510,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_ goto out; } - // Check if preflight check perfomred by estimator has failed + // Check if preflight check performed by estimator has failed if (status.pre_flt_fail) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF INTERNAL CHECKS"); @@ -570,7 +619,7 @@ out: } bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool checkAirspeed, bool checkRC, bool checkGNSS, - bool checkDynamic, bool isVTOL, bool reportFailures, bool prearm, const hrt_abstime& time_since_boot) + bool checkDynamic, bool checkPower, bool isVTOL, bool reportFailures, bool prearm, const hrt_abstime &time_since_boot) { if (time_since_boot < 2000000) { @@ -622,7 +671,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check /* check if the primary device is present */ if (!prime_found && prime_id != 0) { if ((reportFailures && !failed)) { - mavlink_log_critical(mavlink_log_pub, "Warning: Primary compass not found"); + mavlink_log_critical(mavlink_log_pub, "Primary compass not found"); } failed = true; } @@ -660,7 +709,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check /* check if the primary device is present */ if (!prime_found && prime_id != 0) { if ((reportFailures && !failed)) { - mavlink_log_critical(mavlink_log_pub, "Warning: Primary accelerometer not found"); + mavlink_log_critical(mavlink_log_pub, "Primary accelerometer not found"); } failed = true; } @@ -692,7 +741,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check /* check if the primary device is present */ if (!prime_found && prime_id != 0) { if ((reportFailures && !failed)) { - mavlink_log_critical(mavlink_log_pub, "Warning: Primary gyro not found"); + mavlink_log_critical(mavlink_log_pub, "Primary gyro not found"); } failed = true; } @@ -725,7 +774,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check // // check if the primary device is present if (!prime_found && prime_id != 0) { if (reportFailures && !failed) { - mavlink_log_critical(mavlink_log_pub, "warning: primary barometer not operational"); + mavlink_log_critical(mavlink_log_pub, "Primary barometer not operational"); } failed = true; } @@ -751,6 +800,14 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check if (reportFailures) { mavlink_log_critical(mavlink_log_pub, "RC calibration check failed"); } + + failed = true; + } + } + + /* ---- SYSTEM POWER ---- */ + if (checkPower) { + if (!powerCheck(mavlink_log_pub, (reportFailures && !failed), prearm)) { failed = true; } } diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index 98a94b71e1..e84f17bb0c 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -67,9 +67,11 @@ namespace Preflight * true if the Remote Controller should be checked * @param checkGNSS * true if the GNSS receiver should be checked +* @param checkPower +* true if the system power should be checked **/ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool checkAirspeed, bool checkRC, bool checkGNSS, - bool checkDynamic, bool isVTOL, bool reportFailures, bool prearm, const hrt_abstime& time_since_boot); + bool checkDynamic, bool checkPower, bool isVTOL, bool reportFailures, bool prearm, const hrt_abstime &time_since_boot); static constexpr unsigned max_mandatory_gyro_count = 1; static constexpr unsigned max_optional_gyro_count = 3; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cc3a3e7dac..78166c4bf7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -135,7 +135,7 @@ typedef enum VEHICLE_MODE_FLAG #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 -#define HOTPLUG_SENS_TIMEOUT (8 * 1000 * 1000) /**< wait for hotplug sensors to come online for upto 8 seconds */ +static constexpr uint64_t HOTPLUG_SENS_TIMEOUT = (8 * 1000 * 1000); /**< wait for hotplug sensors to come online for upto 8 seconds */ #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 500000 @@ -214,8 +214,6 @@ static struct vehicle_status_flags_s status_flags = {}; static uint64_t rc_signal_lost_timestamp; // Time at which the RC reception was lost -static float avionics_power_rail_voltage; // voltage of the avionics power rail - static uint8_t arm_requirements = ARM_REQ_NONE; static bool _last_condition_global_position_valid = false; @@ -389,8 +387,11 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "check")) { - bool checkres = prearm_check(&mavlink_log_pub, status_flags, battery, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); - PX4_INFO("Prearm check: %s", checkres ? "OK" : "FAILED"); + bool preflight_check_res = Commander::preflight_check(true); + PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED"); + + bool prearm_check_res = prearm_check(&mavlink_log_pub, status_flags, battery, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); + PX4_INFO("Prearm check: %s", prearm_check_res ? "OK" : "FAILED"); return 0; } @@ -580,7 +581,6 @@ void print_status() warnx("type: %s", (status.is_rotary_wing) ? "symmetric motion" : "forward motion"); warnx("safety: USB enabled: %s, power state valid: %s", (status_flags.usb_connected) ? "[OK]" : "[NO]", (status_flags.condition_power_input_valid) ? " [OK]" : "[NO]"); - warnx("avionics rail: %6.2f V", (double)avionics_power_rail_voltage); warnx("home: lat = %.7f, lon = %.7f, alt = %.2f, yaw: %.2f", _home.lat, _home.lon, (double)_home.alt, (double)_home.yaw); warnx("home: x = %.7f, y = %.7f, z = %.2f ", (double)_home.x, (double)_home.y, (double)_home.z); warnx("datalink: %s %s", (status.data_link_lost) ? "LOST" : "OK", (status.high_latency_data_link_active) ? "(high latency)" : " "); @@ -605,7 +605,6 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co true /* fRunPreArmChecks */, mavlink_log_pub_local, &status_flags, - avionics_power_rail_voltage, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); @@ -1272,7 +1271,6 @@ Commander::run() status.timestamp = hrt_absolute_time(); status_flags.condition_power_input_valid = true; - avionics_power_rail_voltage = -1.0f; status_flags.usb_connected = false; // CIRCUIT BREAKERS @@ -1384,8 +1382,6 @@ Commander::run() /* Subscribe to system power */ int system_power_sub = orb_subscribe(ORB_ID(system_power)); - struct system_power_s system_power; - memset(&system_power, 0, sizeof(system_power)); /* Subscribe to actuator controls (outputs) */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); @@ -1432,14 +1428,6 @@ Commander::run() status.is_rotary_wing = is_rotary_wing(&status) || is_vtol(&status); status.is_vtol = is_vtol(&status); - bool checkAirspeed = false; - /* Perform airspeed check only if circuit breaker is not - * engaged and it's not a rotary wing */ - if (!status_flags.circuit_breaker_engaged_airspd_check && - (!status.is_rotary_wing || status.is_vtol)) { - checkAirspeed = true; - } - commander_boot_timestamp = hrt_absolute_time(); // initially set to failed @@ -1449,7 +1437,6 @@ Commander::run() // Run preflight check int32_t rc_in_off = 0; - bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; param_get(_param_rc_in_off, &rc_in_off); @@ -1465,15 +1452,6 @@ Commander::run() arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT)); status.rc_input_mode = rc_in_off; - if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { - // HIL configuration selected: real sensors will be disabled - status_flags.condition_system_sensors_initialized = false; - } else { - // sensor diagnostics done continuously, not just at boot so don't warn about any issues just yet - status_flags.condition_system_sensors_initialized = Preflight::preflightCheck(&mavlink_log_pub, true, - checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status_flags.circuit_breaker_engaged_gpsfailure_check, - false, is_vtol(&status), false, false, hrt_elapsed_time(&commander_boot_timestamp)); - } // user adjustable duration required to assert arm/disarm via throttle/rudder stick int32_t rc_arm_hyst = 100; @@ -1719,9 +1697,6 @@ Commander::run() status_flags.condition_power_input_valid = true; } - /* copy avionics voltage */ - avionics_power_rail_voltage = system_power.voltage5V_v; - /* if the USB hardware connection went away, reboot */ if (status_flags.usb_connected && !system_power.usb_connected) { /* @@ -1747,7 +1722,7 @@ Commander::run() && safety.safety_switch_available && !safety.safety_off) { if (TRANSITION_CHANGED == arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, &mavlink_log_pub, - &status_flags, avionics_power_rail_voltage, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)) + &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)) ) { status_changed = true; } @@ -2108,7 +2083,7 @@ Commander::run() if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { arming_ret = arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, - true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, + true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); if (arming_ret == TRANSITION_DENIED) { @@ -2330,7 +2305,7 @@ Commander::run() } else if ((stick_off_counter == rc_arm_hyst && stick_on_counter < rc_arm_hyst) || arm_switch_to_disarm_transition) { arming_ret = arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, - &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); + &mavlink_log_pub, &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); } stick_off_counter++; /* do not reset the counter when holding the arm button longer than needed */ @@ -2371,7 +2346,7 @@ Commander::run() } else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { arming_ret = arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, - &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); + &mavlink_log_pub, &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); if (arming_ret != TRANSITION_CHANGED) { usleep(100000); @@ -2758,20 +2733,14 @@ Commander::run() } /* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */ - hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + status_flags.condition_system_hotplug_timeout = (hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT); - if (!sensor_fail_tune_played && (!status_flags.condition_system_sensors_initialized && hotplug_timeout)) { + if (!sensor_fail_tune_played && (!status_flags.condition_system_sensors_initialized && status_flags.condition_system_hotplug_timeout)) { set_tune_override(TONE_GPS_WARNING_TUNE); sensor_fail_tune_played = true; status_changed = true; } - /* update timeout flag */ - if (!(hotplug_timeout == status_flags.condition_system_hotplug_timeout)) { - status_flags.condition_system_hotplug_timeout = hotplug_timeout; - status_changed = true; - } - counter++; int blink_state = blink_msg_state(); @@ -2872,7 +2841,6 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu uint8_t led_mode = led_control_s::MODE_OFF; uint8_t led_color = led_control_s::COLOR_WHITE; bool set_normal_color = false; - bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; int overload_warn_delay = (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 1000 : 250000; @@ -2885,7 +2853,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu led_mode = led_control_s::MODE_ON; set_normal_color = true; - } else if (!status_flags.condition_system_sensors_initialized && hotplug_timeout) { + } else if (!status_flags.condition_system_sensors_initialized && status_flags.condition_system_hotplug_timeout) { led_mode = led_control_s::MODE_BLINK_FAST; led_color = led_control_s::COLOR_RED; @@ -2893,7 +2861,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu led_mode = led_control_s::MODE_BREATHE; set_normal_color = true; - } else if (!status_flags.condition_system_sensors_initialized && !hotplug_timeout) { + } else if (!status_flags.condition_system_sensors_initialized && !status_flags.condition_system_hotplug_timeout) { led_mode = led_control_s::MODE_BREATHE; set_normal_color = true; @@ -3812,7 +3780,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ if (TRANSITION_DENIED == arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_INIT, &armed, - false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, + false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp))) { answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); @@ -3892,26 +3860,10 @@ void *commander_low_prio_loop(void *arg) if (calib_ret == OK) { tune_positive(true); - // Update preflight check status - // we do not set the calibration return value based on it because the calibration - // might have worked just fine, but the preflight check fails for a different reason, - // so this would be prone to false negatives. - - bool checkAirspeed = false; - bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; - /* Perform airspeed check only if circuit breaker is not - * engaged and it's not a rotary wing */ - if (!status_flags.circuit_breaker_engaged_airspd_check && - (!status.is_rotary_wing || status.is_vtol)) { - checkAirspeed = true; - } - - status_flags.condition_system_sensors_initialized = Preflight::preflightCheck(&mavlink_log_pub, true, checkAirspeed, - !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), arm_requirements & ARM_REQ_GPS_BIT, - true, is_vtol(&status), hotplug_timeout, false, hrt_elapsed_time(&commander_boot_timestamp)); + Commander::preflight_check(false); arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, - &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); + &mavlink_log_pub, &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); } else { tune_negative(true); @@ -4078,6 +4030,34 @@ void Commander::mission_init() } } +bool Commander::preflight_check(bool report) +{ + const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON); + + const bool checkSensors = !hil_enabled; + const bool checkRC = (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT); + const bool checkGNSS = (arm_requirements & ARM_REQ_GPS_BIT); + const bool checkDynamic = !hil_enabled; + const bool checkPower = (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check); + + const bool reportFailures = (report && status_flags.condition_system_hotplug_timeout); + + bool checkAirspeed = false; + + /* Perform airspeed check only if circuit breaker is not + * engaged and it's not a rotary wing */ + if (!status_flags.circuit_breaker_engaged_airspd_check && (!status.is_rotary_wing || status.is_vtol)) { + checkAirspeed = true; + } + + bool success = Preflight::preflightCheck(&mavlink_log_pub, checkSensors, checkAirspeed, checkRC, checkGNSS, checkDynamic, checkPower, + status.is_vtol, reportFailures, false, hrt_elapsed_time(&commander_boot_timestamp)); + + status_flags.condition_system_sensors_initialized = success; + + return success; +} + void Commander::poll_telemetry_status(bool checkAirspeed, bool *hotplug_timeout) { bool updated = false; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 5f28465881..8f13d812c1 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -265,7 +265,6 @@ bool StateMachineHelperTest::armingStateTransitionTest() false /* no pre-arm checks */, nullptr /* no mavlink_log_pub */, &status_flags, - 5.0f, /* avionics rail voltage */ (check_gps ? ARM_REQ_GPS_BIT : 0), 2e6 /* 2 seconds after boot, everything should be checked */ ); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index b0ba62c3b3..9236be2dd4 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -94,8 +94,8 @@ void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, c transition_result_t arming_state_transition(vehicle_status_s *status, const battery_status_s &battery, const safety_s &safety, 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 float avionics_power_rail_voltage, - const uint8_t arm_requirements, const hrt_abstime &time_since_boot) + orb_advert_t *mavlink_log_pub, vehicle_status_flags_s *status_flags, const uint8_t arm_requirements, + const hrt_abstime &time_since_boot) { // Double check that our static arrays are still valid static_assert(vehicle_status_s::ARMING_STATE_INIT == 0, "ARMING_STATE_INIT == 0"); @@ -120,8 +120,14 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt bool preflight_check_ret = false; bool prearm_check_ret = false; + const bool checkSensors = !hil_enabled; + const bool checkRC = (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT); + const bool checkGNSS = (arm_requirements & ARM_REQ_GPS_BIT); + const bool checkDynamic = !hil_enabled; + const bool checkPower = (status_flags->condition_power_input_valid + && !status_flags->circuit_breaker_engaged_power_check); + bool checkAirspeed = false; - bool sensor_checks = !hil_enabled; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ @@ -133,9 +139,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED && !hil_enabled) { - preflight_check_ret = Preflight::preflightCheck(mavlink_log_pub, sensor_checks, checkAirspeed, - (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), arm_requirements & ARM_REQ_GPS_BIT, true, - status->is_vtol, true, true, time_since_boot); + preflight_check_ret = Preflight::preflightCheck(mavlink_log_pub, checkSensors, checkAirspeed, checkRC, checkGNSS, + checkDynamic, checkPower, status->is_vtol, true, true, time_since_boot); if (preflight_check_ret) { status_flags->condition_system_sensors_initialized = true; @@ -155,10 +160,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) { - status_flags->condition_system_sensors_initialized = Preflight::preflightCheck(mavlink_log_pub, sensor_checks, - checkAirspeed, - (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), arm_requirements & ARM_REQ_GPS_BIT, true, - status->is_vtol, false, false, time_since_boot); + status_flags->condition_system_sensors_initialized = Preflight::preflightCheck(mavlink_log_pub, checkSensors, + checkAirspeed, checkRC, checkGNSS, checkDynamic, checkPower, status->is_vtol, false, false, time_since_boot); last_preflight_check = hrt_absolute_time(); } @@ -212,31 +215,6 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt feedback_provided = true; valid_transition = false; } - - // Perform power checks only if circuit breaker is not - // engaged for these checks - if (!status_flags->circuit_breaker_engaged_power_check) { - - // Fail transition if power levels on the avionics rail - // are measured but are insufficient - if (status_flags->condition_power_input_valid && (avionics_power_rail_voltage > 0.0f)) { - // Check avionics rail voltages - if (avionics_power_rail_voltage < 4.5f) { - mavlink_log_critical(mavlink_log_pub, "NOT ARMING: Avionics power low: %6.2f Volt", - (double)avionics_power_rail_voltage); - feedback_provided = true; - valid_transition = false; - - } else if (avionics_power_rail_voltage < 4.9f) { - mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics power low: %6.2f Volt", (double)avionics_power_rail_voltage); - feedback_provided = true; - - } else if (avionics_power_rail_voltage > 5.4f) { - mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics power high: %6.2f Volt", (double)avionics_power_rail_voltage); - feedback_provided = true; - } - } - } } } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 8f77e69fd1..799242234b 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -81,7 +81,7 @@ bool is_safe(const safety_s &safety, const actuator_armed_s &armed); transition_result_t arming_state_transition(vehicle_status_s *status, const battery_status_s &battery, const safety_s &safety, 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 float avionics_power_rail_voltage, + orb_advert_t *mavlink_log_pub, vehicle_status_flags_s *status_flags, const uint8_t arm_requirements, const hrt_abstime &time_since_boot); transition_result_t