diff options
author | sjwilks <sjwilks@gmail.com> | 2013-07-19 03:42:51 -0700 |
---|---|---|
committer | sjwilks <sjwilks@gmail.com> | 2013-07-19 03:42:51 -0700 |
commit | a8ac56b9e5eb8c1501ea592b4417aa8becd7236c (patch) | |
tree | 2a9c9cefec98d31bb7f8d8541fc595ee2560231e /ROMFS | |
parent | 7106565e94ab683b3b9d4b6182bb123e018cbb8b (diff) | |
parent | 4d88b56e38cfcef91890ec3baec16fbda41cee75 (diff) | |
download | px4-firmware-a8ac56b9e5eb8c1501ea592b4417aa8becd7236c.tar.gz px4-firmware-a8ac56b9e5eb8c1501ea592b4417aa8becd7236c.tar.bz2 px4-firmware-a8ac56b9e5eb8c1501ea592b4417aa8becd7236c.zip |
Merge pull request #338 from PX4/autostart
Implemented new, simple system boot config and sane default value system
Diffstat (limited to 'ROMFS')
-rw-r--r-- | ROMFS/px4fmu_common/init.d/01_fmu_quad_x | 107 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/02_io_quad_x (renamed from ROMFS/px4fmu_common/init.d/rc.IO_QUAD) | 32 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/08_ardrone (renamed from ROMFS/px4fmu_common/init.d/rc.PX4IOAR) | 0 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/09_ardrone_flow (renamed from ROMFS/px4fmu_common/init.d/rc.PX4IOAR_PX4FLOW_example) | 0 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/10_io_f330 | 139 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/30_io_camflyer (renamed from ROMFS/px4fmu_common/init.d/rc.PX4IO) | 26 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/31_io_phantom | 121 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.FMU_quad_x | 67 | ||||
-rwxr-xr-x | ROMFS/px4fmu_common/init.d/rcS | 39 |
9 files changed, 450 insertions, 81 deletions
diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x new file mode 100644 index 000000000..58a970eba --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x @@ -0,0 +1,107 @@ +#!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 + +# +# 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 + +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 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_QUAD b/ROMFS/px4fmu_common/init.d/02_io_quad_x index 5f2de0d7e..131abf8c4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.IO_QUAD +++ b/ROMFS/px4fmu_common/init.d/02_io_quad_x @@ -1,8 +1,11 @@ #!nsh +# +# Flight startup script for PX4FMU+PX4IO +# -# Disable USB and autostart +# disable USB and autostart set USB no -set MODE quad +set MODE custom # # Start the ORB (first app to start) @@ -18,6 +21,16 @@ 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 save /fs/microsd/params +fi # # Force some key parameters to sane values @@ -68,6 +81,11 @@ 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) @@ -87,21 +105,19 @@ attitude_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # -mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +pwm -u 400 -m 0xff multirotor_att_control start # # Start logging # -#sdlog start -s 4 +sdlog2 start -r 50 -a -b 14 # # Start system state # if blinkm start then - echo "using BlinkM for state indication" blinkm systemstate -else - echo "no BlinkM found, OK." -fi
\ No newline at end of file +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR b/ROMFS/px4fmu_common/init.d/08_ardrone index f55ac2ae3..f55ac2ae3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR +++ b/ROMFS/px4fmu_common/init.d/08_ardrone diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR_PX4FLOW_example b/ROMFS/px4fmu_common/init.d/09_ardrone_flow index e7173f6e6..e7173f6e6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR_PX4FLOW_example +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 new file mode 100644 index 000000000..4107fab4f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -0,0 +1,139 @@ +#!nsh +# +# Flight startup script for PX4FMU+PX4IO +# + +# disable USB and autostart +set USB no +set MODE custom + +# +# 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.007 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.13 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 7.0 + param set MC_POS_P 0.1 + param set MC_RCLOSS_THR 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.5 + param set MC_YAWPOS_P 1.0 + 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 + +# +# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) +# +if [ -f /fs/microsd/px4io2.bin ] +then + echo "PX4IO Firmware found. Checking Upgrade.." + if cmp /fs/microsd/px4io2.bin /fs/microsd/px4io2.cur + then + echo "No newer version, skipping upgrade." + else + echo "Loading /fs/microsd/px4io2.bin" + if px4io update /fs/microsd/px4io2.bin > /fs/microsd/px4io2.log + then + cp /fs/microsd/px4io2.bin /fs/microsd/px4io2.cur + echo "Flashed /fs/microsd/px4io2.bin OK" >> /fs/microsd/px4io2.log + else + echo "Failed flashing /fs/microsd/px4io2.bin" >> /fs/microsd/px4io2.log + echo "Failed to upgrade PX4IO2 firmware - check if PX4IO2 is in bootloader mode" + fi + fi +fi + +# +# Start MAVLink (depends on orb) +# +mavlink start +usleep 5000 + +# +# 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 + +# +# Disable px4io topic limiting +# +px4io limit 200 + +# +# 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 1200 1200 1200 1200 + +# +# Upper limits could be higher, this is on the safe side +# +px4io max 1800 1800 1800 1800 + +# +# 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) +# +gps start + +# +# Start the attitude estimator (depends on orb) +# +attitude_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +multirotor_att_control start + +# +# Start logging +# +sdlog2 start -r 20 -a -b 14 + +# +# Start system state +# +if blinkm start +then + blinkm systemstate +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IO b/ROMFS/px4fmu_common/init.d/30_io_camflyer index 925a5703e..5090b98a4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.PX4IO +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -1,8 +1,11 @@ #!nsh +# +# Flight startup script for PX4FMU+PX4IO +# -# Disable USB and autostart +# disable USB and autostart set USB no -set MODE camflyer +set MODE custom # # Start the ORB (first app to start) @@ -18,6 +21,16 @@ 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 save /fs/microsd/params +fi # # Force some key parameters to sane values @@ -68,6 +81,10 @@ px4io start # Allow PX4IO to recover from midair restarts. # this is very unlikely, but quite safe and robust. px4io recovery + +# +# Set actuator limit to 100 Hz update (50 Hz PWM) +px4io limit 100 # # Start the sensors (depends on orb, px4io) @@ -93,15 +110,12 @@ control_demo start # # Start logging # -#sdlog start -s 4 +sdlog2 start -r 50 -a -b 14 # # Start system state # if blinkm start then - echo "using BlinkM for state indication" blinkm systemstate -else - echo "no BlinkM found, OK." fi diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom new file mode 100644 index 000000000..5090b98a4 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -0,0 +1,121 @@ +#!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 +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + 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 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 + +# +# Allow PX4IO to recover from midair restarts. +# this is very unlikely, but quite safe and robust. +px4io recovery + +# +# Set actuator limit to 100 Hz update (50 Hz PWM) +px4io limit 100 + +# +# 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) +# +kalman_demo start + +# +# Load mixer and start controllers (depends on px4io) +# +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/rc.FMU_quad_x b/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x deleted file mode 100644 index 980197d68..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x +++ /dev/null @@ -1,67 +0,0 @@ -#!nsh -# -# Flight startup script for PX4FMU with PWM outputs. -# - -# Disable the USB interface -set USB no - -# Disable autostarting other apps -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 - -# -# 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 the attitude estimator -# -attitude_estimator_ekf start - -echo "[init] starting PWM output" -fmu mode_pwm -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Start attitude control -# -multirotor_att_control start - -echo "[init] startup done, exiting" -exit
\ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index bbd86a474..b22591f3c 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -82,3 +82,42 @@ else 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 +fi + +if param compare SYS_AUTOSTART 2 +then + sh /etc/init.d/02_io_quad_x +fi + +if param compare SYS_AUTOSTART 8 +then + sh /etc/init.d/08_ardrone +fi + +if param compare SYS_AUTOSTART 9 +then + sh /etc/init.d/09_ardrone_flow +fi + +if param compare SYS_AUTOSTART 10 +then + sh /etc/init.d/10_io_f330 +fi + +if param compare SYS_AUTOSTART 30 +then + sh /etc/init.d/30_io_camflyer +fi + +if param compare SYS_AUTOSTART 31 +then + sh /etc/init.d/31_io_phantom +fi |