diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index 35d3e6dab8..6201190f74 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -14,7 +14,7 @@ bool condition_local_velocity_valid # set to true by the commander app if the q bool condition_local_altitude_valid bool condition_power_input_valid # set if input power is valid bool condition_battery_healthy # set if battery is available and not low - +bool condition_escs_error # set to true in case of standard ESCs that do not provide status OR if all the ESCs are online bool circuit_breaker_engaged_power_check bool circuit_breaker_engaged_airspd_check bool circuit_breaker_engaged_enginefailure_check @@ -22,6 +22,7 @@ bool circuit_breaker_engaged_gpsfailure_check bool circuit_breaker_flight_termination_disabled bool circuit_breaker_engaged_usb_check bool circuit_breaker_engaged_posfailure_check # set to true when the position valid checks have been disabled +bool circuit_breaker_engaged_escs_check bool offboard_control_signal_found_once bool offboard_control_signal_lost @@ -36,5 +37,3 @@ bool usb_connected # status of the USB power supp bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter bool avoidance_system_valid # Status of the obstacle avoidance system - -uint8 number_of_actuators # Number of active actuators diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 8b67b5836a..976a0ee2de 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -95,6 +95,7 @@ #include #include #include +#include typedef enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ @@ -149,6 +150,7 @@ static uint8_t _last_sp_man_arm_switch = 0; static struct vtol_vehicle_status_s vtol_status = {}; static struct cpuload_s cpuload = {}; +static struct esc_status_s esc_status = {}; static bool last_overload = false; @@ -1198,6 +1200,7 @@ Commander::run() param_t _param_arm_switch_is_button = param_find("COM_ARM_SWISBTN"); param_t _param_rc_override = param_find("COM_RC_OVERRIDE"); param_t _param_arm_mission_required = param_find("COM_ARM_MIS_REQ"); + param_t _param_escs_checks_required = param_find("COM_ARM_CHK_ESCS"); param_t _param_flight_uuid = param_find("COM_FLIGHT_UUID"); param_t _param_takeoff_finished_action = param_find("COM_TAKEOFF_ACT"); @@ -1243,6 +1246,7 @@ Commander::run() PX4_ERR("Failed to register power notification callback"); } + get_circuit_breaker_params(); /* armed topic */ @@ -1275,6 +1279,7 @@ Commander::run() uORB::Subscription subsys_sub{ORB_ID(subsystem_info)}; uORB::Subscription system_power_sub{ORB_ID(system_power)}; uORB::Subscription vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; + uORB::Subscription esc_status_sub{ORB_ID(esc_status)}; geofence_result_s geofence_result {}; @@ -1329,6 +1334,10 @@ Commander::run() param_get(_param_arm_mission_required, &arm_mission_required_param); arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT)); + int32_t arm_escs_checks_required_param = 0; + param_get(_param_escs_checks_required, &arm_escs_checks_required_param); + arm_requirements |= (arm_escs_checks_required_param == 0) ? ARM_REQ_NONE : ARM_REQ_ESCS_CHECK_BIT; + status.rc_input_mode = rc_in_off; // user adjustable duration required to assert arm/disarm via throttle/rudder stick @@ -1458,6 +1467,9 @@ Commander::run() arm_requirements = (arm_without_gps_param == 1) ? ARM_REQ_NONE : ARM_REQ_GPS_BIT; param_get(_param_arm_mission_required, &arm_mission_required_param); arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT)); + param_get(_param_escs_checks_required, &arm_escs_checks_required_param); + arm_requirements |= (arm_escs_checks_required_param == 0) ? ARM_REQ_NONE : ARM_REQ_ESCS_CHECK_BIT; + /* flight mode slots */ param_get(_param_fmode_1, &_flight_mode_slots[0]); @@ -1610,6 +1622,19 @@ Commander::run() } } + if (orb_exists(ORB_ID(esc_status), 0) == PX4_OK) { + + if (esc_status_sub.updated()) { + /* ESCs status changed */ + esc_status_sub.copy(&esc_status); + esc_status_check(); + } + + } else { + status_flags.condition_escs_error = false; + } + + estimator_check(&status_changed); airspeed_use_check(); @@ -4347,3 +4372,37 @@ Commander::offboard_control_update(bool &status_changed) } } + +void Commander::esc_status_check() +{ + char esc_fail_msg[50]; + esc_fail_msg[0] = '\0'; + + int online_bitmask = (1 << (esc_status.esc_count)) - 1; + + // Check if ALL the ESCs are online + if (online_bitmask == esc_status.esc_online_flags) { + status_flags.condition_escs_error = false; + _last_esc_online_flags = esc_status.esc_online_flags; + } + + // Avoid checking the status if the flags are the same or if the mixer has not yet been loaded from uavcan_main + else if (_last_esc_online_flags == esc_status.esc_online_flags || esc_status.esc_count == 0) { + status_flags.condition_escs_error = true; + } + + // Only warn the user when an ESC goes from ONLINE to OFFLINE. This is done to prevent showing Offline ESCs warning messages at boot + else if (esc_status.esc_online_flags < _last_esc_online_flags) { + + for (int index = 0; index < esc_status.esc_count; index++) { + if ((bool)!(esc_status.esc_online_flags & (1 << index))) { + snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", index + 1); + } + } + + mavlink_log_critical(&mavlink_log_pub, "%soffline", esc_fail_msg); + + _last_esc_online_flags = esc_status.esc_online_flags; + status_flags.condition_escs_error = true; + } +} diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index ffe7db5738..f1d9bbf954 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -217,6 +217,8 @@ private: void battery_status_check(); + void esc_status_check(); + /** * Checks the status of all available data links and handles switching between different system telemetry states. */ @@ -240,6 +242,8 @@ private: hrt_abstime _high_latency_datalink_heartbeat{0}; hrt_abstime _high_latency_datalink_lost{0}; + int _last_esc_online_flags{-1}; + uORB::Subscription _battery_sub{ORB_ID(battery_status)}; uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE}; float _battery_current{0.0f}; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f1886b3c47..e530b317fb 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -926,3 +926,13 @@ PARAM_DEFINE_INT32(COM_ASPD_FS_DLY, 0); * @group Commander */ PARAM_DEFINE_INT32(COM_FLT_PROFILE, 0); + +/** + * Require all the ESCs to be detected to arm. + * + * This param is specific for ESCs reporting status. Normal ESCs configurations are not affected by the change of this param. + * + * @group Commander + * @boolean + */ +PARAM_DEFINE_INT32(COM_ARM_CHK_ESCS, 1); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 0d0f31026d..e8b424111b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -1010,6 +1010,14 @@ bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &s } + if (status_flags.condition_escs_error && (arm_requirements & ARM_REQ_ESCS_CHECK_BIT)) { + if (prearm_ok && reportFailures) { + mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs are offline"); + prearm_ok = false; + } + } + + return prearm_ok; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e6dc84c80e..4d238fb4b4 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -51,6 +51,7 @@ #include #include #include +#include typedef enum { TRANSITION_DENIED = -1, @@ -73,6 +74,7 @@ typedef enum { ARM_REQ_MISSION_BIT = (1 << 0), ARM_REQ_ARM_AUTH_BIT = (1 << 1), ARM_REQ_GPS_BIT = (1 << 2), + ARM_REQ_ESCS_CHECK_BIT = (1 << 3) } arm_requirements_t; extern const char *const arming_state_names[];