diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-07 08:03:57 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-07 08:03:57 +0100 |
commit | 16dd054fa6bfc305d3a66f17d295ceaf1f554c12 (patch) | |
tree | 7a1be2efea0c24a292682e9823e013d959013c9b /ROMFS/px4fmu_common | |
parent | 40deb405c1e9232475c4b5e7bda8080f85fec19a (diff) | |
parent | cf9fa61a39f83e6fe4611ecf9336c1fcd1faaa78 (diff) | |
download | px4-firmware-16dd054fa6bfc305d3a66f17d295ceaf1f554c12.tar.gz px4-firmware-16dd054fa6bfc305d3a66f17d295ceaf1f554c12.tar.bz2 px4-firmware-16dd054fa6bfc305d3a66f17d295ceaf1f554c12.zip |
Merge branch 'master' of github.com:PX4/Firmware into paul_estimator
Diffstat (limited to 'ROMFS/px4fmu_common')
-rw-r--r-- | ROMFS/px4fmu_common/init.d/4008_ardrone | 35 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.autostart | 9 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.interface | 9 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rcS | 23 |
4 files changed, 68 insertions, 8 deletions
diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone new file mode 100644 index 000000000..0f98f7b6c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -0,0 +1,35 @@ +#!nsh +# +# ARDrone +# + +echo "[init] 4008_ardrone: PX4FMU on PX4IOAR carrier board" + +# Just use the default multicopter settings. +sh /etc/init.d/rc.mc_defaults + +# +# Load default params for this platform +# +if [ $DO_AUTOCONFIG == yes ] +then + # Set all params here, then disable autoconfig + param set MC_ROLL_P 5.0 + param set MC_ROLLRATE_P 0.13 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.0 + param set MC_PITCH_P 5.0 + param set MC_PITCHRATE_P 0.13 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.0 + param set MC_YAW_P 1.0 + param set MC_YAW_D 0.1 + param set MC_YAWRATE_P 0.15 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 + param set MC_YAW_FF 0.15 +fi + +set OUTPUT_MODE ardrone +set USE_IO no +set MIXER skip diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 3968af58e..7aaf7133e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -106,6 +106,15 @@ then sh /etc/init.d/4001_quad_x fi +# +# ARDrone +# + +if param compare SYS_AUTOSTART 4008 8 +then + sh /etc/init.d/4008_ardrone +fi + if param compare SYS_AUTOSTART 4010 10 then sh /etc/init.d/4010_dji_f330 diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index d25f01dde..7f793b219 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -3,7 +3,7 @@ # Script to configure control interface # -if [ $MIXER != none ] +if [ $MIXER != none -a $MIXER != skip ] then # # Load mixer @@ -33,8 +33,11 @@ then tone_alarm $TUNE_OUT_ERROR fi else - echo "[init] Mixer not defined" - tone_alarm $TUNE_OUT_ERROR + if [ $MIXER != skip ] + then + echo "[init] Mixer not defined" + tone_alarm $TUNE_OUT_ERROR + fi fi if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 2356dba54..8ca14b320 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -240,6 +240,11 @@ then fi fi + if [ $OUTPUT_MODE == ardrone ] + then + set FMU_MODE gpio_serial + fi + if [ $HIL == yes ] then set OUTPUT_MODE hil @@ -277,9 +282,9 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - if [ $OUTPUT_MODE == fmu ] + if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ] then - echo "[init] Use FMU PWM as primary output" + echo "[init] Use FMU as primary output" if fmu mode_$FMU_MODE then echo "[init] FMU mode_$FMU_MODE started" @@ -294,7 +299,7 @@ then then set TTYS1_BUSY yes fi - if [ $FMU_MODE == pwm_gpio ] + if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ] then set TTYS1_BUSY yes fi @@ -351,7 +356,7 @@ then fi fi else - if [ $OUTPUT_MODE != fmu ] + if [ $OUTPUT_MODE != fmu -a $OUTPUT_MODE != ardrone ] then if fmu mode_$FMU_MODE then @@ -367,7 +372,7 @@ then then set TTYS1_BUSY yes fi - if [ $FMU_MODE == pwm_gpio ] + if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ] then set TTYS1_BUSY yes fi @@ -429,6 +434,14 @@ then fi # + # Start up ARDrone Motor interface + # + if [ $OUTPUT_MODE == ardrone ] + then + ardrone_interface start -d /dev/ttyS1 + fi + + # # Fixed wing setup # if [ $VEHICLE_TYPE == fw ] |