aboutsummaryrefslogtreecommitdiff
path: root/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
diff options
context:
space:
mode:
authorAnton Babushkin <anton.babushkin@me.com>2014-01-10 13:18:34 +0100
committerAnton Babushkin <anton.babushkin@me.com>2014-01-10 13:18:34 +0100
commitb5d56523bc100d7bf95a6dfbac95c1afc89e345e (patch)
treea54be4fa60168cab7c03aa8622ddf05c1cf275b3 /ROMFS/px4fmu_common/init.d/10015_tbs_discovery
parent891cb3f8c2755fdc566711448f1f19a06938bd2f (diff)
downloadpx4-firmware-b5d56523bc100d7bf95a6dfbac95c1afc89e345e.tar.gz
px4-firmware-b5d56523bc100d7bf95a6dfbac95c1afc89e345e.tar.bz2
px4-firmware-b5d56523bc100d7bf95a6dfbac95c1afc89e345e.zip
Init scripts cleanup, WIP
Diffstat (limited to 'ROMFS/px4fmu_common/init.d/10015_tbs_discovery')
-rw-r--r--ROMFS/px4fmu_common/init.d/10015_tbs_discovery85
1 files changed, 21 insertions, 64 deletions
diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
index 81d4b5d57..610d482c1 100644
--- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
+++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
@@ -1,74 +1,31 @@
#!nsh
+# Maintainers: Anton Babushkin <anton.babushkin@me.com>
+
echo "[init] Team Blacksheep Discovery Quad"
-#
-# Load default params for this platform
-#
-if param compare SYS_AUTOCONFIG 1
+if [ $DO_AUTOCONFIG == yes ]
then
- # Set all params here, then disable autoconfig
- param set SYS_AUTOCONFIG 0
-
- param set MC_ATTRATE_D 0.006
- param set MC_ATTRATE_I 0.0
- param set MC_ATTRATE_P 0.17
- param set MC_ATT_D 0.0
- param set MC_ATT_I 0.0
+ #
+ # Default parameters for this platform
+ #
param set MC_ATT_P 5.0
- param set MC_YAWPOS_D 0.0
- param set MC_YAWPOS_I 0.15
+ param set MC_ATT_I 0.0
+ param set MC_ATT_D 0.0
+ 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_YAWRATE_D 0.0
- param set MC_YAWRATE_I 0.0
+ param set MC_YAWPOS_I 0.15
+ param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.2
-
- param save
-fi
-
-#
-# Force some key parameters to sane values
-# MAV_TYPE 2 = quadrotor
-#
-param set MAV_TYPE 2
-
-#
-# Start and configure PX4IO or FMU interface
-#
-if px4io detect
-then
- # Start MAVLink (depends on orb)
- mavlink start
- usleep 5000
-
- sh /etc/init.d/rc.io
-else
- # Start MAVLink (on UART1 / ttyS0)
- mavlink start -d /dev/ttyS0
- usleep 5000
- fmu mode_pwm
- param set BAT_V_SCALING 0.004593
- set EXIT_ON_END yes
+ param set MC_YAWRATE_I 0.0
+ param set MC_YAWRATE_D 0.0
fi
-#
-# Load the mixer for a quad with wide arms
-#
-mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
-
-#
-# Set PWM output frequency
-#
-pwm rate -c 1234 -r 400
-
-#
-# Set disarmed, min and max PWM signals
-#
-pwm disarmed -c 1234 -p 900
-pwm min -c 1234 -p 1100
-pwm max -c 1234 -p 1900
-
-#
-# Start common for all multirotors apps
-#
-sh /etc/init.d/rc.multirotor
+set FRAME_TYPE mc
+set FRAME_GEOMETRY quad_w
+set PWM_RATE 400
+set PWM_DISARMED 900
+set PWM_MIN 1100
+set PWM_MAX 1900