diff options
Diffstat (limited to 'ROMFS')
-rw-r--r-- | ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 138 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/4010_dji_f330 | 67 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/4011_dji_f450 | 66 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.autostart | 180 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.autostart_hil | 34 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.io | 32 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.mc_apps (renamed from ROMFS/px4fmu_common/init.d/rc.multirotor) | 15 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.mc_interface | 23 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rcS | 383 |
9 files changed, 453 insertions, 485 deletions
diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil index 25ea25ae8..8732673f7 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -1,108 +1,42 @@ #!nsh -# -# USB HIL start -# -echo "[HIL] HILS quadrotor starting.." +echo "[init] PX4FMU v1, v2 with or without IO on HIL Quad" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.0 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.05 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 3.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.1 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.05 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.5 - param set MPC_THR_MIN 0.1 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# 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 we got an IO -# -if px4io start -then - echo "IO started" -else - if hw_ver compare PX4FMU_V1 - then - fmu mode_serial - echo "FMU started" - fi + # + # Default parameters for this platform + # + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.8 + param set MPC_THR_MIN 0.2 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 fi -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Start position estimator -# -position_estimator_inav start - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start position control -# -multirotor_pos_control start - -echo "[HIL] setup done, running" - +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 4 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 7054210e2..58c6d99bb 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -2,30 +2,27 @@ echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 + # + # Default parameters for this platform + # param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 + param set MPC_THR_MAX 0.8 + param set MPC_THR_MIN 0.2 param set MPC_XY_D 0 param set MPC_XY_P 0.5 param set MPC_XY_VEL_D 0 @@ -38,32 +35,12 @@ then param set MPC_Z_VEL_I 0.1 param set MPC_Z_VEL_MAX 2 param set MPC_Z_VEL_P 0.20 - - param save fi -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -sh /etc/init.d/rc.mc_interface - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 4 +set PWM_RATE 400 +set PWM_MIN 1200 +set PWM_MAX 1900 +set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index a1d253191..ca055dfcb 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -2,14 +2,11 @@ echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - + # + # Default parameters for this platform + # param set MC_ATTRATE_D 0.004 param set MC_ATTRATE_I 0.0 param set MC_ATTRATE_P 0.12 @@ -22,53 +19,12 @@ then param set MC_YAWRATE_D 0.005 param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_P 0.3 - - param save fi -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals (for DJI ESCs) -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1200 -pwm max -c 1234 -p 1800 - -# -# Start common multirotor apps -# -sh /etc/init.d/rc.multirotor +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 4 +set PWM_RATE 400 +set PWM_MIN 1200 +set PWM_MAX 1900 +set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart new file mode 100644 index 000000000..10b7bc424 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -0,0 +1,180 @@ +# +# Check if auto-setup from one of the standard scripts is wanted +# SYS_AUTOSTART = 0 means no autostart (default) +# +# AUTOSTART PARTITION: +# 0 .. 999 Reserved (historical) +# 1000 .. 1999 Simulation setups +# 2000 .. 2999 Standard planes +# 3000 .. 3999 Flying wing +# 4000 .. 4999 Quad X +# 5000 .. 5999 Quad + +# 6000 .. 6999 Hexa X +# 7000 .. 7999 Hexa + +# 8000 .. 8999 Octo X +# 9000 .. 9999 Octo + +# 10000 .. 10999 Wide arm / H frame +# 11000 .. 11999 Hexa Cox +# 12000 .. 12999 Octo Cox + +if param compare SYS_AUTOSTART 4008 8 +then + #sh /etc/init.d/4008_ardrone +fi + +if param compare SYS_AUTOSTART 4009 9 +then + #sh /etc/init.d/4009_ardrone_flow +fi + +if param compare SYS_AUTOSTART 4010 10 +then + sh /etc/init.d/4010_dji_f330 +fi + +if param compare SYS_AUTOSTART 4011 11 +then + sh /etc/init.d/4011_dji_f450 +fi + +if param compare SYS_AUTOSTART 4012 +then + #sh /etc/init.d/666_fmu_q_x550 +fi + +if param compare SYS_AUTOSTART 6012 12 +then + #set MIXER /etc/mixers/FMU_hex_x.mix + #sh /etc/init.d/rc.hexa +fi + +if param compare SYS_AUTOSTART 7013 13 +then + #set MIXER /etc/mixers/FMU_hex_+.mix + #sh /etc/init.d/rc.hexa +fi + +if param compare SYS_AUTOSTART 8001 +then + #set MIXER /etc/mixers/FMU_octo_x.mix + #sh /etc/init.d/rc.octo +fi + +if param compare SYS_AUTOSTART 9001 +then + #set MIXER /etc/mixers/FMU_octo_+.mix + #sh /etc/init.d/rc.octo +fi + +if param compare SYS_AUTOSTART 12001 +then + #set MIXER /etc/mixers/FMU_octo_cox.mix + #sh /etc/init.d/rc.octo +fi + +if param compare SYS_AUTOSTART 10015 15 +then + #sh /etc/init.d/10015_tbs_discovery +fi + +if param compare SYS_AUTOSTART 10016 16 +then + #sh /etc/init.d/10016_3dr_iris +fi + +if param compare SYS_AUTOSTART 4017 17 +then + #set MKBLCTRL_MODE no + #set MKBLCTRL_FRAME x + #sh /etc/init.d/rc.custom_dji_f330_mkblctrl +fi + +if param compare SYS_AUTOSTART 5018 18 +then + #set MKBLCTRL_MODE no + #set MKBLCTRL_FRAME + + #sh /etc/init.d/rc.custom_dji_f330_mkblctrl +fi + +if param compare SYS_AUTOSTART 4019 19 +then + #set MKBLCTRL_MODE yes + #set MKBLCTRL_FRAME x + #sh /etc/init.d/rc.custom_dji_f330_mkblctrl +fi + +if param compare SYS_AUTOSTART 5020 20 +then + #set MKBLCTRL_MODE yes + #set MKBLCTRL_FRAME + + #sh /etc/init.d/rc.custom_dji_f330_mkblctrl +fi + +if param compare SYS_AUTOSTART 4021 21 +then + #set FRAME_GEOMETRY x + #set ESC_MAKER afro + #sh /etc/init.d/rc.custom_io_esc +fi + +if param compare SYS_AUTOSTART 10022 22 +then + #set FRAME_GEOMETRY w + #sh /etc/init.d/rc.custom_io_esc +fi + +if param compare SYS_AUTOSTART 3030 30 +then + #sh /etc/init.d/3030_io_camflyer +fi + +if param compare SYS_AUTOSTART 3031 31 +then + #sh /etc/init.d/3031_io_phantom +fi + +if param compare SYS_AUTOSTART 3032 32 +then + #sh /etc/init.d/3032_skywalker_x5 +fi + +if param compare SYS_AUTOSTART 3033 33 +then + #sh /etc/init.d/3033_io_wingwing +fi + +if param compare SYS_AUTOSTART 3034 34 +then + #sh /etc/init.d/3034_io_fx79 + #set MODE custom +fi + +if param compare SYS_AUTOSTART 40 +then + #sh /etc/init.d/40_io_segway + #set MODE custom +fi + +if param compare SYS_AUTOSTART 2100 100 +then + #sh /etc/init.d/2100_mpx_easystar + #set MODE custom +fi + +if param compare SYS_AUTOSTART 2101 101 +then + #sh /etc/init.d/2101_hk_bixler + #set MODE custom +fi + +if param compare SYS_AUTOSTART 2102 102 +then + #sh /etc/init.d/2102_3dr_skywalker + #set MODE custom +fi + +if param compare SYS_AUTOSTART 800 +then + #sh /etc/init.d/800_sdlogger + #set MODE custom +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart_hil b/ROMFS/px4fmu_common/init.d/rc.autostart_hil new file mode 100644 index 000000000..d5fc5eb08 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.autostart_hil @@ -0,0 +1,34 @@ +# +# Check if auto-setup from one of the standard scripts for HIL is wanted +# + +if param compare SYS_AUTOSTART 1000 +then + #sh /etc/init.d/1000_rc_fw_easystar.hil + set MODE hil +fi + +if param compare SYS_AUTOSTART 1001 +then + sh /etc/init.d/1001_rc_quad.hil + set MODE hil +fi + +if param compare SYS_AUTOSTART 1002 +then + #sh /etc/init.d/1002_rc_fw_state.hil + set MODE hil +fi + +if param compare SYS_AUTOSTART 1003 +then + #sh /etc/init.d/1003_rc_quad_+.hil + set MODE hil +fi + +if param compare SYS_AUTOSTART 1004 +then + #sh /etc/init.d/1004_rc_fw_Rascal110.hil + set MODE hil +fi + diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index 24784610c..5a010cc9b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -1,23 +1,19 @@ # -# Start PX4IO interface (depends on orb, commander) +# Init PX4IO interface # -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 hw_ver compare PX4FMU_V1 - then - px4io limit 200 - else - px4io limit 400 - fi +# +# Allow PX4IO to recover from midair restarts. +# this is very unlikely, but quite safe and robust. +# +px4io recovery + +# +# Adjust px4io topic limiting +# +if hw_ver compare PX4FMU_V1 +then + px4io limit 200 else - # SOS - tone_alarm error + px4io limit 400 fi diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.mc_apps index bc550ac5a..16a7a33c6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -4,21 +4,6 @@ # # -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# # Start the attitude estimator # attitude_estimator_ekf start diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 6bb2e84ec..2a05012a6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -35,15 +35,26 @@ else fi fi - # # Set PWM output frequency # -pwm rate -c $OUTPUTS -r $PWM_RATE +if [ $PWM_RATE != none ] +then + pwm rate -c $OUTPUTS -r $PWM_RATE +fi # -# Set disarmed, min and max PWM signals (for DJI ESCs) +# Set disarmed, min and max PWM values # -pwm disarmed -c $OUTPUTS -p $PWM_DISARMED -pwm min -c $OUTPUTS -p $PWM_MIN -pwm max -c $OUTPUTS -p $PWM_MAX +if [ $PWM_DISARMED != none ] +then + pwm disarmed -c $OUTPUTS -p $PWM_DISARMED +fi +if [ $PWM_MIN != none ] +then + pwm min -c $OUTPUTS -p $PWM_MIN +fi +if [ $PWM_MAX != none ] +then + pwm max -c $OUTPUTS -p $PWM_MAX +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8801d1126..4dd9af407 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -60,12 +60,9 @@ fi if [ $MODE == autostart ] then # - # Start terminal + # Start CDC/ACM serial driver # - if sercon - then - echo "USB connected" - fi + sercon # # Start the ORB (first app to start) @@ -107,52 +104,79 @@ then blinkm systemstate fi fi + + set USE_IO no + set FRAME_TYPE none + set PWM_RATE none + set PWM_DISARMED none + set PWM_MIN none + set PWM_MAX none + + if param compare SYS_AUTOCONFIG 1 + then + set DO_AUTOCONFIG yes + else + set DO_AUTOCONFIG no + fi # # Start the Commander (needs to be this early for in-air-restarts) # commander start - - if param compare SYS_AUTOSTART 1000 - then - sh /etc/init.d/1000_rc_fw_easystar.hil - set MODE custom - fi - - if param compare SYS_AUTOSTART 1001 - then - sh /etc/init.d/1001_rc_quad.hil - set MODE custom - fi - if param compare SYS_AUTOSTART 1002 - then - sh /etc/init.d/1002_rc_fw_state.hil - set MODE custom - fi + # + # Set parameters and env variables for selected AUTOSTART (HIL setups) + # + sh /etc/init.d/rc.autostart_hil - if param compare SYS_AUTOSTART 1003 - then - sh /etc/init.d/1003_rc_quad_+.hil - set MODE custom - fi - - if param compare SYS_AUTOSTART 1004 - then - sh /etc/init.d/1004_rc_fw_Rascal110.hil - set MODE custom - fi + if [ $MODE == hil ] + then + # + # Do common HIL setup depending on env variables + # + # Allow USB some time to come up + sleep 1 + + # Start MAVLink on USB port + mavlink start -b 230400 -d /dev/ttyACM0 + usleep 5000 + + # Create a fake HIL /dev/pwm_output interface + hil mode_pwm + + # Sensors + echo "Start sensors" + sh /etc/init.d/rc.sensors + + # + # Fixed wing setup + # + if [ $FRAME_TYPE == fw ] + then + echo "Setup FIXED WING" + fi - if [ $MODE != custom ] - then - # Try to get an USB console + # + # Multicopters setup + # + if [ $FRAME_TYPE == mc ] + then + echo "Setup MULTICOPTER" + + # Load mixer and configure outputs + sh /etc/init.d/rc.mc_interface + + # Start common multicopter apps + sh /etc/init.d/rc.mc_apps + fi + else + # Try to get an USB console if not in HIL mode nshterm /dev/ttyACM0 & fi - + # # Upgrade PX4IO firmware # - if [ -f /etc/extras/px4io-v2_default.bin ] then set io_file /etc/extras/px4io-v2_default.bin @@ -170,6 +194,7 @@ then then echo "PX4IO CRC OK" echo "PX4IO CRC OK" >> $logfile + set USE_IO yes else echo "PX4IO CRC failure" echo "PX4IO CRC failure" >> $logfile @@ -182,6 +207,7 @@ then echo "PX4IO restart OK" echo "PX4IO restart OK" >> $logfile tone_alarm MSPAA + set USE_IO yes else echo "PX4IO restart failed" echo "PX4IO restart failed" >> $logfile @@ -200,222 +226,91 @@ then fi set EXIT_ON_END no + set USE_LOGGING yes + set USE_GPS yes # - # Check if auto-setup from one of the standard scripts is wanted - # SYS_AUTOSTART = 0 means no autostart (default) + # Set parameters and env variables for selected AUTOSTART # - # AUTOSTART PARTITION: - # 0 .. 999 Reserved (historical) - # 1000 .. 1999 Simulation setups - # 2000 .. 2999 Standard planes - # 3000 .. 3999 Flying wing - # 4000 .. 4999 Quad X - # 5000 .. 5999 Quad + - # 6000 .. 6999 Hexa X - # 7000 .. 7999 Hexa + - # 8000 .. 8999 Octo X - # 9000 .. 9999 Octo + - # 10000 .. 10999 Wide arm / H frame - # 11000 .. 11999 Hexa Cox - # 12000 .. 12999 Octo Cox - - if param compare SYS_AUTOSTART 4008 8 - then - sh /etc/init.d/4008_ardrone - set MODE custom - fi - - if param compare SYS_AUTOSTART 4009 9 - then - sh /etc/init.d/4009_ardrone_flow - set MODE custom - fi - - if param compare SYS_AUTOSTART 4010 10 - then - set FRAME_GEOMETRY x - set FRAME_COUNT 4 - set PWM_MIN 1200 - set PWM_MAX 1900 - set PWM_DISARMED 900 - sh /etc/init.d/4010_dji_f330 - set MODE custom - fi - - if param compare SYS_AUTOSTART 4011 11 - then - sh /etc/init.d/4011_dji_f450 - set MODE custom - fi - - if param compare SYS_AUTOSTART 4012 - then - sh /etc/init.d/666_fmu_q_x550 - set MODE custom - fi - - if param compare SYS_AUTOSTART 6012 12 - then - set MIXER /etc/mixers/FMU_hex_x.mix - sh /etc/init.d/rc.hexa - set MODE custom - fi - - if param compare SYS_AUTOSTART 7013 13 - then - set MIXER /etc/mixers/FMU_hex_+.mix - sh /etc/init.d/rc.hexa - set MODE custom - fi - - if param compare SYS_AUTOSTART 8001 - then - set MIXER /etc/mixers/FMU_octo_x.mix - sh /etc/init.d/rc.octo - set MODE custom - fi - - if param compare SYS_AUTOSTART 9001 - then - set MIXER /etc/mixers/FMU_octo_+.mix - sh /etc/init.d/rc.octo - set MODE custom - fi - - if param compare SYS_AUTOSTART 12001 - then - set MIXER /etc/mixers/FMU_octo_cox.mix - sh /etc/init.d/rc.octo - set MODE custom - fi + sh /etc/init.d/rc.autostart - if param compare SYS_AUTOSTART 10015 15 - then - sh /etc/init.d/10015_tbs_discovery - set MODE custom - fi - - if param compare SYS_AUTOSTART 10016 16 - then - sh /etc/init.d/10016_3dr_iris - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame - if param compare SYS_AUTOSTART 4017 17 - then - set MKBLCTRL_MODE no - set MKBLCTRL_FRAME x - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame - if param compare SYS_AUTOSTART 5018 18 - then - set MKBLCTRL_MODE no - set MKBLCTRL_FRAME + - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing - if param compare SYS_AUTOSTART 4019 19 - then - set MKBLCTRL_MODE yes - set MKBLCTRL_FRAME x - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing - if param compare SYS_AUTOSTART 5020 20 - then - set MKBLCTRL_MODE yes - set MKBLCTRL_FRAME + - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 4021 21 - then - set FRAME_GEOMETRY x - set ESC_MAKER afro - sh /etc/init.d/rc.custom_io_esc - set MODE custom - fi - - # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 10022 22 - then - set FRAME_GEOMETRY w - sh /etc/init.d/rc.custom_io_esc - set MODE custom - fi - - if param compare SYS_AUTOSTART 3030 30 - then - sh /etc/init.d/3030_io_camflyer - set MODE custom - fi - - if param compare SYS_AUTOSTART 3031 31 + # + # If autoconfig parameter was set, reset it and save parameters + # + if [ $DO_AUTOCONFIG == yes ] then - sh /etc/init.d/3031_io_phantom - set MODE custom + param set SYS_AUTOCONFIG 0 + param save fi - if param compare SYS_AUTOSTART 3032 32 - then - sh /etc/init.d/3032_skywalker_x5 - set MODE custom - fi - - if param compare SYS_AUTOSTART 3033 33 - then - sh /etc/init.d/3033_io_wingwing - set MODE custom - fi - - if param compare SYS_AUTOSTART 3034 34 - then - sh /etc/init.d/3034_io_fx79 - set MODE custom - fi - - if param compare SYS_AUTOSTART 40 - then - sh /etc/init.d/40_io_segway - set MODE custom - fi + if [ $MODE == autostart ] + then + # + # Do common setup depending on env variables + # + if [ $USE_IO == yes ] + then + echo "Use IO" + + # Start MAVLink on default port: ttyS1 + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io + else + echo "Don't use IO" + + # Start MAVLink on ttyS0 + mavlink start -d /dev/ttyS0 + usleep 5000 + + # Configure FMU for PWM outputs + fmu mode_pwm + + # Exit from nsh to free port for mavlink + set EXIT_ON_END yes + fi + + # Sensors + echo "Start sensors" + sh /etc/init.d/rc.sensors + + # Logging + if [ $USE_LOGGING == yes ] + then + sh /etc/init.d/rc.logging + fi + + # GPS interface + if [ $USE_GPS == yes ] + then + gps start + fi + + # + # Fixed wing setup + # + if [ $FRAME_TYPE == fw ] + then + echo "Setup FIXED WING" + fi - if param compare SYS_AUTOSTART 2100 100 - then - sh /etc/init.d/2100_mpx_easystar - set MODE custom - fi - - if param compare SYS_AUTOSTART 2101 101 - then - sh /etc/init.d/2101_hk_bixler - set MODE custom + # + # Multicopters setup + # + if [ $FRAME_TYPE == mc ] + then + echo "Setup MULTICOPTER" + + # Load mixer and configure outputs + sh /etc/init.d/rc.mc_interface + + # Start common multicopter apps + sh /etc/init.d/rc.mc_apps + fi fi - if param compare SYS_AUTOSTART 2102 102 - then - sh /etc/init.d/2102_3dr_skywalker - set MODE custom - fi - - if param compare SYS_AUTOSTART 800 - then - sh /etc/init.d/800_sdlogger - set MODE custom - fi - - # Start any custom extensions that might be missing + # Start any custom extensions if [ -f /fs/microsd/etc/rc.local ] then sh /fs/microsd/etc/rc.local |