From 171ccd1203508e32aad5fbb9c6414108e9c4f4e2 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Sun, 25 Dec 2016 17:06:27 +0100
Subject: [PATCH] POSIX SITL configs: Update default parameters to improve
 simulation behaviour

The main changes include:
 * Better attitude tuning for airframes (more realistic models, the models had previously not as much thrust as the real vehicles)
 * Better waypoint and navigation default parameters which match the on-hardware parameters
 * More suitable minimum and trim airspeeds for VTOL and fixed wing which prevents stalls that happened in SITL previously (the new airspeeds match the real vehicles nicely)
---
 posix-configs/SITL/init/ekf2/plane         |  3 +++
 posix-configs/SITL/init/ekf2/solo          |  4 ++--
 posix-configs/SITL/init/ekf2/standard_vtol | 11 +++++++++--
 posix-configs/SITL/init/lpe/plane          |  4 ++++
 posix-configs/SITL/init/lpe/standard_vtol  | 13 ++++++++++---
 5 files changed, 28 insertions(+), 7 deletions(-)

diff --git a/posix-configs/SITL/init/ekf2/plane b/posix-configs/SITL/init/ekf2/plane
index f7755ae80d..e7f4806664 100644
--- a/posix-configs/SITL/init/ekf2/plane
+++ b/posix-configs/SITL/init/ekf2/plane
@@ -25,6 +25,9 @@ param set SENS_BOARD_X_OFF 0.000001
 param set COM_RC_IN_MODE 1
 param set NAV_DLL_ACT 2
 param set NAV_ACC_RAD 15.0
+param set NAV_LOITER_RAD 50
+param set MIS_LTRMIN_ALT 30
+param set MIS_TAKEOFF_ALT 30
 param set FW_THR_IDLE 0.8
 param set EKF2_GBIAS_INIT 0.01
 param set EKF2_ANGERR_INIT 0.01
diff --git a/posix-configs/SITL/init/ekf2/solo b/posix-configs/SITL/init/ekf2/solo
index 692c48dfb1..ac8d44e84c 100644
--- a/posix-configs/SITL/init/ekf2/solo
+++ b/posix-configs/SITL/init/ekf2/solo
@@ -29,8 +29,8 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0
 param set MIS_TAKEOFF_ALT 2.5
-param set MC_ROLLRATE_P 0.2
-param set MC_PITCHRATE_P 0.2
+param set MC_ROLLRATE_P 0.15
+param set MC_PITCHRATE_P 0.15
 param set MC_PITCH_P 6
 param set MC_ROLL_P 6
 param set MPC_HOLD_MAX_Z 2.0
diff --git a/posix-configs/SITL/init/ekf2/standard_vtol b/posix-configs/SITL/init/ekf2/standard_vtol
index b4cccc5c04..f1abfcb8c1 100644
--- a/posix-configs/SITL/init/ekf2/standard_vtol
+++ b/posix-configs/SITL/init/ekf2/standard_vtol
@@ -35,13 +35,19 @@ param set MPC_XY_FF 0.1
 param set MPC_Z_VEL_MAX 1.5
 param set MPC_Z_VEL_P 0.6
 param set MPC_Z_VEL_I 0.15
+param set FW_AIRSPD_MIN 14
+param set FW_AIRSPD_TRIM 16
+param set FW_AIRSPD_MAX 25
 param set SENS_BOARD_ROT 0
 param set SENS_DPRES_OFF 0.001
 param set SENS_BOARD_X_OFF 0.000001
 param set COM_RC_IN_MODE 1
 param set NAV_DLL_ACT 2
-param set NAV_ACC_RAD 3.0
+param set NAV_ACC_RAD 5.0
+param set NAV_LOITER_RAD 80
 param set MPC_TKO_SPEED 1.0
+param set MIS_LTRMIN_ALT 10
+param set MIS_TAKEOFF_ALT 10
 param set MIS_YAW_TMT 10
 param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
@@ -52,7 +58,7 @@ param set EKF2_GBIAS_INIT 0.01
 param set EKF2_ANGERR_INIT 0.01
 param set EKF2_MAG_TYPE 1
 replay tryapplyparams
-simulator start -s 
+simulator start -s
 rgbledsim start
 tone_alarm start
 gyrosim start
@@ -76,6 +82,7 @@ fw_att_control start
 mixer load /dev/pwm_output0 ROMFS/sitl/mixers/standard_vtol_sitl.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
+mavlink stream -r 20 -s EXTENDED_SYS_STATE -u 14540
 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
diff --git a/posix-configs/SITL/init/lpe/plane b/posix-configs/SITL/init/lpe/plane
index 89b9965109..48d3f0025e 100644
--- a/posix-configs/SITL/init/lpe/plane
+++ b/posix-configs/SITL/init/lpe/plane
@@ -25,6 +25,10 @@ param set SENS_BOARD_X_OFF 0.000001
 param set COM_RC_IN_MODE 1
 param set NAV_DLL_ACT 2
 param set NAV_ACC_RAD 15.0
+param set NAV_LOITER_RAD 50
+param set MIS_LTRMIN_ALT 30
+param set MIS_TAKEOFF_ALT 30
+
 param set FW_THR_IDLE 0.8
 param set EKF2_GBIAS_INIT 0.01
 param set EKF2_ANGERR_INIT 0.01
diff --git a/posix-configs/SITL/init/lpe/standard_vtol b/posix-configs/SITL/init/lpe/standard_vtol
index 7ca845a591..1f3fb8e7c3 100644
--- a/posix-configs/SITL/init/lpe/standard_vtol
+++ b/posix-configs/SITL/init/lpe/standard_vtol
@@ -35,13 +35,19 @@ param set MPC_XY_FF 0.1
 param set MPC_Z_VEL_MAX 1.5
 param set MPC_Z_VEL_P 0.6
 param set MPC_Z_VEL_I 0.15
+param set FW_AIRSPD_MIN 14
+param set FW_AIRSPD_TRIM 16
+param set FW_AIRSPD_MAX 25
 param set SENS_BOARD_ROT 0
 param set SENS_DPRES_OFF 0.001
 param set SENS_BOARD_X_OFF 0.000001
 param set COM_RC_IN_MODE 1
 param set NAV_DLL_ACT 2
-param set NAV_ACC_RAD 3.0
+param set NAV_ACC_RAD 5.0
+param set NAV_LOITER_RAD 50
 param set MPC_TKO_SPEED 1.0
+param set MIS_LTRMIN_ALT 10
+param set MIS_TAKEOFF_ALT 10
 param set MIS_YAW_TMT 10
 param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
@@ -49,7 +55,7 @@ param set RTL_LAND_DELAY 0
 param set COM_DISARM_LAND 5
 param set MPC_ACC_HOR_MAX 2
 replay tryapplyparams
-simulator start -s 
+simulator start -s
 rgbledsim start
 tone_alarm start
 gyrosim start
@@ -74,6 +80,7 @@ fw_att_control start
 mixer load /dev/pwm_output0 ROMFS/sitl/mixers/standard_vtol_sitl.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
+mavlink stream -r 20 -s EXTENDED_SYS_STATE -u 14540
 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
@@ -83,6 +90,6 @@ 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
-sdlog2 start -r 200 -e -t -a
+logger start -e -t
 mavlink boot_complete
 replay trystart