diff --git a/ROMFS/px4fmu_common/init.d/rc.balloon_apps b/ROMFS/px4fmu_common/init.d/rc.balloon_apps index baccef641c..2adb216ec1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.balloon_apps +++ b/ROMFS/px4fmu_common/init.d/rc.balloon_apps @@ -8,4 +8,35 @@ # # Start the attitude and position estimator. # -ekf2 start & + +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 px4_fmu-v2 due to a limited flash. + # + if attitude_estimator_q start + then + echo "WARN [init] Estimator LPE unsupported, EKF2 recommended." + local_position_estimator start + else + echo "ERROR [init] Estimator LPE not available. Using EKF2" + param set SYS_MC_EST_GROUP 2 + param save + reboot + fi +else + # + # Q estimator (attitude estimation only) + # + if param compare SYS_MC_EST_GROUP 3 + then + attitude_estimator_q start + else + # + # EKF2 + # + param set SYS_MC_EST_GROUP 2 + ekf2 start & + fi +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.balloon_defaults b/ROMFS/px4fmu_common/init.d/rc.balloon_defaults index dfc0f55fa4..83d4d3851c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.balloon_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.balloon_defaults @@ -16,4 +16,5 @@ then # FW takeoff acceleration can easily exceed ublox GPS 2G default. # param set GPS_UBX_DYNMODEL 8 + param set SYS_MC_EST_GROUP 1 fi