From c9a28fc0ebcc96f5055eee23519c3185706d82d2 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Sun, 4 Jun 2017 12:43:28 +0200
Subject: [PATCH] ROMFS: Reset the estimator param to EKF2 if LPE fails to
 start

---
 ROMFS/px4fmu_common/init.d/rc.fw_apps   | 9 +++++++++
 ROMFS/px4fmu_common/init.d/rc.mc_apps   | 7 +++++--
 ROMFS/px4fmu_common/init.d/rc.vtol_apps | 9 ++++++---
 3 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps
index 040bcbde0354..b55b2b951f94 100644
--- a/ROMFS/px4fmu_common/init.d/rc.fw_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps
@@ -3,6 +3,15 @@
 # Standard apps for fixed wing
 #
 
+# LPE
+if param compare SYS_MC_EST_GROUP 1
+then
+	echo "ERROR [init] Estimator LPE not supported on fixed wing. Using EKF2"
+	param set SYS_MC_EST_GROUP 2
+	param save
+	ekf2 start
+fi
+
 #
 # Start the attitude and position estimator
 #
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps
index 3f8b17e2aa3e..85c475e40af4 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps
@@ -11,8 +11,8 @@
 # INAV (deprecated)
 if param compare SYS_MC_EST_GROUP 0
 then
-	echo "ERROR [init] Estimator INAV deprecated. Using LPE"
-	param set SYS_MC_EST_GROUP 1
+	echo "ERROR [init] Estimator INAV deprecated. Using EKF2"
+	param set SYS_MC_EST_GROUP 2
 	param save
 fi
 
@@ -25,6 +25,9 @@ then
 	then
 		local_position_estimator start
 	else
+		echo "ERROR [init] Estimator LPE not available. Using EKF2"
+		param set SYS_MC_EST_GROUP 2
+		param save
 		ekf2 start
 	fi
 fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps
index 3af68c798a22..8f96469c4b50 100644
--- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps
@@ -12,8 +12,8 @@
 # INAV (deprecated)
 if param compare SYS_MC_EST_GROUP 0
 then
-	echo "ERROR [init] Estimator INAV deprecated. Using LPE"
-	param set SYS_MC_EST_GROUP 1
+	echo "ERROR [init] Estimator INAV deprecated. Using EKF2"
+	param set SYS_MC_EST_GROUP 2
 	param save
 fi
 
@@ -21,11 +21,14 @@ fi
 if param compare SYS_MC_EST_GROUP 1
 then
 	# Try to start LPE. If it fails, start EKF2 as a default
-	# Unfortunately we do not build it on px4fmu-v2 duo to a limited flash.
+	# Unfortunately we do not build it on px4fmu-v2 due to a limited flash.
 	if attitude_estimator_q start
 	then
 		local_position_estimator start
 	else
+		echo "ERROR [init] Estimator LPE not available. Using EKF2"
+		param set SYS_MC_EST_GROUP 2
+		param save
 		ekf2 start
 	fi
 fi