Commander: added esc_status prearm checks

Signed-off-by: Claudio Micheli <claudio@auterion.com>

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

src/modules/commander/Commander.cpp

@@ -95,6 +95,7 @@
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vtol_vehicle_status.h>
+#include <uORB/topics/esc_status.h>
 
 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;
+	}
+}

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};

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);

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;
 }
 

src/modules/commander/state_machine_helper.h

@@ -51,6 +51,7 @@
 #include <uORB/topics/safety.h>
 #include <uORB/topics/commander_state.h>
 #include <uORB/topics/vehicle_status_flags.h>
+#include <uORB/topics/esc_status.h>
 
 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[];