Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

SYS_MC_EST_GROUP: add q-estimator option #12197

Merged
merged 4 commits into from
Jun 6, 2019
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion ROMFS/px4fmu_common/init.d-posix/rcS
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
14 changes: 11 additions & 3 deletions ROMFS/px4fmu_common/init.d/rc.mc_apps
Original file line number Diff line number Diff line change
Expand Up @@ -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

###############################################################################
Expand Down
15 changes: 0 additions & 15 deletions boards/intel/aerofc-v1/init/rc.board_defaults
Original file line number Diff line number Diff line change
Expand Up @@ -13,21 +13,6 @@ fi
# other configurations.
param set SYS_AUTOSTART 4070

if [ $AUTOCNF = yes ]
then
# Disable safety switch by default
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 ATT_ACC_COMP 0
param set ATT_W_ACC 0.4000
param set ATT_W_GYRO_BIAS 0.0000

param set SYS_HAS_MAG 0
fi


set DATAMAN_OPT -i
set LOGGER_ARGS "-m mavlink"
set MIXER_AUX none
1 change: 1 addition & 0 deletions boards/omnibus/f4sd/default.cmake
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,7 @@ px4_add_board(

SYSTEMCMDS
#bl_update
dmesg
config
dumpfile
esc_calib
Expand Down
13 changes: 5 additions & 8 deletions boards/omnibus/f4sd/init/rc.board_defaults
Original file line number Diff line number Diff line change
Expand Up @@ -4,23 +4,20 @@
#------------------------------------------------------------------------------


# transition from params file to flash-based params (2018-12-20)
if [ -f $PARAM_FILE ]
# transition from LPE+Q to Q estimator (2019-06-05)
if param compare SYS_MC_EST_GROUP 1
then
param load $PARAM_FILE
param save
# create a backup
mv $PARAM_FILE ${PARAM_FILE}.bak
reboot
param set SYS_MC_EST_GROUP 3
fi


if [ $AUTOCNF = yes ]
then
# Disable safety switch by default
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
Expand Down
3 changes: 3 additions & 0 deletions boards/omnibus/f4sd/src/board_config.h
Original file line number Diff line number Diff line change
Expand Up @@ -264,6 +264,9 @@

#define BOARD_HAS_ON_RESET 1

#define BOARD_ENABLE_CONSOLE_BUFFER
#define BOARD_CONSOLE_BUFFER_SIZE (1024*3)

__BEGIN_DECLS

/****************************************************************************************************
Expand Down
3 changes: 1 addition & 2 deletions src/lib/systemlib/system_params.c
Original file line number Diff line number Diff line change
Expand Up @@ -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
*/
Expand Down