diff options
Diffstat (limited to 'ROMFS/px4fmu_common')
-rw-r--r-- | ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 6 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/10016_3dr_iris | 34 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 38 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 38 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/4009_ardrone_flow | 2 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/4010_dji_f330 | 34 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/4011_dji_f450 | 6 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/4012_hk_x550 | 6 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.logging | 2 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.mc_apps | 4 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rcS | 18 |
11 files changed, 73 insertions, 115 deletions
diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 63798bb3c..56c74a3b5 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -12,13 +12,11 @@ then # param set MC_ATT_P 5.0 param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 + param set MC_YAW_P 0.5 + param set MC_YAW_I 0.15 param set MC_ATTRATE_P 0.17 param set MC_ATTRATE_I 0.0 param set MC_ATTRATE_D 0.006 - param set MC_YAWPOS_P 0.5 - param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_D 0.0 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 67c24fab3..a3bcb63eb 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -12,32 +12,28 @@ then # param set MC_ATT_P 9.0 param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 + param set MC_YAW_P 0.5 + param set MC_YAW_I 0.15 param set MC_ATTRATE_P 0.13 param set MC_ATTRATE_I 0.0 param set MC_ATTRATE_D 0.004 - param set MC_YAWPOS_P 0.5 - param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_D 0.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.8 - param set MPC_THR_MIN 0.2 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 + param set MPC_TILT_MAX 1.0 + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_VEL_P 0.1 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_VEL_P 0.1 param set BAT_V_SCALING 0.00989 param set BAT_C_SCALING 0.0124 diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil index 8c0797d7c..2d497374a 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -10,34 +10,30 @@ then # # Default parameters for this platform # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_YAW_P 2.0 + param set MC_YAW_I 0.0 param set MC_ATTRATE_P 0.12 param set MC_ATTRATE_I 0.0 param set MC_ATTRATE_D 0.004 - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.3 param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_D 0.005 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.8 - param set MPC_THR_MIN 0.2 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 + param set MPC_TILT_MAX 1.0 + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_VEL_P 0.1 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_VEL_P 0.1 fi set HIL yes diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index bce3015fc..e95844891 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -5,42 +5,6 @@ # Maintainers: Anton Babushkin <anton.babushkin@me.com> # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ATTRATE_P 0.12 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.8 - param set MPC_THR_MIN 0.2 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 -fi +sh /etc/init.d/1001_rc_quad_x.hil -set HIL yes - -set VEHICLE_TYPE mc set MIXER FMU_quad_+ diff --git a/ROMFS/px4fmu_common/init.d/4009_ardrone_flow b/ROMFS/px4fmu_common/init.d/4009_ardrone_flow index 2886bcb75..e2cb8833d 100644 --- a/ROMFS/px4fmu_common/init.d/4009_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/4009_ardrone_flow @@ -67,7 +67,7 @@ flow_position_estimator start # # Fire up the multi rotor attitude controller # -multirotor_att_control start +mc_att_control_vector start # # Fire up the flow position controller diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index ab1db94d0..e0cf92d97 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -12,32 +12,28 @@ then # param set MC_ATT_P 7.0 param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 + param set MC_YAW_P 2.8 + param set MC_YAW_I 0.0 param set MC_ATTRATE_P 0.12 param set MC_ATTRATE_I 0.0 param set MC_ATTRATE_D 0.004 - param set MC_YAWPOS_P 2.8 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.05 param set MC_YAWRATE_D 0.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.8 - param set MPC_THR_MIN 0.2 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 + param set MPC_TILT_MAX 1.0 + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_VEL_P 0.1 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_VEL_P 0.1 fi set VEHICLE_TYPE mc diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 299771c1d..ced69783d 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -12,13 +12,11 @@ then # param set MC_ATT_P 7.0 param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 + param set MC_YAW_P 2.0 + param set MC_YAW_I 0.0 param set MC_ATTRATE_P 0.12 param set MC_ATTRATE_I 0.0 param set MC_ATTRATE_D 0.004 - param set MC_YAWPOS_P 2.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.3 param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_D 0.005 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index 7e020cf59..e1423e008 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -12,13 +12,11 @@ then # param set MC_ATT_P 5.5 param set MC_ATT_I 0 - param set MC_ATT_D 0 + param set MC_YAW_P 0.6 + param set MC_YAW_I 0 param set MC_ATTRATE_P 0.14 param set MC_ATTRATE_I 0 param set MC_ATTRATE_D 0.006 - param set MC_YAWPOS_P 0.6 - param set MC_YAWPOS_I 0 - param set MC_YAWPOS_D 0 param set MC_YAWRATE_P 0.08 param set MC_YAWRATE_I 0 param set MC_YAWRATE_D 0 diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index ac620844c..dcf5bbced 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -7,8 +7,10 @@ if [ -d /fs/microsd ] then if hw_ver compare PX4FMU_V1 then + echo "Start sdlog2 at 50Hz" sdlog2 start -r 50 -a -b 16 -t else + echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -a -b 16 -t fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 8b51d57e5..96fe32c8a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -16,9 +16,9 @@ position_estimator_inav start # # Start attitude control # -multirotor_att_control start +mc_att_control start # # Start position control # -multirotor_pos_control start +mc_pos_control start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index a9c5c59ea..45dec5ba6 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -323,7 +323,7 @@ then if [ $OUTPUT_MODE == hil ] then echo "[init] Use HIL as primary output" - if hil mode_pwm + if hil mode_port2_pwm8 then echo "[init] HIL output started" else @@ -401,6 +401,16 @@ then fi # + # Start the datamanager + # + dataman start + + # + # Start the navigator + # + navigator start + + # # Sensors, Logging, GPS # echo "[init] Start sensors" @@ -464,15 +474,15 @@ then # Use mixer to detect vehicle type if [ $MIXER == FMU_hex_x -o $MIXER == FMU_hex_+ ] then - param set MAV_TYPE 13 + set MAV_TYPE 13 fi if [ $MIXER == FMU_octo_x -o $MIXER == FMU_octo_+ ] then - param set MAV_TYPE 14 + set MAV_TYPE 14 fi if [ $MIXER == FMU_octo_cox ] then - param set MAV_TYPE 14 + set MAV_TYPE 14 fi fi |