diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 48636292c2..0e6d3f5d54 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 0000000000..237bb4267c --- /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 7f04095194..650224cf17 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