From adda7702f9cef732dd6665f80af421f0ced8d6ea Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 31 Jul 2015 20:05:59 +0200 Subject: [PATCH] MAVLink: Reduce default link data rates --- ROMFS/px4fmu_common/init.d/10020_3dr_quad | 37 +++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 4 +-- 2 files changed, 39 insertions(+), 2 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/10020_3dr_quad diff --git a/ROMFS/px4fmu_common/init.d/10020_3dr_quad b/ROMFS/px4fmu_common/init.d/10020_3dr_quad new file mode 100644 index 0000000000..0dbffba00f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/10020_3dr_quad @@ -0,0 +1,37 @@ +#!nsh +# +# @name 3DR Iris Quadrotor +# +# @type Quadrotor Wide +# +# @maintainer Anton Babushkin +# + +sh /etc/init.d/rc.mc_defaults + +if [ $AUTOCNF == yes ] +then + # TODO tune roll/pitch separately + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.13 + param set MC_ROLLRATE_I 0.05 + param set MC_ROLLRATE_D 0.004 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.13 + param set MC_PITCHRATE_I 0.05 + param set MC_PITCHRATE_D 0.004 + param set MC_YAW_P 2.5 + param set MC_YAWRATE_P 0.25 + param set MC_YAWRATE_I 0.25 + param set MC_YAWRATE_D 0.0 + + param set BAT_V_SCALING 0.00989 + param set BAT_C_SCALING 0.0124 +fi + +set MIXER quad_x + +set PWM_OUT 1234 + +set PWM_MIN 1100 +set PWM_MAX 1950 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 7c112d7386..c4ca341372 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -468,13 +468,13 @@ then if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - set MAVLINK_F "-r 5000 -d /dev/ttyS0" + set MAVLINK_F "-r 1200 -d /dev/ttyS0" # Exit from nsh to free port for mavlink set EXIT_ON_END yes else # Start MAVLink on default port: ttyS1 - set MAVLINK_F "-r 5000" + set MAVLINK_F "-r 1200" fi fi