From ed8645a83b9a1abdbca64fe8645e78dd41134fca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Dec 2015 11:41:06 +0100 Subject: [PATCH] Add EKF2 config and startup option for jMAVSim --- Makefile | 3 + cmake/configs/posix_sitl_ekf2.cmake | 91 +++++++++++++++++++ posix-configs/SITL/init/rcS_ekf2_jmavsim_iris | 62 +++++++++++++ 3 files changed, 156 insertions(+) create mode 100644 cmake/configs/posix_sitl_ekf2.cmake create mode 100644 posix-configs/SITL/init/rcS_ekf2_jmavsim_iris diff --git a/Makefile b/Makefile index 1c269a8b56..ee2670244d 100644 --- a/Makefile +++ b/Makefile @@ -148,6 +148,9 @@ posix_sitl_default: posix_sitl_lpe: $(call cmake-build,$@) +posix_sitl_ekf2: + $(call cmake-build,$@) + ros_sitl_default: $(call cmake-build,$@) diff --git a/cmake/configs/posix_sitl_ekf2.cmake b/cmake/configs/posix_sitl_ekf2.cmake new file mode 100644 index 0000000000..aefe3255d9 --- /dev/null +++ b/cmake/configs/posix_sitl_ekf2.cmake @@ -0,0 +1,91 @@ +include(posix/px4_impl_posix) + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake) + +set(config_module_list + drivers/device + drivers/boards/sitl + drivers/pwm_out_sim + platforms/common + platforms/posix/px4_layer + platforms/posix/work_queue + platforms/posix/drivers/adcsim + platforms/posix/drivers/gpssim + platforms/posix/drivers/tonealrmsim + platforms/posix/drivers/accelsim + platforms/posix/drivers/airspeedsim + platforms/posix/drivers/barosim + platforms/posix/drivers/gyrosim + platforms/posix/drivers/rgbledsim + platforms/posix/drivers/ledsim + systemcmds/param + systemcmds/mixer + systemcmds/ver + systemcmds/esc_calib + systemcmds/reboot + systemcmds/topic_listener + systemcmds/perf + modules/uORB + modules/param + modules/systemlib + modules/systemlib/mixer + modules/sensors + modules/simulator + modules/mavlink + modules/dataman + modules/attitude_estimator_ekf + #modules/attitude_estimator_q + modules/ekf_att_pos_estimator + modules/position_estimator_inav + modules/mc_pos_control + modules/mc_att_control + modules/navigator + modules/commander + modules/vtol_att_control + modules/controllib + modules/ekf2 + lib/mathlib + lib/mathlib/math/filter + lib/conversion + lib/ecl + lib/external_lgpl + lib/geo + lib/geo_lookup + lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff + ) + +set(config_extra_builtin_cmds + serdis + sercon + ) + +set(config_sitl_rcS + posix-configs/SITL/init/rcS_ekf2 + CACHE FILEPATH "init script for sitl" + ) + +set(config_sitl_viewer + jmavsim + CACHE STRING "viewer for sitl" + ) +set_property(CACHE config_sitl_viewer + PROPERTY STRINGS "jmavsim;none") + +set(config_sitl_debugger + disable + CACHE STRING "debugger for sitl" + ) +set_property(CACHE config_sitl_debugger + PROPERTY STRINGS "disable;gdb;lldb") + + + +add_custom_target(sercon) +set_target_properties(sercon PROPERTIES + MAIN "sercon" STACK "2048") + +add_custom_target(serdis) +set_target_properties(serdis PROPERTIES + MAIN "serdis" STACK "2048") diff --git a/posix-configs/SITL/init/rcS_ekf2_jmavsim_iris b/posix-configs/SITL/init/rcS_ekf2_jmavsim_iris new file mode 100644 index 0000000000..58237da408 --- /dev/null +++ b/posix-configs/SITL/init/rcS_ekf2_jmavsim_iris @@ -0,0 +1,62 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 2 +param set MC_PITCHRATE_P 0.15 +param set MC_PITCH_P 7 +param set MC_ROLL_P 7 +param set MC_ROLLRATE_P 0.15 +param set MC_YAW_P 2.8 +param set MC_YAWRATE_P 0.35 +param set SYS_AUTOSTART 4010 +param set SYS_RESTART_TYPE 2 +dataman start +param set CAL_GYRO0_ID 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set MPC_XY_P 0.4 +param set MPC_XY_VEL_P 0.2 +param set MPC_XY_VEL_D 0.005 +param set SENS_BOARD_ROT 0 +param set COM_RC_IN_MODE 1 +param set NAV_ACC_RAD 2.0 +param set RTL_RETURN_ALT 30.0 +param set RTL_DESCEND_ALT 10.0 +rgbledsim start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +mc_pos_control start +mc_att_control start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix +mavlink start -u 14556 -r 2000000 +mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 80 -s ATTITUDE -u 14556 +mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 20 -s RC_CHANNELS -u 14556 +mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +mavlink boot_complete +sdlog2 start -r 100 -e -t -a +ekf2 start