4 files changed, 95 insertions, 4 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 0ada8e978..b9a0bd2cb 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -686,6 +686,9 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T");
 	param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T");
 	param_t _param_onboard_sysid = param_find("COM_ONBSYSID");
+	param_t _param_ef_throttle_thres = param_find("COM_EF_THROT");
+	param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T");
+	param_t _param_ef_time_thres = param_find("COM_EF_TIME");
 
 	/* welcome user */
 	warnx("starting");
@@ -974,8 +977,16 @@ int commander_thread_main(int argc, char *argv[])
 	int32_t datalink_loss_enabled = false;
 	int32_t datalink_loss_timeout = 10;
 	int32_t datalink_regain_timeout = 0;
-	uint8_t onboard_sysid = 42;		/**< systemid of the onboard computer, telemetry from this sysid
-						  is not validated for the datalink loss check */
+	int32_t onboard_sysid = 42;		/**< systemid of the onboard computer,
+									  telemetry from this sysid is not
+									  validated for the datalink loss check */
+
+	/* Thresholds for engine failure detection */
+	int32_t ef_throttle_thres = 1.0f;
+	int32_t ef_current2throttle_thres = 0.0f;
+	int32_t ef_time_thres = 1000.0f;
+	uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine
+											 was healty*/
 
 	/* check which state machines for changes, clear "changed" flag */
 	bool arming_state_changed = false;
@@ -1039,6 +1050,9 @@ int commander_thread_main(int argc, char *argv[])
 			param_get(_param_datalink_loss_timeout, &datalink_loss_timeout);
 			param_get(_param_datalink_regain_timeout, &datalink_regain_timeout);
 			param_get(_param_onboard_sysid, &onboard_sysid);
+			param_get(_param_ef_throttle_thres, &ef_throttle_thres);
+			param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
+			param_get(_param_ef_time_thres, &ef_time_thres);
 		}
 
 		orb_check(sp_man_sub, &updated);
@@ -1576,10 +1590,11 @@ int commander_thread_main(int argc, char *argv[])
 
 					/* If this is not an onboard link/onboard computer:
 					 * set flag that we have a valid link */
-					if (telemetry_sysid[i] != onboard_sysid) {
+					if (telemetry_sysid[i] != (uint8_t)onboard_sysid) {
 						have_link = true;
 					}
-				} else if (!telemetry_lost[i] && telemetry_sysid[i] != onboard_sysid) {
+				} else if (!telemetry_lost[i] && telemetry_sysid[i] !=
+						(uint8_t)onboard_sysid) {
 					/* telemetry was healthy also in last iteration
 					 * we don't have to check a timeout,
 					 * telemetry from onboard computers is not accepted as a valid datalink
@@ -1612,6 +1627,35 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
+		/* Check engine failure
+		 * only for fixed wing for now
+		 */
+		if (!circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY) &&
+				status.is_rotary_wing == false &&
+				armed.armed &&
+				((actuator_controls.control[3] > ef_throttle_thres &&
+				battery.current_a/actuator_controls.control[3] <
+				ef_current2throttle_thres) ||
+				 (status.engine_failure))) {
+			/* potential failure, measure time */
+			if (timestamp_engine_healthy > 0 &&
+					hrt_elapsed_time(&timestamp_engine_healthy) >
+					ef_time_thres * 1e6 &&
+					!status.engine_failure) {
+					status.engine_failure = true;
+					status_changed = true;
+					mavlink_log_critical(mavlink_fd, "Engine Failure");
+			}
+		} else {
+			/* no failure reset flag */
+			timestamp_engine_healthy = hrt_absolute_time();
+			if (status.engine_failure) {
+				status.engine_failure = false;
+				status_changed = true;
+			}
+		}
+
+
 		/* handle commands last, as the system needs to be updated to handle them */
 		orb_check(cmd_sub, &updated);
 
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 98c0982b2..15c299a8b 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -138,3 +138,35 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0);
  * @max 255
  */
 PARAM_DEFINE_INT32(COM_ONBSYSID, 42);
+
+/** Engine Failure Throttle Threshold
+ *
+ * Engine failure triggers only above this throttle value
+ *
+ * @group commander
+ * @min 0.0f
+ * @max 1.0f
+ */
+PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
+
+/** Engine Failure Current/Throttle Threshold
+ *
+ * Engine failure triggers only below this current/throttle value
+ *
+ * @group commander
+ * @min 0.0f
+ * @max 7.0f
+ */
+PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
+
+/** Engine Failure Time Threshold
+ *
+ * Engine failure triggers only if the throttle threshold and the
+ * current to throttle threshold are violated for this time
+ *
+ * @group commander
+ * @unit second
+ * @min 0.0f
+ * @max 7.0f
+ */
+PARAM_DEFINE_FLOAT(COM_EF_TIME, 5.0f);
diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c
index b0f95aedf..9e5429988 100644
--- a/src/modules/systemlib/circuit_breaker.c
+++ b/src/modules/systemlib/circuit_breaker.c
@@ -108,6 +108,20 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
  */
 PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 0);
 
+/**
+ * Circuit breaker for engine failure detection
+ *
+ * Setting this parameter to 284953 will disable the engine failure detection.
+ * If the aircraft is in engine failure mode the enine failure flag will be
+ * set to healthy
+ * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
+ *
+ * @min 0
+ * @max 284953
+ * @group Circuit Breaker
+ */
+PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 0);
+
 bool circuit_breaker_enabled(const char* breaker, int32_t magic)
 {
 	int32_t val;
diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h
index 445a89d3a..6a55e4948 100644
--- a/src/modules/systemlib/circuit_breaker.h
+++ b/src/modules/systemlib/circuit_breaker.h
@@ -54,6 +54,7 @@
 #define CBRK_IO_SAFETY_KEY	22027
 #define CBRK_AIRSPD_CHK_KEY	162128
 #define CBRK_FLIGHTTERM_KEY	121212
+#define CBRK_ENGINEFAIL_KEY	284953
 
 #include <stdbool.h>