diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 2191d48e42..f7afafc143 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -65,7 +65,9 @@ fi # Use environment variable PX4_ESTIMATOR to choose estimator. -if [ "$PX4_ESTIMATOR" = "ekf2" ]; then +if [ "$PX4_ESTIMATOR" = "q" ]; then + param set SYS_MC_EST_GROUP 3 +elif [ "$PX4_ESTIMATOR" = "ekf2" ]; then param set SYS_MC_EST_GROUP 2 elif [ "$PX4_ESTIMATOR" = "lpe" ]; then param set SYS_MC_EST_GROUP 1 diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 65329398ef..5c652e6a64 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -30,10 +30,18 @@ then fi else # - # EKF2 + # Q estimator (attitude estimation only) # - param set SYS_MC_EST_GROUP 2 - ekf2 start + 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/boards/omnibus/f4sd/init/rc.board_defaults b/boards/omnibus/f4sd/init/rc.board_defaults index daae85b4cb..e26bffe55f 100644 --- a/boards/omnibus/f4sd/init/rc.board_defaults +++ b/boards/omnibus/f4sd/init/rc.board_defaults @@ -4,6 +4,12 @@ #------------------------------------------------------------------------------ +# transition from LPE+Q to Q estimator (2019-06-05) +if param compare SYS_MC_EST_GROUP 1 +then + param set SYS_MC_EST_GROUP 3 +fi + if [ $AUTOCNF = yes ] then @@ -11,7 +17,7 @@ then param set CBRK_IO_SAFETY 22027 # use the Q attitude estimator, it works w/o mag or GPS. - param set SYS_MC_EST_GROUP 1 + param set SYS_MC_EST_GROUP 3 param set ATT_ACC_COMP 0 param set ATT_W_ACC 0.4000 param set ATT_W_GYRO_BIAS 0.0000 diff --git a/src/lib/systemlib/system_params.c b/src/lib/systemlib/system_params.c index f0041bef89..2b916f06e6 100644 --- a/src/lib/systemlib/system_params.c +++ b/src/lib/systemlib/system_params.c @@ -102,9 +102,8 @@ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); * * @value 1 local_position_estimator, attitude_estimator_q (unsupported) * @value 2 ekf2 (recommended) + * @value 3 Q attitude estimator (no position) * - * @min 1 - * @max 2 * @reboot_required true * @group System */