From f73af95b388ff66602c1f2918f52f5f968c44b0e Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 14 Apr 2016 15:22:25 -0400 Subject: [PATCH] Added SYS_MC_EST_GROUP to allow fine grained algorithm selection. --- ROMFS/px4fmu_common/init.d/rc.mc_apps | 32 ++++++++++------- ROMFS/px4fmu_common/init.d/rc.vtol_apps | 34 ++++++++++++------- .../position_estimator_inav_params.cpp | 12 ------- src/modules/sensors/sensors.cpp | 2 +- src/modules/systemlib/system_params.c | 16 +++++++++ 5 files changed, 59 insertions(+), 37 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 9c782d7bb0..56a349457f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -4,23 +4,31 @@ # att & pos estimator, att & pos control. # -# The system is defaulting to INAV_ENABLED = 1 -# but users can alternatively try the EKF-based -# filter by setting INAV_ENABLED = 0 -if param compare INAV_ENABLED 1 + +#--------------------------------------- +# Estimator group selction +# +# INAV +if param compare SYS_MC_EST_GROUP 0 then attitude_estimator_q start position_estimator_inav start -else - if param compare LPE_ENABLED 1 - then - attitude_estimator_q start - local_position_estimator start - else - ekf2 start - fi fi +# LPE +if param compare SYS_MC_EST_GROUP 1 +then + attitude_estimator_q start + local_position_estimator start +fi + +# EKF +if param compare SYS_MC_EST_GROUP 2 +then + ekf2 start +fi +#--------------------------------------- + if mc_att_control start then else diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index 95a6092d75..c3d54fd299 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -3,24 +3,34 @@ # Standard apps for vtol: # att & pos estimator, att & pos control. # +# -# The system is defaulting to INAV_ENABLED = 1 -# but users can alternatively try the EKF-based -# filter by setting INAV_ENABLED = 0 -if param compare INAV_ENABLED 1 + +#--------------------------------------- +# Estimator group selction +# +# INAV +if param compare SYS_MC_EST_GROUP 0 then attitude_estimator_q start position_estimator_inav start -else - if param compare LPE_ENABLED 1 - then - attitude_estimator_q start - local_position_estimator start - else - ekf2 start - fi fi +# LPE +if param compare SYS_MC_EST_GROUP 1 +then + attitude_estimator_q start + local_position_estimator start +fi + +# EKF +if param compare SYS_MC_EST_GROUP 2 +then + ekf2 start +fi +#--------------------------------------- + + vtol_att_control start mc_att_control start mc_pos_control start diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.cpp b/src/modules/position_estimator_inav/position_estimator_inav_params.cpp index a680f6b487..854fc61638 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.cpp @@ -345,18 +345,6 @@ PARAM_DEFINE_FLOAT(INAV_LIDAR_OFF, 0.0f); */ PARAM_DEFINE_INT32(CBRK_NO_VISION, 0); -/** - * INAV enabled - * - * If set to 1, use INAV for position estimation. - * Else the system uses the combined attitude / position - * filter framework. - * - * @boolean - * @group Position Estimator INAV - */ -PARAM_DEFINE_INT32(INAV_ENABLED, 1); - int inav_parameters_init(struct position_estimator_inav_param_handles *h) { h->w_z_baro = param_find("INAV_W_Z_BARO"); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 2766565d7a..78be227e90 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -682,7 +682,7 @@ Sensors::Sensors() : (void)param_find("PWM_AUX_DISARMED"); (void)param_find("TRIG_MODE"); (void)param_find("UAVCAN_ENABLE"); - (void)param_find("LPE_ENABLED"); + (void)param_find("SYS_MC_EST_GROUP"); /* fetch initial parameter values */ parameters_update(); diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index a77feff43f..29e0388a73 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -82,6 +82,22 @@ PARAM_DEFINE_INT32(SYS_USE_IO, 1); */ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); +/** + * Set multicopter estimator group + * + * Set the group of estimators used for multicopters and vtols + * + * @value 0 position_estimator_inav, attitude_estimator_q + * @value 1 local_position_estimator, attitude_estimator_q + * @value 2 ekf2 + * + * @min 0 + * @max 2 + * @reboot_required true + * @group System + */ +PARAM_DEFINE_INT32(SYS_MC_EST_GROUP, 0); + /** * Companion computer interface *