From 31c6440b6491cd6310c9db72be200a4ffba689c9 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 17 Apr 2013 11:54:24 -0700 Subject: Un-ignore the *.d directories in the ROMFS directory to avoid ignoring the init.d directory and friends. This is rinky, but the alternatives are all a mess. --- ROMFS/px4fmu_common/init.d/rcS | 79 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 79 insertions(+) create mode 100755 ROMFS/px4fmu_common/init.d/rcS (limited to 'ROMFS/px4fmu_common/init.d/rcS') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS new file mode 100755 index 000000000..89a767879 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -0,0 +1,79 @@ +#!nsh +# +# PX4FMU startup script. +# +# This script is responsible for: +# +# - mounting the microSD card (if present) +# - running the user startup script from the microSD card (if present) +# - detecting the configuration of the system and picking a suitable +# startup script to continue with +# +# Note: DO NOT add configuration-specific commands to this script; +# add them to the per-configuration scripts instead. +# + +# +# Default to auto-start mode. An init script on the microSD card +# can change this to prevent automatic startup of the flight script. +# +set MODE autostart +set USB autoconnect + +# +# Start playing the startup tune +# +tone_alarm start + +# +# Try to mount the microSD card. +# +echo "[init] looking for microSD..." +if mount -t vfat /dev/mmcsd0 /fs/microsd +then + echo "[init] card mounted at /fs/microsd" +else + echo "[init] no microSD card found" +fi + +# +# Look for an init script on the microSD card. +# +# To prevent automatic startup in the current flight mode, +# the script should set MODE to some other value. +# +if [ -f /fs/microsd/etc/rc ] +then + echo "[init] reading /fs/microsd/etc/rc" + sh /fs/microsd/etc/rc +fi +# Also consider rc.txt files +if [ -f /fs/microsd/etc/rc.txt ] +then + echo "[init] reading /fs/microsd/etc/rc.txt" + sh /fs/microsd/etc/rc.txt +fi + +# +# Check for USB host +# +if [ $USB != autoconnect ] +then + echo "[init] not connecting USB" +else + if sercon + then + echo "[init] USB interface connected" + else + echo "[init] No USB connected" + fi +fi + +# if this is an APM build then there will be a rc.APM script +# from an EXTERNAL_SCRIPTS build option +if [ -f /etc/init.d/rc.APM ] +then + echo Running rc.APM + # if APM startup is successful then nsh will exit + sh /etc/init.d/rc.APM +fi -- cgit v1.2.3 From 39d88471896ac46c4aae475f7d6c73e44e7b5f25 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 16 Jul 2013 16:43:11 +0200 Subject: sercon is only used by APM now --- ROMFS/px4fmu_common/init.d/rcS | 33 +++++++++++++++++---------------- 1 file changed, 17 insertions(+), 16 deletions(-) (limited to 'ROMFS/px4fmu_common/init.d/rcS') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 22dec87cb..498c93f28 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -42,30 +42,31 @@ then sh /fs/microsd/etc/rc.txt fi -# -# Check for USB host -# -if [ $USB != autoconnect ] +# if this is an APM build then there will be a rc.APM script +# from an EXTERNAL_SCRIPTS build option +if [ -f /etc/init.d/rc.APM ] then - echo "[init] not connecting USB" -else - if sercon + + # + # Check for USB host + # + if [ $USB != autoconnect ] then - echo "[init] USB interface connected" + echo "[init] not connecting USB" else - if [ -f /dev/ttyACM0 ] - echo "[init] NSH via USB" + if sercon then + echo "[init] USB interface connected" else - echo "[init] No USB connected" + if [ -f /dev/ttyACM0 ] + echo "[init] NSH via USB" + then + else + echo "[init] No USB connected" + fi fi fi -fi -# if this is an APM build then there will be a rc.APM script -# from an EXTERNAL_SCRIPTS build option -if [ -f /etc/init.d/rc.APM ] -then echo Running rc.APM # if APM startup is successful then nsh will exit sh /etc/init.d/rc.APM -- cgit v1.2.3 From b85d74336d62d467b21f98f69020c5db2f21efb0 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 30 Jul 2013 22:46:41 -0700 Subject: Add missing 'fi' at the end of rc script; if you had a microSD card that set MODE to something other than autostart the result was a prompt that ignored your commands. --- ROMFS/px4fmu_common/init.d/rcS | 3 +++ 1 file changed, 3 insertions(+) (limited to 'ROMFS/px4fmu_common/init.d/rcS') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6d141f1b6..ccbae2cbc 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -119,3 +119,6 @@ if param compare SYS_AUTOSTART 31 then sh /etc/init.d/31_io_phantom fi + +# End of autostart +fi -- cgit v1.2.3 From e9b6cfd671c72b75f2bf79bf1031ea8697f430b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 15:48:13 +0200 Subject: Fixed startup order of F330 script --- ROMFS/px4fmu_common/init.d/10_io_f330 | 18 +++++------------- ROMFS/px4fmu_common/init.d/rcS | 13 +++++++++++++ 2 files changed, 18 insertions(+), 13 deletions(-) (limited to 'ROMFS/px4fmu_common/init.d/rcS') diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index b3fb02757..0634d650e 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -44,6 +44,11 @@ param set MAV_TYPE 2 # mavlink start usleep 5000 + +# +# Start the commander (depends on orb, mavlink) +# +commander start # # Start PX4IO interface (depends on orb, commander) @@ -77,11 +82,6 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix # Start the sensors (depends on orb, px4io) # sh /etc/init.d/rc.sensors - -# -# Start the commander (depends on orb, mavlink) -# -commander start # # Start GPS interface (depends on orb) @@ -102,15 +102,7 @@ if [ $BOARD == fmuv1 ] then px4io limit 200 sdlog2 start -r 50 -a -b 16 - if blinkm start - then - blinkm systemstate - fi else px4io limit 400 sdlog2 start -r 200 -a -b 16 - if rgbled start - then - #rgbled systemstate - fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f0ee1a0c6..60cc13611 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -81,6 +81,19 @@ else fi fi +# +# Start system state indicator +# +if rgbled start +then + echo "Using external RGB Led" +else + if blinkm start + then + blinkm systemstate + fi +fi + # # Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) # -- cgit v1.2.3 From 80f38e3dea6b707314b407c7c511c19aa4eade39 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 18 Aug 2013 21:00:47 +0200 Subject: Put console and syslog on UART8, added support to nshterm for proper serial port config --- ROMFS/px4fmu_common/init.d/rcS | 9 +++++++++ makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 2 ++ nuttx-configs/px4fmu-v2/nsh/defconfig | 14 +++++++------- src/systemcmds/nshterm/nshterm.c | 31 ++++++++++++++++++++++++++++--- 5 files changed, 47 insertions(+), 10 deletions(-) (limited to 'ROMFS/px4fmu_common/init.d/rcS') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f0ee1a0c6..e06d90d1c 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -58,6 +58,15 @@ fi if [ $MODE == autostart ] then +# +# Start terminal +# +if sercon +then + echo "USB connected" + nshterm /dev/ttyACM0 & +fi + # # Start the ORB (first app to start) # diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index db13cc197..8f2ade8dc 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -54,6 +54,7 @@ MODULES += systemcmds/reboot MODULES += systemcmds/top MODULES += systemcmds/tests MODULES += systemcmds/config +MODULES += systemcmds/nshterm # # General system control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index cc182e6af..101b49712 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -52,6 +52,8 @@ MODULES += systemcmds/pwm MODULES += systemcmds/reboot MODULES += systemcmds/top MODULES += systemcmds/tests +MODULES += systemcmds/config +MODULES += systemcmds/nshterm # # General system control diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 85bf6dd2f..3e2a48108 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -420,7 +420,7 @@ CONFIG_TASK_NAME_SIZE=24 CONFIG_START_YEAR=1970 CONFIG_START_MONTH=1 CONFIG_START_DAY=1 -# CONFIG_DEV_CONSOLE is not set +CONFIG_DEV_CONSOLE=y # CONFIG_MUTEX_TYPES is not set CONFIG_PRIORITY_INHERITANCE=y CONFIG_SEM_PREALLOCHOLDERS=8 @@ -523,7 +523,7 @@ CONFIG_PIPES=y # CONFIG_POWER is not set # CONFIG_SENSORS is not set CONFIG_SERIAL=y -# CONFIG_DEV_LOWCONSOLE is not set +CONFIG_DEV_LOWCONSOLE=y CONFIG_SERIAL_REMOVABLE=y # CONFIG_16550_UART is not set CONFIG_ARCH_HAVE_UART4=y @@ -542,8 +542,8 @@ CONFIG_SERIAL_NPOLLWAITERS=2 # CONFIG_UART4_SERIAL_CONSOLE is not set # CONFIG_USART6_SERIAL_CONSOLE is not set # CONFIG_UART7_SERIAL_CONSOLE is not set -# CONFIG_UART8_SERIAL_CONSOLE is not set -CONFIG_NO_SERIAL_CONSOLE=y +CONFIG_UART8_SERIAL_CONSOLE=y +# CONFIG_NO_SERIAL_CONSOLE is not set # # USART1 Configuration @@ -650,7 +650,7 @@ CONFIG_USBDEV_MAXPOWER=500 # CONFIG_USBDEV_COMPOSITE is not set # CONFIG_PL2303 is not set CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=y +CONFIG_CDCACM_CONSOLE=n CONFIG_CDCACM_EP0MAXPACKET=64 CONFIG_CDCACM_EPINTIN=1 CONFIG_CDCACM_EPINTIN_FSSIZE=64 @@ -716,10 +716,10 @@ CONFIG_FS_BINFS=y # # System Logging # -# CONFIG_SYSLOG_ENABLE is not set +CONFIG_SYSLOG_ENABLE=y CONFIG_SYSLOG=y CONFIG_SYSLOG_CHAR=y -CONFIG_SYSLOG_DEVPATH="/dev/ttyS0" +CONFIG_SYSLOG_DEVPATH="/dev/ttyS6" # # Graphics Support diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index bde0e7841..2341068a2 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -40,6 +40,7 @@ */ #include +#include #include #include #include @@ -48,6 +49,7 @@ #include #include #include +#include __EXPORT int nshterm_main(int argc, char *argv[]); @@ -61,8 +63,8 @@ nshterm_main(int argc, char *argv[]) uint8_t retries = 0; int fd = -1; while (retries < 5) { - // the retries are to cope with the behaviour of /dev/ttyACM0 - // which may not be ready immediately. + /* the retries are to cope with the behaviour of /dev/ttyACM0 */ + /* which may not be ready immediately. */ fd = open(argv[1], O_RDWR); if (fd != -1) { break; @@ -74,7 +76,30 @@ nshterm_main(int argc, char *argv[]) perror(argv[1]); exit(1); } - // setup standard file descriptors + + /* set up the serial port with output processing */ + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(fd, &uart_config)) < 0) { + warnx("ERROR get termios config %s: %d\n", argv[1], termios_state); + close(fd); + return -1; + } + + /* Set ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag |= (ONLCR | OPOST | OCRNL); + + if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]); + close(fd); + return -1; + } + + /* setup standard file descriptors */ close(0); close(1); close(2); -- cgit v1.2.3 From 95260d453592903bfcd3dba9379db033738d5b89 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 20 Aug 2013 09:36:26 +0200 Subject: Fixed NSH terminal init --- ROMFS/px4fmu_common/init.d/rcS | 9 ++++----- src/systemcmds/nshterm/nshterm.c | 2 +- 2 files changed, 5 insertions(+), 6 deletions(-) (limited to 'ROMFS/px4fmu_common/init.d/rcS') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index bb78b6a65..7f0409519 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -61,11 +61,7 @@ then # # Start terminal # -if sercon -then - echo "USB connected" - nshterm /dev/ttyACM0 & -fi +sercon # # Start the ORB (first app to start) @@ -164,5 +160,8 @@ then sh /etc/init.d/31_io_phantom fi +# Try to get an USB console +nshterm /dev/ttyACM0 & + # End of autostart fi diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 41d108ffc..458bb2259 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -62,7 +62,7 @@ nshterm_main(int argc, char *argv[]) } uint8_t retries = 0; int fd = -1; - while (retries < 5) { + while (retries < 50) { /* the retries are to cope with the behaviour of /dev/ttyACM0 */ /* which may not be ready immediately. */ fd = open(argv[1], O_RDWR); -- cgit v1.2.3 From 5a8dc9c504f70b4ce1b45f91b3bdd9b7126ef0d3 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Mon, 19 Aug 2013 22:58:50 +0200 Subject: Added TBS script --- ROMFS/px4fmu_common/init.d/10_io_f330 | 9 ++- ROMFS/px4fmu_common/init.d/15_io_tbs | 138 ++++++++++++++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 5 ++ 3 files changed, 150 insertions(+), 2 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/15_io_tbs (limited to 'ROMFS/px4fmu_common/init.d/rcS') diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 48636292c..0e6d3f5d5 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -1,12 +1,17 @@ #!nsh # -# Flight startup script for PX4FMU+PX4IO +# Flight startup script for PX4FMU+PX4IO on an F330 quad. # # disable USB and autostart set USB no set MODE custom +# +# Start the ORB (first app to start) +# +uorb start + # # Load default params for this platform # @@ -49,7 +54,7 @@ usleep 5000 # Start the commander (depends on orb, mavlink) # commander start - + # # Start PX4IO interface (depends on orb, commander) # diff --git a/ROMFS/px4fmu_common/init.d/15_io_tbs b/ROMFS/px4fmu_common/init.d/15_io_tbs new file mode 100644 index 000000000..237bb4267 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/15_io_tbs @@ -0,0 +1,138 @@ +#!nsh +# +# Flight startup script for PX4FMU+PX4IO on a Team Blacksheep Discovery quad +# with stock DJI ESCs, motors and props. +# + +# disable USB and autostart +set USB no +set MODE custom + +# +# Start the ORB (first app to start) +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +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 + param set MC_ATT_P 5.0 + param set MC_RCLOSS_THR 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 0.5 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.2 + + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start MAVLink (depends on orb) +# +mavlink start +usleep 5000 + +# +# Start the commander (depends on orb, mavlink) +# +commander start + +# +# Start PX4IO interface (depends on orb, commander) +# +px4io start +pwm -u 400 -m 0xff + +# +# Allow PX4IO to recover from midair restarts. +# This is very unlikely, but quite safe and robust. +px4io recovery + +# +# This sets a PWM right after startup (regardless of safety button) +# +px4io idle 900 900 900 900 + +# +# The values are for spinning motors when armed using DJI ESCs +# +px4io min 1180 1180 1180 1180 + +# +# Upper limits could be higher, this is on the safe side +# +px4io max 1800 1800 1800 1800 + +# +# Load the mixer for a quad with wide arms +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator (depends on orb) +# +attitude_estimator_ekf start + +# +# Start the controllers (depends on orb) +# +multirotor_att_control start + +# +# Disable px4io topic limiting and start logging +# +if [ $BOARD == fmuv1 ] +then + px4io limit 200 + sdlog2 start -r 50 -a -b 16 + if blinkm start + then + blinkm systemstate + fi +else + px4io limit 400 + sdlog2 start -r 200 -a -b 16 + if rgbled start + then + #rgbled systemstate + fi +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 7f0409519..650224cf1 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -150,6 +150,11 @@ then sh /etc/init.d/10_io_f330 fi +if param compare SYS_AUTOSTART 15 +then + sh /etc/init.d/15_io_tbs +fi + if param compare SYS_AUTOSTART 30 then sh /etc/init.d/30_io_camflyer -- cgit v1.2.3 From cfa9054aa4e6accae1fefaad1a13316301ce56fc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 22 Aug 2013 09:25:13 +0200 Subject: Moved to USART1 for the main console, starting a 2nd NSH instance on USB if needed, reworked start scripts to not fall over --- ROMFS/px4fmu_common/init.d/01_fmu_quad_x | 14 ++++---- ROMFS/px4fmu_common/init.d/02_io_quad_x | 2 +- ROMFS/px4fmu_common/init.d/08_ardrone | 25 ++++----------- ROMFS/px4fmu_common/init.d/09_ardrone_flow | 15 +++++---- ROMFS/px4fmu_common/init.d/10_io_f330 | 51 +++++++++++++++--------------- ROMFS/px4fmu_common/init.d/rcS | 47 ++++++++++++++++++++++++++- nuttx-configs/px4fmu-v1/nsh/defconfig | 8 ++--- 7 files changed, 97 insertions(+), 65 deletions(-) (limited to 'ROMFS/px4fmu_common/init.d/rcS') diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x index 58a970eba..c1f3187f9 100644 --- a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x +++ b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x @@ -97,11 +97,9 @@ multirotor_att_control start # Start logging # sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi + +# Try to get an USB console +nshterm /dev/ttyACM0 & + +# Exit, because /dev/ttyS0 is needed for MAVLink +exit diff --git a/ROMFS/px4fmu_common/init.d/02_io_quad_x b/ROMFS/px4fmu_common/init.d/02_io_quad_x index c63e92f6d..49483d14f 100644 --- a/ROMFS/px4fmu_common/init.d/02_io_quad_x +++ b/ROMFS/px4fmu_common/init.d/02_io_quad_x @@ -88,7 +88,7 @@ pwm -u 400 -m 0xff multirotor_att_control start # -# Start logging +# Start logging once armed # sdlog2 start -r 50 -a -b 14 diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone index f55ac2ae3..5bb1491e9 100644 --- a/ROMFS/px4fmu_common/init.d/08_ardrone +++ b/ROMFS/px4fmu_common/init.d/08_ardrone @@ -2,12 +2,6 @@ # # Flight startup script for PX4FMU on PX4IOAR carrier board. # - -# Disable the USB interface -set USB no - -# Disable autostarting other apps -set MODE ardrone echo "[init] doing PX4IOAR startup..." @@ -70,25 +64,14 @@ multirotor_att_control start ardrone_interface start -d /dev/ttyS1 # -# Start logging +# Start logging once armed # -sdlog start -s 10 +sdlog2 start -r 50 -a -b 14 # # Start GPS capture # gps start - -# -# Start system state -# -if blinkm start -then - echo "using BlinkM for state indication" - blinkm systemstate -else - echo "no BlinkM found, OK." -fi # # startup is done; we don't want the shell because we @@ -96,4 +79,8 @@ fi # echo "[init] startup done" +# Try to get an USB console +nshterm /dev/ttyACM0 & + +# Exit, because /dev/ttyS0 is needed for MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow index e7173f6e6..a223ef7aa 100644 --- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow @@ -2,12 +2,6 @@ # # Flight startup script for PX4FMU on PX4IOAR carrier board. # - -# Disable the USB interface -set USB no - -# Disable autostarting other apps -set MODE ardrone echo "[init] doing PX4IOAR startup..." @@ -84,6 +78,11 @@ flow_speed_control start # Fire up the AR.Drone interface. # ardrone_interface start -d /dev/ttyS1 + +# +# Start logging once armed +# +sdlog2 start -r 50 -a -b 14 # # startup is done; we don't want the shell because we @@ -91,4 +90,8 @@ ardrone_interface start -d /dev/ttyS1 # echo "[init] startup done" +# Try to get an USB console +nshterm /dev/ttyACM0 & + +# Exit, because /dev/ttyS0 is needed for MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 0e6d3f5d5..b2fc0c96f 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -2,10 +2,6 @@ # # Flight startup script for PX4FMU+PX4IO on an F330 quad. # - -# disable USB and autostart -set USB no -set MODE custom # # Start the ORB (first app to start) @@ -60,33 +56,36 @@ commander start # if px4io start then + # + # This sets a PWM right after startup (regardless of safety button) + # + px4io idle 900 900 900 900 + pwm -u 400 -m 0xff -else - # SOS - tone_alarm 6 -fi - -# -# Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. -px4io recovery -# -# This sets a PWM right after startup (regardless of safety button) -# -px4io idle 900 900 900 900 + # + # Allow PX4IO to recover from midair restarts. + # this is very unlikely, but quite safe and robust. + px4io recovery -# -# The values are for spinning motors when armed using DJI ESCs -# -px4io min 1200 1200 1200 1200 -# -# Upper limits could be higher, this is on the safe side -# -px4io max 1800 1800 1800 1800 -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + # + # The values are for spinning motors when armed using DJI ESCs + # + px4io min 1200 1200 1200 1200 + + # + # Upper limits could be higher, this is on the safe side + # + px4io max 1800 1800 1800 1800 + + mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +else + # SOS + tone_alarm 6 +fi # # Start the sensors (depends on orb, px4io) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 650224cf1..8674c3c04 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -61,7 +61,13 @@ then # # Start terminal # -sercon +if sercon +then + echo "USB connected" +else + # second attempt + sercon & +fi # # Start the ORB (first app to start) @@ -128,45 +134,84 @@ fi if param compare SYS_AUTOSTART 1 then sh /etc/init.d/01_fmu_quad_x + set MODE custom fi if param compare SYS_AUTOSTART 2 then sh /etc/init.d/02_io_quad_x + set MODE custom fi if param compare SYS_AUTOSTART 8 then sh /etc/init.d/08_ardrone + set MODE custom fi if param compare SYS_AUTOSTART 9 then sh /etc/init.d/09_ardrone_flow + set MODE custom fi if param compare SYS_AUTOSTART 10 then sh /etc/init.d/10_io_f330 + set MODE custom fi if param compare SYS_AUTOSTART 15 then sh /etc/init.d/15_io_tbs + set MODE custom fi if param compare SYS_AUTOSTART 30 then sh /etc/init.d/30_io_camflyer + set MODE custom fi if param compare SYS_AUTOSTART 31 then sh /etc/init.d/31_io_phantom + set MODE custom fi # Try to get an USB console nshterm /dev/ttyACM0 & +# If none of the autostart scripts triggered, get a minimal setup +if [ $MODE == autostart ] +then + # Telemetry port is on both FMU boards ttyS1 + mavlink start -b 57600 -d /dev/ttyS1 + + # Start commander + commander start + + # Start px4io if present + if px4io start + then + echo "PX4IO driver started" + else + if fmu mode_serial + then + echo "FMU driver started" + fi + fi + + # Start sensors + sh /etc/init.d/rc.sensors + + # Start one of the estimators + attitude_estimator_ekf start + + # Start GPS + gps start + +fi + # End of autostart fi diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a7a6725c6..412a29fbe 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -368,7 +368,7 @@ CONFIG_TASK_NAME_SIZE=24 CONFIG_START_YEAR=1970 CONFIG_START_MONTH=1 CONFIG_START_DAY=1 -# CONFIG_DEV_CONSOLE is not set +CONFIG_DEV_CONSOLE=y # CONFIG_MUTEX_TYPES is not set CONFIG_PRIORITY_INHERITANCE=y CONFIG_SEM_PREALLOCHOLDERS=0 @@ -476,11 +476,11 @@ CONFIG_ARCH_HAVE_USART6=y CONFIG_MCU_SERIAL=y CONFIG_STANDARD_SERIAL=y CONFIG_SERIAL_NPOLLWAITERS=2 -# CONFIG_USART1_SERIAL_CONSOLE is not set +CONFIG_USART1_SERIAL_CONSOLE=y # CONFIG_USART2_SERIAL_CONSOLE is not set # CONFIG_UART5_SERIAL_CONSOLE is not set # CONFIG_USART6_SERIAL_CONSOLE is not set -CONFIG_NO_SERIAL_CONSOLE=y +# CONFIG_NO_SERIAL_CONSOLE is not set # # USART1 Configuration @@ -550,7 +550,7 @@ CONFIG_USBDEV_MAXPOWER=500 # CONFIG_USBDEV_COMPOSITE is not set # CONFIG_PL2303 is not set CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=y +CONFIG_CDCACM_CONSOLE=n CONFIG_CDCACM_EP0MAXPACKET=64 CONFIG_CDCACM_EPINTIN=1 CONFIG_CDCACM_EPINTIN_FSSIZE=64 -- cgit v1.2.3 From 85eafa323aec397e4ed5c394f25d48ce6d878f9f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 22 Aug 2013 10:43:19 +0200 Subject: Fix to RC param updates on IO --- ROMFS/px4fmu_common/init.d/rcS | 1 + src/modules/px4iofirmware/registers.c | 8 ++++++-- 2 files changed, 7 insertions(+), 2 deletions(-) (limited to 'ROMFS/px4fmu_common/init.d/rcS') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8674c3c04..6b624b278 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -187,6 +187,7 @@ if [ $MODE == autostart ] then # Telemetry port is on both FMU boards ttyS1 mavlink start -b 57600 -d /dev/ttyS1 + usleep 5000 # Start commander commander start diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 655a0c7a8..9c95fd1c5 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -499,8 +499,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { - /* do not allow a RC config change while outputs armed */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + /** + * do not allow a RC config change while outputs armed + */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || + (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || + (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { break; } -- cgit v1.2.3 From e119bbb0f1161c71b8c2dcbbbc150d40b356c4b1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Aug 2013 16:33:14 +0200 Subject: A lot more on calibration and RC checks. Needs more testing, but no known issues --- ROMFS/px4fmu_common/init.d/rcS | 6 + .../commander/accelerometer_calibration.cpp | 18 ++- src/modules/commander/commander.cpp | 6 + src/modules/commander/mag_calibration.cpp | 10 +- src/modules/sensors/sensor_params.c | 10 +- src/modules/sensors/sensors.cpp | 37 +----- src/modules/systemlib/module.mk | 3 +- src/modules/systemlib/rc_check.c | 148 +++++++++++++++++++++ src/modules/systemlib/rc_check.h | 52 ++++++++ src/systemcmds/preflight_check/preflight_check.c | 97 +------------- 10 files changed, 246 insertions(+), 141 deletions(-) create mode 100644 src/modules/systemlib/rc_check.c create mode 100644 src/modules/systemlib/rc_check.h (limited to 'ROMFS/px4fmu_common/init.d/rcS') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6b624b278..30edf679a 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -182,6 +182,12 @@ fi # Try to get an USB console nshterm /dev/ttyACM0 & +# Start any custom extensions that might be missing +if [ -f /fs/microsd/etc/rc.local ] +then + sh /fs/microsd/etc/rc.local +fi + # If none of the autostart scripts triggered, get a minimal setup if [ $MODE == autostart ] then diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index c2b2e9258..82df7c37d 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -136,6 +136,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo int do_accel_calibration(int mavlink_fd) { /* announce change */ mavlink_log_info(mavlink_fd, "accel calibration started"); + mavlink_log_info(mavlink_fd, "accel cal progress <0> percent"); /* measure and calculate offsets & scales */ float accel_offs[3]; @@ -211,17 +212,28 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + + unsigned done_count = 0; + while (true) { bool done = true; - char str[80]; + char str[60]; int str_ptr; str_ptr = sprintf(str, "keep vehicle still:"); + unsigned old_done_count = done_count; + done_count = 0; for (int i = 0; i < 6; i++) { if (!data_collected[i]) { str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]); done = false; + } else { + done_count++; } } + + if (old_done_count != done_count) + mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); + if (done) break; mavlink_log_info(mavlink_fd, str); @@ -236,8 +248,8 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float continue; } - sprintf(str, "meas started: %s", orientation_strs[orient]); - mavlink_log_info(mavlink_fd, str); + // sprintf(str, + mavlink_log_info(mavlink_fd, "accel meas started: %s", orientation_strs[orient]); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[orient][0], diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3654839fb..e3d314881 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -84,6 +84,7 @@ #include #include #include +#include #include "px4_custom_mode.h" #include "commander_helper.h" @@ -617,6 +618,8 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; + bool rc_calibration_ok = (OK == rc_calibration_check()); + /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); memset(&safety, 0, sizeof(safety)); @@ -727,6 +730,9 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); status_changed = true; + + /* Re-check RC calibration */ + rc_calibration_ok = (OK == rc_calibration_check()); } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 9263c6a15..42f0190c7 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -100,6 +100,8 @@ int do_mag_calibration(int mavlink_fd) close(fd); + mavlink_log_info(mavlink_fd, "mag cal progress <20> percent"); + /* calibrate offsets */ // uint64_t calibration_start = hrt_absolute_time(); @@ -135,9 +137,8 @@ int do_mag_calibration(int mavlink_fd) axis_index++; - char buf[50]; - sprintf(buf, "please rotate around %c", axislabels[axis_index]); - mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter); tune_neutral(); axis_deadline += calibration_interval / 3; @@ -251,6 +252,8 @@ int do_mag_calibration(int mavlink_fd) warnx("Setting Z mag scale failed!\n"); } + mavlink_log_info(mavlink_fd, "mag cal progress <90> percent"); + /* auto-save to EEPROM */ int save_ret = param_save_default(); @@ -274,6 +277,7 @@ int do_mag_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "mag calibration done"); + mavlink_log_info(mavlink_fd, "mag cal progress <100> percent"); return OK; /* third beep by cal end routine */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index b45317dbe..fd2a6318e 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -157,7 +157,6 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000); PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); -PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */ @@ -184,10 +183,7 @@ PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera yaw / azimuth */ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera pitch / tilt */ -PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera trigger */ -PARAM_DEFINE_INT32(RC_MAP_AUX4, 0); /**< default function: camera roll */ -PARAM_DEFINE_INT32(RC_MAP_AUX5, 0); /**< default function: payload drop */ -PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f); -PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f); -PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f); +PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); +PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); +PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ded39c465..e857b1c2f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -305,8 +305,6 @@ private: int board_rotation; int external_mag_rotation; - int rc_type; - int rc_map_roll; int rc_map_pitch; int rc_map_yaw; @@ -342,9 +340,6 @@ private: param_t max[_rc_max_chan_count]; param_t rev[_rc_max_chan_count]; param_t dz[_rc_max_chan_count]; - param_t rc_type; - - param_t rc_demix; param_t gyro_offset[3]; param_t gyro_scale[3]; @@ -566,8 +561,6 @@ Sensors::Sensors() : } - _parameter_handles.rc_type = param_find("RC_TYPE"); - /* mandatory input switched, mapped to channels 1-4 per default */ _parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); @@ -692,11 +685,6 @@ Sensors::parameters_update() if (!rc_valid) warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); - /* remote control type */ - if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) { - warnx("Failed getting remote control type"); - } - /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { warnx("Failed getting roll chan index"); @@ -738,26 +726,11 @@ Sensors::parameters_update() // warnx("Failed getting offboard control mode sw chan index"); // } - if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) { - warnx("Failed getting mode aux 1 index"); - } - - if (param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)) != OK) { - warnx("Failed getting mode aux 2 index"); - } - - if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) { - warnx("Failed getting mode aux 3 index"); - } - - if (param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)) != OK) { - warnx("Failed getting mode aux 4 index"); - } - - if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) { - warnx("Failed getting mode aux 5 index"); - } - + param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)); + param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)); + param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); + param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); + param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)); param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 94c744c03..843cda722 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -48,4 +48,5 @@ SRCS = err.c \ systemlib.c \ airspeed.c \ system_params.c \ - mavlink_log.c + mavlink_log.c \ + rc_check.c diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c new file mode 100644 index 000000000..9d47d8000 --- /dev/null +++ b/src/modules/systemlib/rc_check.c @@ -0,0 +1,148 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rc_check.c + * + * RC calibration check + */ + +#include + +#include +#include + +#include +#include +#include +#include + +int rc_calibration_check(void) { + + char nbuf[20]; + param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, + _parameter_handles_rev, _parameter_handles_dz; + + int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + float param_min, param_max, param_trim, param_rev, param_dz; + + /* first check channel mappings */ + /* check which map param applies */ + // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { + // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); + // /* give system time to flush error message in case there are more */ + // usleep(100000); + // count++; + // } + + + + for (int i = 0; i < RC_CHANNELS_MAX; i++) { + /* should the channel be enabled? */ + uint8_t count = 0; + + /* min values */ + sprintf(nbuf, "RC%d_MIN", i + 1); + _parameter_handles_min = param_find(nbuf); + param_get(_parameter_handles_min, ¶m_min); + + /* trim values */ + sprintf(nbuf, "RC%d_TRIM", i + 1); + _parameter_handles_trim = param_find(nbuf); + param_get(_parameter_handles_trim, ¶m_trim); + + /* max values */ + sprintf(nbuf, "RC%d_MAX", i + 1); + _parameter_handles_max = param_find(nbuf); + param_get(_parameter_handles_max, ¶m_max); + + /* channel reverse */ + sprintf(nbuf, "RC%d_REV", i + 1); + _parameter_handles_rev = param_find(nbuf); + param_get(_parameter_handles_rev, ¶m_rev); + + /* channel deadzone */ + sprintf(nbuf, "RC%d_DZ", i + 1); + _parameter_handles_dz = param_find(nbuf); + param_get(_parameter_handles_dz, ¶m_dz); + + /* assert min..center..max ordering */ + if (param_min < 500) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_max > 2500) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_trim < param_min) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_trim > param_max) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + + /* assert deadzone is sane */ + if (param_dz > 500) { + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + count++; + } + + /* check which map param applies */ + // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { + // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); + // /* give system time to flush error message in case there are more */ + // usleep(100000); + // count++; + // } + + /* sanity checks pass, enable channel */ + if (count) { + mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); + usleep(100000); + } + } +} diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h new file mode 100644 index 000000000..e2238d151 --- /dev/null +++ b/src/modules/systemlib/rc_check.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rc_check.h + * + * RC calibration check + */ + +#pragma once + + __BEGIN_DECLS + +/** + * Check the RC calibration + * + * @return 0 / OK if RC calibration is ok, index + 1 of the first + * channel that failed else (so 1 == first channel failed) + */ +__EXPORT int rc_calibration_check(void); + +__END_DECLS diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index e7d9ce85f..4c19dcffb 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -57,6 +57,7 @@ #include #include +#include __EXPORT int preflight_check_main(int argc, char *argv[]); static int led_toggle(int leds, int led); @@ -139,101 +140,7 @@ int preflight_check_main(int argc, char *argv[]) /* ---- RC CALIBRATION ---- */ - param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, - _parameter_handles_rev, _parameter_handles_dz; - - float param_min, param_max, param_trim, param_rev, param_dz; - - bool rc_ok = true; - char nbuf[20]; - - /* first check channel mappings */ - /* check which map param applies */ - // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { - // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); - // /* give system time to flush error message in case there are more */ - // usleep(100000); - // count++; - // } - - for (int i = 0; i < 12; i++) { - /* should the channel be enabled? */ - uint8_t count = 0; - - /* min values */ - sprintf(nbuf, "RC%d_MIN", i + 1); - _parameter_handles_min = param_find(nbuf); - param_get(_parameter_handles_min, ¶m_min); - - /* trim values */ - sprintf(nbuf, "RC%d_TRIM", i + 1); - _parameter_handles_trim = param_find(nbuf); - param_get(_parameter_handles_trim, ¶m_trim); - - /* max values */ - sprintf(nbuf, "RC%d_MAX", i + 1); - _parameter_handles_max = param_find(nbuf); - param_get(_parameter_handles_max, ¶m_max); - - /* channel reverse */ - sprintf(nbuf, "RC%d_REV", i + 1); - _parameter_handles_rev = param_find(nbuf); - param_get(_parameter_handles_rev, ¶m_rev); - - /* channel deadzone */ - sprintf(nbuf, "RC%d_DZ", i + 1); - _parameter_handles_dz = param_find(nbuf); - param_get(_parameter_handles_dz, ¶m_dz); - - /* assert min..center..max ordering */ - if (param_min < 500) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_max > 2500) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_trim < param_min) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_trim > param_max) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - - /* assert deadzone is sane */ - if (param_dz > 500) { - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - count++; - } - - /* check which map param applies */ - // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { - // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); - // /* give system time to flush error message in case there are more */ - // usleep(100000); - // count++; - // } - - /* sanity checks pass, enable channel */ - if (count) { - mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); - usleep(100000); - rc_ok = false; - } - } + bool rc_ok = (OK == rc_calibration_check()); /* require RC ok to keep system_ok */ system_ok &= rc_ok; -- cgit v1.2.3 From 557d3f22de25a392995f2803363f32fdbc6ce843 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 25 Aug 2013 19:27:11 +0200 Subject: Startup scripts major cleanup --- ROMFS/px4fmu_common/init.d/01_fmu_quad_x | 52 ++--- ROMFS/px4fmu_common/init.d/02_io_quad_x | 88 ++------ ROMFS/px4fmu_common/init.d/08_ardrone | 73 ++----- ROMFS/px4fmu_common/init.d/09_ardrone_flow | 67 +++---- ROMFS/px4fmu_common/init.d/10_io_f330 | 80 ++------ ROMFS/px4fmu_common/init.d/15_io_tbs | 91 +-------- ROMFS/px4fmu_common/init.d/30_io_camflyer | 64 +----- ROMFS/px4fmu_common/init.d/31_io_phantom | 93 ++------- ROMFS/px4fmu_common/init.d/40_io_segway | 82 +------- ROMFS/px4fmu_common/init.d/666_fmu_q_x550 | 64 ++++++ ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 | 116 ----------- ROMFS/px4fmu_common/init.d/rc.io | 23 +++ ROMFS/px4fmu_common/init.d/rc.logging | 7 +- ROMFS/px4fmu_common/init.d/rc.multirotor | 10 - ROMFS/px4fmu_common/init.d/rcS | 287 ++++++++++++++------------- 15 files changed, 366 insertions(+), 831 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/666_fmu_q_x550 delete mode 100644 ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 create mode 100644 ROMFS/px4fmu_common/init.d/rc.io (limited to 'ROMFS/px4fmu_common/init.d/rcS') diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x index 8223b3ea5..f57e4bd68 100644 --- a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x +++ b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x @@ -1,28 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU with PWM outputs. -# - -# disable USB and autostart -set USB no -set MODE custom - -echo "[init] doing PX4FMU Quad startup..." - -# -# Start the ORB -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi + +echo "[init] 01_fmu_quad_x: PX4FMU Quad X with PWM outputs" # # Load default params for this platform @@ -52,11 +30,10 @@ fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 - + # # Start MAVLink # @@ -64,19 +41,24 @@ mavlink start -d /dev/ttyS0 -b 57600 usleep 5000 # -# Start common for all multirotors apps +# Start PWM output # -sh /etc/init.d/rc.multirotor +fmu mode_pwm # -# Start PWM output +# Load mixer # -fmu mode_pwm mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Set PWM output frequency +# pwm -u 400 -m 0xff - -# Try to get an USB console -nshterm /dev/ttyACM0 & +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + # Exit, because /dev/ttyS0 is needed for MAVLink -#exit +exit diff --git a/ROMFS/px4fmu_common/init.d/02_io_quad_x b/ROMFS/px4fmu_common/init.d/02_io_quad_x index 49483d14f..a37c26ad1 100644 --- a/ROMFS/px4fmu_common/init.d/02_io_quad_x +++ b/ROMFS/px4fmu_common/init.d/02_io_quad_x @@ -1,26 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO -# - -# disable USB and autostart -set USB no -set MODE custom - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi + +echo "[init] 02_io_quad_x: PX4FMU+PX4IO Quad X with PWM outputs" # # Load default params for this platform @@ -28,74 +8,40 @@ fi if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + # TODO + param set SYS_AUTOCONFIG 0 param save /fs/microsd/params fi - + # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 # -# Start MAVLink (depends on orb) +# Start MAVLink # mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 - -# -# Start the commander (depends on orb, mavlink) -# -commander start - -# -# Start PX4IO interface (depends on orb, commander) -# -px4io start - -# -# Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. -px4io recovery # -# Disable px4io topic limiting -# -px4io limit 200 - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator (depends on orb) +# Start and configure PX4IO interface # -attitude_estimator_ekf start - +sh /etc/init.d/rc.io + # -# Load mixer and start controllers (depends on px4io) +# Load mixer # mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -pwm -u 400 -m 0xff -multirotor_att_control start - + # -# Start logging once armed +# Set PWM output frequency # -sdlog2 start -r 50 -a -b 14 - +pwm -u 400 -m 0xff + # -# Start system state +# Start common for all multirotors apps # -if blinkm start -then - blinkm systemstate -fi +sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone index 5bb1491e9..eb9f82f77 100644 --- a/ROMFS/px4fmu_common/init.d/08_ardrone +++ b/ROMFS/px4fmu_common/init.d/08_ardrone @@ -1,86 +1,45 @@ #!nsh -# -# Flight startup script for PX4FMU on PX4IOAR carrier board. -# - -echo "[init] doing PX4IOAR startup..." - -# -# Start the ORB -# -uorb start + +echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board" # -# Load microSD params +# Load default params for this platform # -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] +if param compare SYS_AUTOCONFIG 1 then - param load /fs/microsd/params + # Set all params here, then disable autoconfig + # TODO + + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 -# -# Configure PX4FMU for operation with PX4IOAR -# -fmu mode_gpio_serial - -# -# Start the sensors. -# -sh /etc/init.d/rc.sensors - # # Start MAVLink # mavlink start -d /dev/ttyS0 -b 57600 usleep 5000 - -# -# Start the commander. -# -commander start - -# -# Start the attitude estimator -# -attitude_estimator_ekf start - + # -# Fire up the multi rotor attitude controller +# Configure PX4FMU for operation with PX4IOAR # -multirotor_att_control start +fmu mode_gpio_serial # # Fire up the AR.Drone interface. # ardrone_interface start -d /dev/ttyS1 - -# -# Start logging once armed -# -sdlog2 start -r 50 -a -b 14 - -# -# Start GPS capture -# -gps start # -# startup is done; we don't want the shell because we -# use the same UART for telemetry +# Start common for all multirotors apps # -echo "[init] startup done" - -# Try to get an USB console -nshterm /dev/ttyACM0 & - +sh /etc/init.d/rc.multirotor + # Exit, because /dev/ttyS0 is needed for MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow index a223ef7aa..44fbb79b7 100644 --- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow @@ -1,49 +1,47 @@ #!nsh + +echo "[init] 09_ardrone_flow: PX4FMU on PX4IOAR carrier board with PX4FLOW" + # -# Flight startup script for PX4FMU on PX4IOAR carrier board. -# - -echo "[init] doing PX4IOAR startup..." - -# -# Start the ORB -# -uorb start - -# -# Load microSD params +# Load default params for this platform # -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] +if param compare SYS_AUTOCONFIG 1 then - param load /fs/microsd/params + # Set all params here, then disable autoconfig + # TODO + + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 +# +# Start MAVLink and MAVLink Onboard (PX4FLOW Sensor) +# +mavlink start -d /dev/ttyS0 -b 57600 +mavlink_onboard start -d /dev/ttyS3 -b 115200 +usleep 5000 + # # Configure PX4FMU for operation with PX4IOAR # fmu mode_gpio_serial + +# +# Fire up the AR.Drone interface. +# +ardrone_interface start -d /dev/ttyS1 # # Start the sensors. # sh /etc/init.d/rc.sensors -# -# Start MAVLink and MAVLink Onboard (Flow Sensor) -# -mavlink start -d /dev/ttyS0 -b 57600 -mavlink_onboard start -d /dev/ttyS3 -b 115200 -usleep 5000 - # # Start the commander. # @@ -73,25 +71,6 @@ flow_position_control start # Fire up the flow speed controller # flow_speed_control start - -# -# Fire up the AR.Drone interface. -# -ardrone_interface start -d /dev/ttyS1 - -# -# Start logging once armed -# -sdlog2 start -r 50 -a -b 14 -# -# startup is done; we don't want the shell because we -# use the same UART for telemetry -# -echo "[init] startup done" - -# Try to get an USB console -nshterm /dev/ttyACM0 & - # Exit, because /dev/ttyS0 is needed for MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 50842764a..7b6509bf8 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -1,12 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO on an F330 quad. -# -# -# Start the ORB (first app to start) -# -uorb start +echo "[init] 10_io_f330: PX4FMU+PX4IO on a DJI F330 quad frame" # # Load default params for this platform @@ -35,8 +29,7 @@ fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 @@ -47,71 +40,28 @@ mavlink start usleep 5000 # -# Start the commander (depends on orb, mavlink) +# Start and configure PX4IO interface # -commander start +sh /etc/init.d/rc.io # -# Start PX4IO interface (depends on orb, commander) +# Set PWM values for DJI ESCs # -if px4io start -then - # - # This sets a PWM right after startup (regardless of safety button) - # - px4io idle 900 900 900 900 - - pwm -u 400 -m 0xff - - # - # Allow PX4IO to recover from midair restarts. - # this is very unlikely, but quite safe and robust. - px4io recovery - - - - # - # The values are for spinning motors when armed using DJI ESCs - # - px4io min 1200 1200 1200 1200 - - # - # Upper limits could be higher, this is on the safe side - # - px4io max 1800 1800 1800 1800 +px4io idle 900 900 900 900 +px4io min 1200 1200 1200 1200 +px4io max 1800 1800 1800 1800 - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -else - # SOS - tone_alarm 6 -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - # -# Start GPS interface (depends on orb) +# Load mixer # -gps start - +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + # -# Start the attitude estimator (depends on orb) +# Set PWM output frequency # -attitude_estimator_ekf start - -multirotor_att_control start +pwm -u 400 -m 0xff # -# Disable px4io topic limiting and start logging +# Start common for all multirotors apps # -if [ $BOARD == fmuv1 ] -then - px4io limit 200 - sdlog2 start -r 50 -a -b 16 -else - px4io limit 400 - sdlog2 start -r 200 -a -b 16 -fi +sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/15_io_tbs b/ROMFS/px4fmu_common/init.d/15_io_tbs index 237bb4267..b4f063e52 100644 --- a/ROMFS/px4fmu_common/init.d/15_io_tbs +++ b/ROMFS/px4fmu_common/init.d/15_io_tbs @@ -1,27 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO on a Team Blacksheep Discovery quad -# with stock DJI ESCs, motors and props. -# - -# disable USB and autostart -set USB no -set MODE custom -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi +echo "[init] 15_io_tbs: PX4FMU+PX4IO on a Team Blacksheep Discovery quad" # # Load default params for this platform @@ -47,11 +26,10 @@ then param save /fs/microsd/params fi - + # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 @@ -62,77 +40,28 @@ mavlink start usleep 5000 # -# Start the commander (depends on orb, mavlink) -# -commander start - -# -# Start PX4IO interface (depends on orb, commander) -# -px4io start -pwm -u 400 -m 0xff - +# Start and configure PX4IO interface # -# Allow PX4IO to recover from midair restarts. -# This is very unlikely, but quite safe and robust. -px4io recovery +sh /etc/init.d/rc.io # -# This sets a PWM right after startup (regardless of safety button) +# Set PWM values for DJI ESCs # px4io idle 900 900 900 900 - -# -# The values are for spinning motors when armed using DJI ESCs -# px4io min 1180 1180 1180 1180 - -# -# Upper limits could be higher, this is on the safe side -# px4io max 1800 1800 1800 1800 # # Load the mixer for a quad with wide arms # mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator (depends on orb) -# -attitude_estimator_ekf start # -# Start the controllers (depends on orb) +# Set PWM output frequency # -multirotor_att_control start +pwm -u 400 -m 0xff # -# Disable px4io topic limiting and start logging +# Start common for all multirotors apps # -if [ $BOARD == fmuv1 ] -then - px4io limit 200 - sdlog2 start -r 50 -a -b 16 - if blinkm start - then - blinkm systemstate - fi -else - px4io limit 400 - sdlog2 start -r 200 -a -b 16 - if rgbled start - then - #rgbled systemstate - fi -fi +sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index 5090b98a4..c9d5d6632 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -1,26 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO -# - -# disable USB and autostart -set USB no -set MODE custom - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi + +cho "[init] 30_io_camflyer: PX4FMU+PX4IO on Camflyer" # # Load default params for this platform @@ -28,39 +8,18 @@ fi if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + # TODO + param set SYS_AUTOCONFIG 0 param save /fs/microsd/params fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 1 = fixed wing # param set MAV_TYPE 1 -# -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) -# -if [ -f /fs/microsd/px4io.bin ] -then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - then - echo "No newer version, skipping upgrade." - else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log - then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log - else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log - echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" - fi - fi -fi - # # Start MAVLink (depends on orb) # @@ -106,16 +65,3 @@ kalman_demo start # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix control_demo start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 5090b98a4..0deffe3f1 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -1,26 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO -# - -# disable USB and autostart -set USB no -set MODE custom - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi + +echo "[init] 31_io_phantom: PX4FMU+PX4IO on Phantom" # # Load default params for this platform @@ -28,76 +8,50 @@ fi if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + # TODO + param set SYS_AUTOCONFIG 0 param save /fs/microsd/params fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 1 = fixed wing # param set MAV_TYPE 1 - -# -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) -# -if [ -f /fs/microsd/px4io.bin ] -then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - then - echo "No newer version, skipping upgrade." - else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log - then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log - else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log - echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" - fi - fi -fi - + # # Start MAVLink (depends on orb) # mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 - -# -# Start the commander (depends on orb, mavlink) -# -commander start - -# -# Start PX4IO interface (depends on orb, commander) + # -px4io start - +# Start and configure PX4IO interface # -# Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. -px4io recovery +sh /etc/init.d/rc.io # # Set actuator limit to 100 Hz update (50 Hz PWM) px4io limit 100 # -# Start the sensors (depends on orb, px4io) +# Start the commander +# +commander start + +# +# Start the sensors # sh /etc/init.d/rc.sensors # -# Start GPS interface (depends on orb) +# Start GPS interface # gps start # -# Start the attitude estimator (depends on orb) +# Start the attitude estimator # kalman_demo start @@ -106,16 +60,3 @@ kalman_demo start # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix control_demo start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway index 5742d685a..2890f43be 100644 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ b/ROMFS/px4fmu_common/init.d/40_io_segway @@ -1,26 +1,4 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO -# - -# disable USB and autostart -set USB no -set MODE custom - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi # # Load default params for this platform @@ -28,39 +6,18 @@ fi if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + # TODO + param set SYS_AUTOCONFIG 0 param save /fs/microsd/params fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 10 = ground rover # param set MAV_TYPE 10 - -# -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) -# -if [ -f /fs/microsd/px4io.bin ] -then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - then - echo "No newer version, skipping upgrade." - else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log - then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log - else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log - echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" - fi - fi -fi - + # # Start MAVLink (depends on orb) # @@ -68,25 +25,15 @@ mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 # -# Start the commander (depends on orb, mavlink) +# Start and configure PX4IO interface # -commander start +sh /etc/init.d/rc.io # -# Start PX4IO interface (depends on orb, commander) -# -px4io start - +# Start the commander (depends on orb, mavlink) # -# Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. -px4io recovery +commander start -# -# Disable px4io topic limiting -# -px4io limit 200 - # # Start the sensors (depends on orb, px4io) # @@ -107,16 +54,3 @@ attitude_estimator_ekf start # md25 start 3 0x58 segway start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 new file mode 100644 index 000000000..2c8218013 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 @@ -0,0 +1,64 @@ +#!nsh + +echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with PWM outputs" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set MC_ATTRATE_P 0.14 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_D 0.006 + param set MC_ATT_P 5.5 + param set MC_ATT_I 0 + param set MC_ATT_D 0 + param set MC_YAWPOS_D 0 + param set MC_YAWPOS_I 0 + param set MC_YAWPOS_P 0.6 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.08 + param set RC_SCALE_PITCH 1 + param set RC_SCALE_ROLL 1 + param set RC_SCALE_YAW 3 + + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +# +# Start MAVLink +# +mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 + +# +# Start PWM output +# +fmu mode_pwm + +# +# Load mixer +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Set PWM output frequency +# +pwm -u 400 -m 0xff + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +# Exit, because /dev/ttyS0 is needed for MAVLink +exit diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 deleted file mode 100644 index a7ffffaee..000000000 --- a/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 +++ /dev/null @@ -1,116 +0,0 @@ -#!nsh -# -# Flight startup script for PX4FMU with PWM outputs. -# - -# disable USB and autostart -set USB no -set MODE custom - -echo "[init] doing PX4FMU Quad startup 666_fmu_quad_X550..." - -# -# Start the ORB -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set MC_ATTRATE_P 0.14 - param set MC_ATTRATE_I 0 - param set MC_ATTRATE_D 0.006 - param set MC_ATT_P 5.5 - param set MC_ATT_I 0 - param set MC_ATT_D 0 - param set MC_YAWPOS_D 0 - param set MC_YAWPOS_I 0 - param set MC_YAWPOS_P 0.6 - param set MC_YAWRATE_D 0 - param set MC_YAWRATE_I 0 - param set MC_YAWRATE_P 0.08 - param set RC_SCALE_PITCH 1 - param set RC_SCALE_ROLL 1 - param set RC_SCALE_YAW 3 - - param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 2 - -# -# Start MAVLink -# -mavlink start -d /dev/ttyS0 -b 57600 -usleep 5000 - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start the commander. -# -commander start - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator -# -attitude_estimator_ekf start -# -# Start the position estimator -# -position_estimator_inav start - -echo "[init] starting PWM output" -fmu mode_pwm -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -pwm -u 400 -m 0xff - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start position control -# -multirotor_pos_control start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io new file mode 100644 index 000000000..85f00e582 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -0,0 +1,23 @@ +# +# Start PX4IO interface (depends on orb, commander) +# +if px4io start +then + # + # Allow PX4IO to recover from midair restarts. + # this is very unlikely, but quite safe and robust. + px4io recovery + + # + # Disable px4io topic limiting + # + if [ $BOARD == fmuv1 ] + then + px4io limit 200 + else + px4io limit 400 + fi +else + # SOS + tone_alarm 6 +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 09c2d00d1..dc4be8055 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -5,5 +5,10 @@ if [ -d /fs/microsd ] then - sdlog start + if [ $BOARD == fmuv1 ] + then + sdlog2 start -r 50 -a -b 16 + else + sdlog2 start -r 200 -a -b 16 + fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor index 81184f363..73f1b3742 100644 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -37,13 +37,3 @@ multirotor_att_control start # Start position control # multirotor_pos_control start - -# -# Start logging -# -if [ $BOARD == fmuv1 ] -then - sdlog2 start -r 50 -a -b 16 -else - sdlog2 start -r 200 -a -b 16 -fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6b624b278..d92a3ce5e 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -57,162 +57,165 @@ fi if [ $MODE == autostart ] then - -# -# Start terminal -# -if sercon -then - echo "USB connected" -else - # second attempt - sercon & -fi - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -if ramtron start -then - param select /ramtron/params - if [ -f /ramtron/params ] + # + # Start terminal + # + if sercon then - param load /ramtron/params + echo "USB connected" + else + # second attempt + sercon & fi -else - param select /fs/microsd/params - if [ -f /fs/microsd/params ] + + # + # Start the ORB (first app to start) + # + uorb start + + # + # Load microSD params + # + if ramtron start then - param load /fs/microsd/params + param select /ramtron/params + if [ -f /ramtron/params ] + then + param load /ramtron/params + fi + else + param select /fs/microsd/params + if [ -f /fs/microsd/params ] + then + param load /fs/microsd/params + fi fi -fi - -# -# Start system state indicator -# -if rgbled start -then - echo "Using external RGB Led" -else - if blinkm start + + # + # Start system state indicator + # + if rgbled start then - blinkm systemstate + echo "Using external RGB Led" + else + if blinkm start + then + blinkm systemstate + fi fi -fi - -# -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) -# -if [ -f /fs/microsd/px4io.bin ] -then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.cur + + # + # Start logging + # + sh /etc/init.d/rc.logging + + # + # Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) + # + if [ -f /fs/microsd/px4io.bin ] then - echo "No newer version, skipping upgrade." - else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io.log + echo "PX4IO Firmware found. Checking Upgrade.." + if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.cur then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.cur - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io.log + echo "No newer version, skipping upgrade." else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io.log - echo "Failed to upgrade px4io firmware - check if px4io is in bootloader mode" + echo "Loading /fs/microsd/px4io.bin" + if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io.log + then + cp /fs/microsd/px4io.bin /fs/microsd/px4io.cur + echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io.log + else + echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io.log + echo "Failed to upgrade px4io firmware - check if px4io is in bootloader mode" + fi fi fi -fi - -# -# Check if auto-setup from one of the standard scripts is wanted -# SYS_AUTOSTART = 0 means no autostart (default) -# -if param compare SYS_AUTOSTART 1 -then - sh /etc/init.d/01_fmu_quad_x - set MODE custom -fi - -if param compare SYS_AUTOSTART 2 -then - sh /etc/init.d/02_io_quad_x - set MODE custom -fi - -if param compare SYS_AUTOSTART 8 -then - sh /etc/init.d/08_ardrone - set MODE custom -fi - -if param compare SYS_AUTOSTART 9 -then - sh /etc/init.d/09_ardrone_flow - set MODE custom -fi - -if param compare SYS_AUTOSTART 10 -then - sh /etc/init.d/10_io_f330 - set MODE custom -fi - -if param compare SYS_AUTOSTART 15 -then - sh /etc/init.d/15_io_tbs - set MODE custom -fi - -if param compare SYS_AUTOSTART 30 -then - sh /etc/init.d/30_io_camflyer - set MODE custom -fi - -if param compare SYS_AUTOSTART 31 -then - sh /etc/init.d/31_io_phantom - set MODE custom -fi - -# Try to get an USB console -nshterm /dev/ttyACM0 & - -# If none of the autostart scripts triggered, get a minimal setup -if [ $MODE == autostart ] -then - # Telemetry port is on both FMU boards ttyS1 - mavlink start -b 57600 -d /dev/ttyS1 - usleep 5000 - - # Start commander - commander start - - # Start px4io if present - if px4io start + + # + # Check if auto-setup from one of the standard scripts is wanted + # SYS_AUTOSTART = 0 means no autostart (default) + # + if param compare SYS_AUTOSTART 1 then - echo "PX4IO driver started" - else - if fmu mode_serial + sh /etc/init.d/01_fmu_quad_x + set MODE custom + fi + + if param compare SYS_AUTOSTART 2 + then + sh /etc/init.d/02_io_quad_x + set MODE custom + fi + + if param compare SYS_AUTOSTART 8 + then + sh /etc/init.d/08_ardrone + set MODE custom + fi + + if param compare SYS_AUTOSTART 9 + then + sh /etc/init.d/09_ardrone_flow + set MODE custom + fi + + if param compare SYS_AUTOSTART 10 + then + sh /etc/init.d/10_io_f330 + set MODE custom + fi + + if param compare SYS_AUTOSTART 15 + then + sh /etc/init.d/15_io_tbs + set MODE custom + fi + + if param compare SYS_AUTOSTART 30 + then + sh /etc/init.d/30_io_camflyer + set MODE custom + fi + + if param compare SYS_AUTOSTART 31 + then + sh /etc/init.d/31_io_phantom + set MODE custom + fi + + # Try to get an USB console + nshterm /dev/ttyACM0 & + + # If none of the autostart scripts triggered, get a minimal setup + if [ $MODE == autostart ] + then + # Telemetry port is on both FMU boards ttyS1 + mavlink start -b 57600 -d /dev/ttyS1 + usleep 5000 + + # Start commander + commander start + + # Start px4io if present + if px4io start then - echo "FMU driver started" + echo "PX4IO driver started" + else + if fmu mode_serial + then + echo "FMU driver started" + fi fi + + # Start sensors + sh /etc/init.d/rc.sensors + + # Start one of the estimators + attitude_estimator_ekf start + + # Start GPS + gps start + fi - - # Start sensors - sh /etc/init.d/rc.sensors - - # Start one of the estimators - attitude_estimator_ekf start - - # Start GPS - gps start - -fi - # End of autostart fi -- cgit v1.2.3 From dfde02c82507d183fdc16aa2d45cb8d9cdf6ff0e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Aug 2013 11:53:52 +0200 Subject: Startup scripts fixup, fixed unmatched dependencies --- ROMFS/px4fmu_common/init.d/30_io_camflyer | 9 +++++++-- ROMFS/px4fmu_common/init.d/31_io_phantom | 7 ++++++- ROMFS/px4fmu_common/init.d/rc.multirotor | 5 +++++ ROMFS/px4fmu_common/init.d/rcS | 11 +++-------- 4 files changed, 21 insertions(+), 11 deletions(-) (limited to 'ROMFS/px4fmu_common/init.d/rcS') diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index c9d5d6632..6a0bd4da8 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -1,6 +1,6 @@ #!nsh -cho "[init] 30_io_camflyer: PX4FMU+PX4IO on Camflyer" +echo "[init] 30_io_camflyer: PX4FMU+PX4IO on Camflyer" # # Load default params for this platform @@ -49,6 +49,11 @@ px4io limit 100 # Start the sensors (depends on orb, px4io) # sh /etc/init.d/rc.sensors + +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging # # Start GPS interface (depends on orb) @@ -64,4 +69,4 @@ kalman_demo start # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -control_demo start +fw_att_control start diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 0deffe3f1..718313862 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -44,6 +44,11 @@ commander start # Start the sensors # sh /etc/init.d/rc.sensors + +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging # # Start GPS interface @@ -59,4 +64,4 @@ kalman_demo start # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -control_demo start +fw_att_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor index 73f1b3742..e3638e3d1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -8,6 +8,11 @@ # sh /etc/init.d/rc.sensors +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging + # # Start the commander. # diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6a82acdcc..c4abd07d2 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -103,11 +103,9 @@ then blinkm systemstate fi fi - - # - # Start logging - # - sh /etc/init.d/rc.logging + + # Try to get an USB console + nshterm /dev/ttyACM0 & # # Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) @@ -183,9 +181,6 @@ then set MODE custom fi - # Try to get an USB console - nshterm /dev/ttyACM0 & - # Start any custom extensions that might be missing if [ -f /fs/microsd/etc/rc.local ] then -- cgit v1.2.3