From 4cffd99db940a9f0cda7643842ccf17d8a3f1b48 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 8 Jan 2014 20:55:12 +0100 Subject: Major autostart rewrite --- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 66 ++++++-------------------------- 1 file changed, 11 insertions(+), 55 deletions(-) (limited to 'ROMFS/px4fmu_common/init.d/4011_dji_f450') 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 -- cgit v1.2.3 From 532c4c771e3da9d0b371101a056c29d0f417cd09 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 9 Jan 2014 10:05:24 +0100 Subject: Autostart: generic quad, hexa and octo added, WIP --- ROMFS/px4fmu_common/init.d/4001_quad_x_pwm | 17 ++++++ ROMFS/px4fmu_common/init.d/4010_dji_f330 | 26 +++++---- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 32 +++++----- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 27 +++++++++ ROMFS/px4fmu_common/init.d/5001_quad_+_pwm | 17 ++++++ ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm | 17 ++++++ ROMFS/px4fmu_common/init.d/666_fmu_q_x550 | 76 ------------------------ ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm | 17 ++++++ ROMFS/px4fmu_common/init.d/8001_octo_x_pwm | 17 ++++++ ROMFS/px4fmu_common/init.d/9001_octo_+_pwm | 17 ++++++ ROMFS/px4fmu_common/init.d/rc.autostart | 31 +++++----- ROMFS/px4fmu_common/init.d/rc.hexa | 94 ------------------------------ ROMFS/px4fmu_common/init.d/rc.mc_interface | 42 ++++++------- 13 files changed, 196 insertions(+), 234 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/4001_quad_x_pwm create mode 100644 ROMFS/px4fmu_common/init.d/4012_hk_x550 create mode 100644 ROMFS/px4fmu_common/init.d/5001_quad_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/666_fmu_q_x550 create mode 100644 ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/8001_octo_x_pwm create mode 100644 ROMFS/px4fmu_common/init.d/9001_octo_+_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/rc.hexa (limited to 'ROMFS/px4fmu_common/init.d/4011_dji_f450') diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm b/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm new file mode 100644 index 000000000..9bee414dc --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic quadcopter X with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 4 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 58c6d99bb..93edb0005 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -1,24 +1,29 @@ #!nsh +# Maintainers: Anton Babushkin + echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" +# Use generic quad X PWM as base +sh /etc/init.d/4001_quad_x_pwm + if [ $DO_AUTOCONFIG == yes ] then # # 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_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.8 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 MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.05 + param set MC_YAWRATE_D 0.0 param set MPC_TILT_MAX 0.5 param set MPC_THR_MAX 0.8 @@ -37,10 +42,7 @@ then param set MPC_Z_VEL_P 0.20 fi -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 4 -set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 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 ca055dfcb..cc0ddccc8 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -1,30 +1,34 @@ #!nsh +# Maintainers: Lorenz Meier + echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" +# Use generic quad X PWM as base +sh /etc/init.d/4001_quad_x_pwm + if [ $DO_AUTOCONFIG == yes ] then # # 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 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 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_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 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 MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters fi -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 4 -set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 set PWM_MIN 1200 set PWM_MAX 1900 -set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 new file mode 100644 index 000000000..a749b7699 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -0,0 +1,27 @@ +#!nsh + +# Maintainers: Todd Stellanova + +echo "[init] PX4FMU v1, v2 with or without IO on HobbyKing X550" + +# Use generic quad X PWM as base +sh /etc/init.d/4001_quad_x_pwm + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 5.5 + param set MC_ATT_I 0 + param set MC_ATT_D 0 + param set MC_ATTRATE_P 0.14 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_D 0.006 + param set MC_YAWPOS_P 0.6 + param set MC_YAWPOS_I 0 + param set MC_YAWPOS_D 0 + param set MC_YAWRATE_P 0.08 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_D 0 +fi diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm new file mode 100644 index 000000000..7d0748668 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic quadcopter + with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY + +set FRAME_COUNT 4 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm new file mode 100644 index 000000000..3ea5479cb --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic hexacopter X with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 6 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 deleted file mode 100644 index acd8027fb..000000000 --- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 +++ /dev/null @@ -1,76 +0,0 @@ -#!nsh - -echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with or without IO" - -# -# 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 -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 -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1100 -pwm max -c 1234 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm new file mode 100644 index 000000000..f747618b8 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic hexacopter + with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY + +set FRAME_COUNT 4 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm new file mode 100644 index 000000000..f8f459185 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic octocopter X with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 8 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm new file mode 100644 index 000000000..f8bcd13a9 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic octocopter + with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY + +set FRAME_COUNT 8 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 10b7bc424..153fbb66b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -17,53 +17,54 @@ # 11000 .. 11999 Hexa Cox # 12000 .. 12999 Octo Cox -if param compare SYS_AUTOSTART 4008 8 +if param compare SYS_AUTOSTART 4001 +then + sh /etc/init.d/4001_quad_x_pwm +fi + +if param compare SYS_AUTOSTART 4008 then #sh /etc/init.d/4008_ardrone fi -if param compare SYS_AUTOSTART 4009 9 +if param compare SYS_AUTOSTART 4009 then #sh /etc/init.d/4009_ardrone_flow fi -if param compare SYS_AUTOSTART 4010 10 +if param compare SYS_AUTOSTART 4010 then sh /etc/init.d/4010_dji_f330 fi -if param compare SYS_AUTOSTART 4011 11 +if param compare SYS_AUTOSTART 4011 then sh /etc/init.d/4011_dji_f450 fi if param compare SYS_AUTOSTART 4012 then - #sh /etc/init.d/666_fmu_q_x550 + sh /etc/init.d/4012_hk_x550 fi -if param compare SYS_AUTOSTART 6012 12 +if param compare SYS_AUTOSTART 6001 then - #set MIXER /etc/mixers/FMU_hex_x.mix - #sh /etc/init.d/rc.hexa + sh /etc/init.d/6001_hexa_x_pwm fi -if param compare SYS_AUTOSTART 7013 13 +if param compare SYS_AUTOSTART 7001 then - #set MIXER /etc/mixers/FMU_hex_+.mix - #sh /etc/init.d/rc.hexa + sh /etc/init.d/7001_hexa_+_pwm fi if param compare SYS_AUTOSTART 8001 then - #set MIXER /etc/mixers/FMU_octo_x.mix - #sh /etc/init.d/rc.octo + sh /etc/init.d/8001_octo_x_pwm fi if param compare SYS_AUTOSTART 9001 then - #set MIXER /etc/mixers/FMU_octo_+.mix - #sh /etc/init.d/rc.octo + sh /etc/init.d/9001_octo_+_pwm fi if param compare SYS_AUTOSTART 12001 diff --git a/ROMFS/px4fmu_common/init.d/rc.hexa b/ROMFS/px4fmu_common/init.d/rc.hexa deleted file mode 100644 index 097db28e4..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.hexa +++ /dev/null @@ -1,94 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Hex" - -# -# 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.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 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_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - 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 - -# -# Force some key parameters to sane values -# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/ -# 13 = hexarotor -# -param set MAV_TYPE 13 - -set EXIT_ON_END no - -# -# Start and configure PX4IO interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # This is not possible on a hexa - tone_alarm error -fi - -# -# Load mixer -# -mixer load /dev/pwm_output $MIXER - -# -# Set PWM output frequency to 400 Hz -# -pwm rate -a -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 123456 -p 900 -pwm min -c 123456 -p 1100 -pwm max -c 123456 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 2a05012a6..9e3d678bf 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -3,38 +3,34 @@ # Script to set PWM min / max limits and mixer # -# -# Load mixer -# -if [ $FRAME_GEOMETRY == x ] -then - echo "Frame geometry X" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -else - if [ $FRAME_GEOMETRY == w ] - then - echo "Frame geometry W" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - else - echo "Frame geometry +" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix - fi -fi - +echo "Rotors count: $FRAME_COUNT" if [ $FRAME_COUNT == 4 ] then + set FRAME_COUNT_STR quad set OUTPUTS 1234 param set MAV_TYPE 2 -else - if [ $FRAME_COUNT == 6 ] - then +fi +if [ $FRAME_COUNT == 6 ] +then + set FRAME_COUNT_STR hex set OUTPUTS 123456 param set MAV_TYPE 13 - else +fi +if [ $FRAME_COUNT == 8 ] +then + set FRAME_COUNT_STR octo set OUTPUTS 12345678 - fi + param set MAV_TYPE 14 fi +# +# Load mixer +# +echo "Frame geometry: ${FRAME_GEOMETRY}" +set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY_STR}_${FRAME_GEOMETRY}.mix +echo "Loading mixer: ${MIXER}" +mixer load /dev/pwm_output ${MIXER} + # # Set PWM output frequency # -- cgit v1.2.3 From b5d56523bc100d7bf95a6dfbac95c1afc89e345e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 10 Jan 2014 13:18:34 +0100 Subject: Init scripts cleanup, WIP --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 85 ++---- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 98 +++---- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 7 +- ROMFS/px4fmu_common/init.d/2100_mpx_easystar | 58 +--- ROMFS/px4fmu_common/init.d/4001_quad_x_pwm | 17 -- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 10 +- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 12 +- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 8 +- ROMFS/px4fmu_common/init.d/40_io_segway | 51 ---- ROMFS/px4fmu_common/init.d/5001_quad_+_pwm | 17 -- ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm | 17 -- ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm | 17 -- ROMFS/px4fmu_common/init.d/8001_octo_x_pwm | 17 -- ROMFS/px4fmu_common/init.d/800_sdlogger | 53 ---- ROMFS/px4fmu_common/init.d/9001_octo_+_pwm | 17 -- ROMFS/px4fmu_common/init.d/rc.autostart | 154 ++++------- ROMFS/px4fmu_common/init.d/rc.autostart_hil | 34 --- .../init.d/rc.custom_dji_f330_mkblctrl | 113 -------- ROMFS/px4fmu_common/init.d/rc.custom_io_esc | 120 -------- ROMFS/px4fmu_common/init.d/rc.fixedwing | 34 --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 19 ++ ROMFS/px4fmu_common/init.d/rc.fw_interface | 18 ++ ROMFS/px4fmu_common/init.d/rc.mc_apps | 2 +- ROMFS/px4fmu_common/init.d/rc.mc_interface | 70 ++--- ROMFS/px4fmu_common/init.d/rc.octo | 94 ------- ROMFS/px4fmu_common/init.d/rc.standalone | 13 - ROMFS/px4fmu_common/init.d/rc.usb | 33 --- ROMFS/px4fmu_common/init.d/rcS | 306 +++++++++++---------- 28 files changed, 372 insertions(+), 1122 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/4001_quad_x_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/40_io_segway delete mode 100644 ROMFS/px4fmu_common/init.d/5001_quad_+_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/8001_octo_x_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/800_sdlogger delete mode 100644 ROMFS/px4fmu_common/init.d/9001_octo_+_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/rc.autostart_hil delete mode 100644 ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl delete mode 100644 ROMFS/px4fmu_common/init.d/rc.custom_io_esc delete mode 100644 ROMFS/px4fmu_common/init.d/rc.fixedwing create mode 100644 ROMFS/px4fmu_common/init.d/rc.fw_apps create mode 100644 ROMFS/px4fmu_common/init.d/rc.fw_interface delete mode 100644 ROMFS/px4fmu_common/init.d/rc.octo delete mode 100644 ROMFS/px4fmu_common/init.d/rc.standalone (limited to 'ROMFS/px4fmu_common/init.d/4011_dji_f450') diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 81d4b5d57..610d482c1 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -1,74 +1,31 @@ #!nsh +# Maintainers: Anton Babushkin + echo "[init] Team Blacksheep Discovery 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.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 + # + # Default parameters for this platform + # param set MC_ATT_P 5.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.15 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.17 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.006 param set MC_YAWPOS_P 0.5 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.2 - - 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 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 fi -# -# Load the mixer for a quad with wide arms -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1100 -pwm max -c 1234 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor +set FRAME_TYPE mc +set FRAME_GEOMETRY quad_w +set PWM_RATE 400 +set PWM_DISARMED 900 +set PWM_MIN 1100 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index b0f4eda79..de5028052 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -1,73 +1,43 @@ #!nsh +# Maintainers: Anton Babushkin + echo "[init] 3DR Iris Quad" -# -# 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 +# Use generic wide arm quad X PWM as base +sh /etc/init.d/10001_quad_w - param set MC_ATTRATE_D 0.004 - 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 +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # param set MC_ATT_P 9.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.15 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.13 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 param set MC_YAWPOS_P 0.5 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.2 - - 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.0098 - set EXIT_ON_END yes + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 + + 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 - -# -# Load the mixer for a quad with wide arms -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1050 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil index 8732673f7..7d21176f2 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -37,6 +37,7 @@ then param set MPC_Z_VEL_P 0.20 fi -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 4 +set HIL yes + +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_x diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index 97c2d7c90..9bf01c60c 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -1,13 +1,12 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on EasyStar" +echo "[init] EasyStar" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig + # + # Default parameters for this platform + # param set FW_P_D 0 param set FW_P_I 0 param set FW_P_IMAX 15 @@ -31,50 +30,7 @@ then param set FW_L1_PERIOD 16 param set RC_SCALE_ROLL 1.0 param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save fi -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_RET.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix -else - echo "Using /etc/mixers/FMU_RET.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set FRAME_TYPE fw +set FRAME_GEOMETRY RET diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm b/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm deleted file mode 100644 index 9bee414dc..000000000 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic quadcopter X with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 4 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 93edb0005..0cfe68b27 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -2,10 +2,7 @@ # Maintainers: Anton Babushkin -echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" - -# Use generic quad X PWM as base -sh /etc/init.d/4001_quad_x_pwm +echo "[init] DJI F330" if [ $DO_AUTOCONFIG == yes ] then @@ -42,6 +39,11 @@ then param set MPC_Z_VEL_P 0.20 fi +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_x + +set PWM_RATE 400 + # DJI ESC range set PWM_DISARMED 900 set PWM_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index cc0ddccc8..4a0953a2e 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -2,10 +2,7 @@ # Maintainers: Lorenz Meier -echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" - -# Use generic quad X PWM as base -sh /etc/init.d/4001_quad_x_pwm +echo "[init] DJI F450" if [ $DO_AUTOCONFIG == yes ] then @@ -27,7 +24,12 @@ then # TODO add default MPC parameters fi - + +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_x + +set PWM_RATE 400 + # DJI ESC range set PWM_DISARMED 900 set PWM_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index a749b7699..396349d9c 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -4,9 +4,6 @@ echo "[init] PX4FMU v1, v2 with or without IO on HobbyKing X550" -# Use generic quad X PWM as base -sh /etc/init.d/4001_quad_x_pwm - if [ $DO_AUTOCONFIG == yes ] then # @@ -25,3 +22,8 @@ then param set MC_YAWRATE_I 0 param set MC_YAWRATE_D 0 fi + +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_x + +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway deleted file mode 100644 index ad455b440..000000000 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ /dev/null @@ -1,51 +0,0 @@ -#!nsh - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - # TODO - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 10 = ground rover -# -param set MAV_TYPE 10 - -# -# Start MAVLink (depends on orb) -# -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 - -# -# Start and configure PX4IO interface -# -sh /etc/init.d/rc.io - -# -# 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 - -# -# Load mixer and start controllers (depends on px4io) -# -roboclaw start /dev/ttyS2 128 1200 -segway start diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm deleted file mode 100644 index 7d0748668..000000000 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic quadcopter + with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY + -set FRAME_COUNT 4 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm deleted file mode 100644 index 3ea5479cb..000000000 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic hexacopter X with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 6 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm deleted file mode 100644 index f747618b8..000000000 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic hexacopter + with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY + -set FRAME_COUNT 4 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm deleted file mode 100644 index f8f459185..000000000 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic octocopter X with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 8 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/800_sdlogger b/ROMFS/px4fmu_common/init.d/800_sdlogger deleted file mode 100644 index 2d2c3737b..000000000 --- a/ROMFS/px4fmu_common/init.d/800_sdlogger +++ /dev/null @@ -1,53 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 init to log only - -# -# 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 -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 - # Set PWM values for DJI ESCs -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -sh /etc/init.d/rc.sensors - -gps start - -attitude_estimator_ekf start - -position_estimator_inav start - -if [ -d /fs/microsd ] -then - if hw_ver compare PX4FMU_V1 - then - echo "Start sdlog2 at 50Hz" - sdlog2 start -r 50 -e -b 16 - else - echo "Start sdlog2 at 200Hz" - sdlog2 start -r 200 -e -b 16 - fi -fi diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm deleted file mode 100644 index f8bcd13a9..000000000 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic octocopter + with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY + -set FRAME_COUNT 8 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 153fbb66b..9da0135b3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -17,114 +17,62 @@ # 11000 .. 11999 Hexa Cox # 12000 .. 12999 Octo Cox -if param compare SYS_AUTOSTART 4001 -then - sh /etc/init.d/4001_quad_x_pwm -fi - -if param compare SYS_AUTOSTART 4008 -then - #sh /etc/init.d/4008_ardrone -fi - -if param compare SYS_AUTOSTART 4009 -then - #sh /etc/init.d/4009_ardrone_flow -fi - -if param compare SYS_AUTOSTART 4010 -then - sh /etc/init.d/4010_dji_f330 -fi - -if param compare SYS_AUTOSTART 4011 -then - sh /etc/init.d/4011_dji_f450 -fi - -if param compare SYS_AUTOSTART 4012 -then - sh /etc/init.d/4012_hk_x550 -fi - -if param compare SYS_AUTOSTART 6001 -then - sh /etc/init.d/6001_hexa_x_pwm -fi - -if param compare SYS_AUTOSTART 7001 -then - sh /etc/init.d/7001_hexa_+_pwm -fi - -if param compare SYS_AUTOSTART 8001 -then - sh /etc/init.d/8001_octo_x_pwm -fi +# +# Simulation setups +# -if param compare SYS_AUTOSTART 9001 +if param compare SYS_AUTOSTART 1000 then - sh /etc/init.d/9001_octo_+_pwm + #sh /etc/init.d/1000_rc_fw_easystar.hil fi -if param compare SYS_AUTOSTART 12001 +if param compare SYS_AUTOSTART 1001 then - #set MIXER /etc/mixers/FMU_octo_cox.mix - #sh /etc/init.d/rc.octo + sh /etc/init.d/1001_rc_quad.hil fi -if param compare SYS_AUTOSTART 10015 15 +if param compare SYS_AUTOSTART 1002 then - #sh /etc/init.d/10015_tbs_discovery + #sh /etc/init.d/1002_rc_fw_state.hil fi -if param compare SYS_AUTOSTART 10016 16 +if param compare SYS_AUTOSTART 1003 then - #sh /etc/init.d/10016_3dr_iris + #sh /etc/init.d/1003_rc_quad_+.hil fi -if param compare SYS_AUTOSTART 4017 17 +if param compare SYS_AUTOSTART 1004 then - #set MKBLCTRL_MODE no - #set MKBLCTRL_FRAME x - #sh /etc/init.d/rc.custom_dji_f330_mkblctrl + #sh /etc/init.d/1004_rc_fw_Rascal110.hil 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 +# +# Standard plane +# -if param compare SYS_AUTOSTART 4019 19 +if param compare SYS_AUTOSTART 2100 100 then - #set MKBLCTRL_MODE yes - #set MKBLCTRL_FRAME x - #sh /etc/init.d/rc.custom_dji_f330_mkblctrl + #sh /etc/init.d/2100_mpx_easystar + #set MODE custom fi -if param compare SYS_AUTOSTART 5020 20 +if param compare SYS_AUTOSTART 2101 101 then - #set MKBLCTRL_MODE yes - #set MKBLCTRL_FRAME + - #sh /etc/init.d/rc.custom_dji_f330_mkblctrl + #sh /etc/init.d/2101_hk_bixler + #set MODE custom fi -if param compare SYS_AUTOSTART 4021 21 +if param compare SYS_AUTOSTART 2102 102 then - #set FRAME_GEOMETRY x - #set ESC_MAKER afro - #sh /etc/init.d/rc.custom_io_esc + #sh /etc/init.d/2102_3dr_skywalker + #set MODE custom fi -if param compare SYS_AUTOSTART 10022 22 -then - #set FRAME_GEOMETRY w - #sh /etc/init.d/rc.custom_io_esc -fi +# +# Flying wing +# -if param compare SYS_AUTOSTART 3030 30 +if param compare SYS_AUTOSTART 3030 then #sh /etc/init.d/3030_io_camflyer fi @@ -147,35 +95,47 @@ 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 +# +# Quad X +# + +if param compare SYS_AUTOSTART 4008 then - #sh /etc/init.d/40_io_segway - #set MODE custom + #sh /etc/init.d/4008_ardrone fi -if param compare SYS_AUTOSTART 2100 100 +if param compare SYS_AUTOSTART 4009 then - #sh /etc/init.d/2100_mpx_easystar - #set MODE custom + #sh /etc/init.d/4009_ardrone_flow fi -if param compare SYS_AUTOSTART 2101 101 +if param compare SYS_AUTOSTART 4010 then - #sh /etc/init.d/2101_hk_bixler - #set MODE custom + sh /etc/init.d/4010_dji_f330 fi -if param compare SYS_AUTOSTART 2102 102 +if param compare SYS_AUTOSTART 4011 then - #sh /etc/init.d/2102_3dr_skywalker - #set MODE custom + sh /etc/init.d/4011_dji_f450 +fi + +if param compare SYS_AUTOSTART 4012 +then + sh /etc/init.d/4012_hk_x550 fi -if param compare SYS_AUTOSTART 800 +# +# Wide arm / H frame +# + +if param compare SYS_AUTOSTART 10015 then - #sh /etc/init.d/800_sdlogger - #set MODE custom + sh /etc/init.d/10015_tbs_discovery +fi + +if param compare SYS_AUTOSTART 10016 +then + sh /etc/init.d/10016_3dr_iris fi diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart_hil b/ROMFS/px4fmu_common/init.d/rc.autostart_hil deleted file mode 100644 index d5fc5eb08..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.autostart_hil +++ /dev/null @@ -1,34 +0,0 @@ -# -# 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.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl deleted file mode 100644 index 40b2ee68b..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl +++ /dev/null @@ -1,113 +0,0 @@ -#!nsh - -# -# 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.002 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.09 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 6.8 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 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_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - 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 - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -set EXIT_ON_END no - -# -# Start the Mikrokopter ESC driver -# -if [ $MKBLCTRL_MODE == yes ] -then - if [ $MKBLCTRL_FRAME == x ] - then - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing" - mkblctrl -mkmode x - else - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing" - mkblctrl -mkmode + - fi -else - if [ $MKBLCTRL_FRAME == x ] - then - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame" - else - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame" - fi - mkblctrl -fi - -usleep 10000 - -# -# 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 -# -if [ $MKBLCTRL_FRAME == x ] -then - mixer load /dev/mkblctrl /etc/mixers/FMU_quad_x.mix -else - mixer load /dev/mkblctrl /etc/mixers/FMU_quad_+.mix -fi - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc deleted file mode 100644 index 045e41e52..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc +++ /dev/null @@ -1,120 +0,0 @@ -#!nsh - -# -# 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.002 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.09 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 6.8 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 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_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - 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 - -echo "RC script for PX4FMU + PX4IO + PPM-ESCs running" - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -set EXIT_ON_END no - -usleep 10000 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start -d /dev/ttyS1 -b 57600 - usleep 5000 - - sh /etc/init.d/rc.io -else - fmu mode_pwm - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS1 -b 57600 - usleep 5000 - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -if [ $ESC_MAKER = afro ] -then - # Set PWM values for Afro ESCs - pwm disarmed -c 1234 -p 1050 - pwm min -c 1234 -p 1080 - pwm max -c 1234 -p 1860 -else - # Set PWM values for typical ESCs - pwm disarmed -c 1234 -p 900 - pwm min -c 1234 -p 980 - pwm max -c 1234 -p 1800 -fi - -# -# Load mixer -# -if [ $FRAME_GEOMETRY == x ] -then - echo "Frame geometry X" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -else - if [ $FRAME_GEOMETRY == w ] - then - echo "Frame geometry W" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - else - echo "Frame geometry +" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix - fi -fi - -# -# Set PWM output frequency -# -pwm rate -r 400 -c 1234 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi - -echo "Script end" diff --git a/ROMFS/px4fmu_common/init.d/rc.fixedwing b/ROMFS/px4fmu_common/init.d/rc.fixedwing deleted file mode 100644 index f02851006..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.fixedwing +++ /dev/null @@ -1,34 +0,0 @@ -#!nsh -# -# Standard everything needed for fixedwing except mixer, actuator output and mavlink -# - -# -# 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 and position estimator -# -att_pos_estimator_ekf start - -# -# Start attitude controller -# -fw_att_control start - -# -# Start the position controller -# -fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps new file mode 100644 index 000000000..d354fb06f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -0,0 +1,19 @@ +#!nsh +# +# Standard apps for fixed wing +# + +# +# Start the attitude and position estimator +# +att_pos_estimator_ekf start + +# +# Start attitude controller +# +fw_att_control start + +# +# Start the position controller +# +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_interface b/ROMFS/px4fmu_common/init.d/rc.fw_interface new file mode 100644 index 000000000..f9517f422 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.fw_interface @@ -0,0 +1,18 @@ +#!nsh +# +# Script to load fixedwing mixer and set rate/disarmed/min/max values for PWM output +# + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing +# +param set MAV_TYPE 1 + +# +# Load mixer +# +echo "Frame geometry: ${FRAME_GEOMETRY}" +set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY}.mix +echo "Loading mixer: ${MIXER}" +mixer load /dev/pwm_output ${MIXER} diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 16a7a33c6..8b51d57e5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -1,6 +1,6 @@ #!nsh # -# Standard everything needed for multirotors except mixer, actuator output and mavlink +# Standard apps for multirotors # # diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 9e3d678bf..003aee81b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -1,24 +1,25 @@ #!nsh # -# Script to set PWM min / max limits and mixer +# Script to load multicopter mixer and set rate/disarmed/min/max values for PWM output # -echo "Rotors count: $FRAME_COUNT" -if [ $FRAME_COUNT == 4 ] +if [ $FRAME_GEOMETRY == quad_x -o $FRAME_GEOMETRY == quad_+ ] then - set FRAME_COUNT_STR quad set OUTPUTS 1234 param set MAV_TYPE 2 fi -if [ $FRAME_COUNT == 6 ] +if [ $FRAME_GEOMETRY == quad_w -o $FRAME_GEOMETRY == quad_v ] +then + set OUTPUTS 1234 + param set MAV_TYPE 2 +fi +if [ $FRAME_GEOMETRY == hex_x -o $FRAME_GEOMETRY == hex_+ ] then - set FRAME_COUNT_STR hex set OUTPUTS 123456 param set MAV_TYPE 13 fi -if [ $FRAME_COUNT == 8 ] +if [ $FRAME_GEOMETRY == octo_x -o $FRAME_GEOMETRY == octo_+ ] then - set FRAME_COUNT_STR octo set OUTPUTS 12345678 param set MAV_TYPE 14 fi @@ -26,31 +27,34 @@ fi # # Load mixer # -echo "Frame geometry: ${FRAME_GEOMETRY}" -set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY_STR}_${FRAME_GEOMETRY}.mix -echo "Loading mixer: ${MIXER}" -mixer load /dev/pwm_output ${MIXER} +echo "Frame geometry: $FRAME_GEOMETRY" +set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY}.mix +echo "Loading mixer: $MIXER" +mixer load /dev/pwm_output $MIXER -# -# Set PWM output frequency -# -if [ $PWM_RATE != none ] -then - pwm rate -c $OUTPUTS -r $PWM_RATE -fi - -# -# Set disarmed, min and max PWM values -# -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 ] +if [ $OUTPUT_MODE == fmu_pwm -o $OUTPUT_MODE == io_pwm ] then - pwm max -c $OUTPUTS -p $PWM_MAX + # + # Set PWM output frequency + # + if [ $PWM_RATE != none ] + then + pwm rate -c $OUTPUTS -r $PWM_RATE + fi + + # + # Set disarmed, min and max PWM values + # + 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 fi diff --git a/ROMFS/px4fmu_common/init.d/rc.octo b/ROMFS/px4fmu_common/init.d/rc.octo deleted file mode 100644 index ecb12e96e..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.octo +++ /dev/null @@ -1,94 +0,0 @@ -#!nsh - -echo "[init] Octorotor startup" - -# -# 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.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 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_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - 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 - -# -# Force some key parameters to sane values -# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/ -# 14 = octorotor -# -param set MAV_TYPE 14 - -set EXIT_ON_END no - -# -# Start and configure PX4IO interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # This is not possible on an octo - tone_alarm error -fi - -# -# Load mixer -# -mixer load /dev/pwm_output $MIXER - -# -# Set PWM output frequency to 400 Hz -# -pwm rate -a -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 12345678 -p 900 -pwm min -c 12345678 -p 1100 -pwm max -c 12345678 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.standalone b/ROMFS/px4fmu_common/init.d/rc.standalone deleted file mode 100644 index 67e95215b..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.standalone +++ /dev/null @@ -1,13 +0,0 @@ -#!nsh -# -# Flight startup script for PX4FMU standalone configuration. -# - -echo "[init] doing standalone PX4FMU startup..." - -# -# Start the ORB -# -uorb start - -echo "[init] startup done" diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index ccf2cd47e..0cd8a0e04 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -36,39 +36,6 @@ then echo "Commander started" fi -# 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 -if attitude_estimator_ekf status -then - echo "multicopter att filter running" -else - if att_pos_estimator_ekf status - then - echo "fixedwing att filter running" - else - attitude_estimator_ekf start - fi -fi - -# Start GPS -if gps start -then - echo "GPS started" -fi - echo "MAVLink started, exiting shell.." # Exit shell to make it available to MAVLink diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 7d38736de..09d66cf41 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -52,7 +52,7 @@ then echo "[init] USB interface connected" fi - echo "Running rc.APM" + echo "[init] Running rc.APM" # if APM startup is successful then nsh will exit sh /etc/init.d/rc.APM fi @@ -85,9 +85,9 @@ then then if param load /fs/microsd/params then - echo "Parameters loaded" + echo "[init] Parameters loaded" else - echo "Parameter file corrupt - ignoring" + echo "[init] Parameter file corrupt - ignoring" fi fi #fi @@ -97,7 +97,7 @@ then # if rgbled start then - echo "Using external RGB Led" + echo "[init] Using external RGB Led" else if blinkm start then @@ -105,75 +105,10 @@ then 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 - - # - # Set parameters and env variables for selected AUTOSTART (HIL setups) - # - sh /etc/init.d/rc.autostart_hil + # Use FMU PWM output by default + set OUTPUT_MODE fmu_pwm + set IO_PRESENT no - 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 - - # - # 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 # @@ -184,19 +119,17 @@ then set io_file /etc/extras/px4io-v1_default.bin fi - if px4io start - then - echo "PX4IO OK" - echo "PX4IO OK" >> $logfile - fi - if px4io checkcrc $io_file then - echo "PX4IO CRC OK" + echo "[init] PX4IO CRC OK" echo "PX4IO CRC OK" >> $logfile - set USE_IO yes + + set IO_PRESENT yes + + # If PX4IO present, use it as primary PWM output by default + set OUTPUT_MODE io_pwm else - echo "PX4IO CRC failure" + echo "[init] PX4IO CRC failure" echo "PX4IO CRC failure" >> $logfile tone_alarm MBABGP if px4io forceupdate 14662 $io_file @@ -204,12 +137,16 @@ then usleep 500000 if px4io start then - echo "PX4IO restart OK" + echo "[init] PX4IO restart OK" echo "PX4IO restart OK" >> $logfile tone_alarm MSPAA - set USE_IO yes + + set IO_PRESENT yes + + # If PX4IO present, use it as primary PWM output by default + set OUTPUT_MODE io_pwm else - echo "PX4IO restart failed" + echo "[init] PX4IO restart failed" echo "PX4IO restart failed" >> $logfile if hw_ver compare PX4FMU_V2 then @@ -219,18 +156,54 @@ then fi fi else - echo "PX4IO update failed" + echo "[init] PX4IO update failed" echo "PX4IO update failed" >> $logfile - tone_alarm MNGGG + if hw_ver compare PX4FMU_V2 + then + tone_alarm MNGGG + fi fi fi - + + set HIL no + set FRAME_TYPE none + set PWM_RATE none + set PWM_DISARMED none + set PWM_MIN none + set PWM_MAX none + set EXIT_ON_END no + 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 + # # Set parameters and env variables for selected AUTOSTART # sh /etc/init.d/rc.autostart + + # Custom configuration + if [ -f /fs/microsd/etc/rc.conf ] + then + sh /fs/microsd/etc/rc.conf + fi + + if [ $HIL == yes ] + then + set OUTPUT_MODE hil + else + # Try to get an USB console if not in HIL mode + nshterm /dev/ttyACM0 & + fi # # If autoconfig parameter was set, reset it and save parameters @@ -240,66 +213,132 @@ then param set SYS_AUTOCONFIG 0 param save fi - + if [ $MODE == autostart ] then # - # Do common setup depending on env variables + # Start primary output # - if [ $USE_IO == yes ] + if [ $OUTPUT_MODE == io_pwm ] 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 + echo "[init] Use PX4IO PWM as primary output" + if px4io start + then + echo "[init] PX4IO OK" + echo "PX4IO OK" >> $logfile + sh /etc/init.d/rc.io + else + echo "[init] PX4IO start error" + echo "PX4IO start error" >> $logfile + tone_alarm MNGGG + fi + fi + if [ $OUTPUT_MODE == fmu_pwm ] + then + echo "[init] Use PX4FMU PWM as primary output" + fmu mode_pwm + fi + if [ $OUTPUT_MODE == mkblctrl ] + then + echo "[init] Use MKBLCTRL as primary output" + mkblctrl + fi + if [ $OUTPUT_MODE == hil ] + then + echo "[init] Use HIL as primary output" + hil mode_pwm + fi + + # + # Start PX4IO as secondary output if needed + # + if [ $IO_PRESENT == yes -a $OUTPUT_MODE != io_pwm ] + then + echo "[init] Use PX4IO PWM as secondary output" + if px4io start + then + echo "[init] PX4IO OK" + echo "PX4IO OK" >> $logfile + sh /etc/init.d/rc.io + else + echo "[init] PX4IO start error" + echo "PX4IO start error" >> $logfile + tone_alarm MNGGG + fi + fi + + # + # MAVLink + # + if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] + then + # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output 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 + else + # Start MAVLink on default port: ttyS1 + mavlink start + usleep 5000 fi - # Sensors - echo "Start sensors" + # + # Sensors, Logging, GPS + # + echo "[init] Start sensors" sh /etc/init.d/rc.sensors - - # Logging - sh /etc/init.d/rc.logging - - # GPS interface - gps start + + if [ $HIL == no ] + then + echo "[init] Start logging" + sh /etc/init.d/rc.logging + fi + + if [ $HIL == no ] + then + echo "[init] Start GPS" + gps start + fi # # Fixed wing setup # - if [ $FRAME_TYPE == fw ] + if [ $VEHICLE_TYPE == fw ] then - echo "Setup FIXED WING" + echo "[init] Vehicle type: FIXED WING" + + # Load mixer and configure outputs + sh /etc/init.d/rc.fw_interface + + # Start standard fixedwing apps + sh /etc/init.d/rc.mc_apps fi # # Multicopters setup # - if [ $FRAME_TYPE == mc ] + if [ $VEHICLE_TYPE == mc ] then - echo "Setup MULTICOPTER" + echo "[init] Vehicle type: MULTICOPTER" # Load mixer and configure outputs sh /etc/init.d/rc.mc_interface - # Start common multicopter apps + # Start standard multicopter apps sh /etc/init.d/rc.mc_apps fi + + # + # Generic setup (autostart ID not found) + # + if [ $VEHICLE_TYPE == none ] + then + echo "[init] Vehicle type: GENERIC" + attitude_estimator_ekf start + position_estimator_inav start + fi fi # Start any custom extensions @@ -307,39 +346,6 @@ then then sh /fs/microsd/etc/rc.local fi - - # If none of the autostart scripts triggered, get a minimal setup - if [ $MODE == autostart ] - then - # Telemetry port is on both FMU boards ttyS1 - # but the AR.Drone motors can be get 'flashed' - # if starting MAVLink on them - so do not - # start it as default (default link: USB) - - # Start commander - commander start - - # Start px4io if present - if px4io detect - then - px4io start - else - if fmu mode_serial - then - echo "FMU driver (no PWM) 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 if [ $EXIT_ON_END == yes ] then -- cgit v1.2.3 From 6e609845569722367b5d38bc508edb69dfa8d47f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 10 Jan 2014 22:04:56 +0100 Subject: rcS and autostart scripts cleanup, WIP --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 9 +- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 15 +- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 43 ------ ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 46 ++++++ ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 133 +++++------------ ROMFS/px4fmu_common/init.d/4010_dji_f330 | 7 +- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 7 +- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 9 +- ROMFS/px4fmu_common/init.d/rc.autostart | 4 +- ROMFS/px4fmu_common/init.d/rcS | 199 ++++++++++++++----------- 10 files changed, 223 insertions(+), 249 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/1001_rc_quad.hil create mode 100644 ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil (limited to 'ROMFS/px4fmu_common/init.d/4011_dji_f450') diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 610d482c1..24f1099d3 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -1,8 +1,9 @@ #!nsh - -# Maintainers: Anton Babushkin - -echo "[init] Team Blacksheep Discovery Quad" +# +# Team Blacksheep Discovery Quadcopter +# +# Maintainers: Simon Wilks +# if [ $DO_AUTOCONFIG == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index de5028052..b8fc5e606 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -1,11 +1,9 @@ #!nsh - +# +# 3DR Iris Quadcopter +# # Maintainers: Anton Babushkin - -echo "[init] 3DR Iris Quad" - -# Use generic wide arm quad X PWM as base -sh /etc/init.d/10001_quad_w +# if [ $DO_AUTOCONFIG == yes ] then @@ -41,3 +39,8 @@ then param set MPC_Z_VEL_MAX 2 param set MPC_Z_VEL_P 0.20 fi + +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_w + +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil deleted file mode 100644 index 7d21176f2..000000000 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ /dev/null @@ -1,43 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on HIL Quad" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # 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 - -set HIL yes - -set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_x diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil new file mode 100644 index 000000000..c5b92d7d4 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -0,0 +1,46 @@ +#!nsh +# +# HIL Quadcopter X +# +# Maintainers: Anton Babushkin +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # 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 + +set HIL yes + +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_x diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 0cc07ad34..5ec70043a 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -1,105 +1,46 @@ #!nsh # -# USB HIL start +# HIL Quadcopter + # - -echo "[HIL] HILS quadrotor + starting.." - -# -# 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.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/ +# Maintainers: Anton Babushkin # -param set MAV_TYPE 2 -# -# Check if we got an IO -# -if px4io start +if [ $DO_AUTOCONFIG == yes ] then - echo "IO started" -else - fmu mode_serial - echo "FMU started" + # + # 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_+.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 HIL yes +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_+ diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 0cfe68b27..94afe91ae 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -1,8 +1,9 @@ #!nsh - +# +# DJI Flame Wheel F330 Quadcopter +# # Maintainers: Anton Babushkin - -echo "[init] DJI F330" +# if [ $DO_AUTOCONFIG == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 4a0953a2e..21b3088d3 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -1,8 +1,9 @@ #!nsh - +# +# DJI Flame Wheel F450 Quadcopter +# # Maintainers: Lorenz Meier - -echo "[init] DJI F450" +# if [ $DO_AUTOCONFIG == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index 396349d9c..27f73471d 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -1,8 +1,9 @@ #!nsh - +# +# HobbyKing X550 Quadcopter +# # Maintainers: Todd Stellanova - -echo "[init] PX4FMU v1, v2 with or without IO on HobbyKing X550" +# if [ $DO_AUTOCONFIG == yes ] then @@ -21,6 +22,8 @@ then param set MC_YAWRATE_P 0.08 param set MC_YAWRATE_I 0 param set MC_YAWRATE_D 0 + + # TODO add default MPC parameters fi set VEHICLE_TYPE mc diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 9da0135b3..352a916ac 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -28,7 +28,7 @@ fi if param compare SYS_AUTOSTART 1001 then - sh /etc/init.d/1001_rc_quad.hil + sh /etc/init.d/1001_rc_quad_x.hil fi if param compare SYS_AUTOSTART 1002 @@ -38,7 +38,7 @@ fi if param compare SYS_AUTOSTART 1003 then - #sh /etc/init.d/1003_rc_quad_+.hil + sh /etc/init.d/1003_rc_quad_+.hil fi if param compare SYS_AUTOSTART 1004 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 09d66cf41..d6d6ec352 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -13,18 +13,31 @@ set logfile /fs/microsd/bootlog.txt # # Try to mount the microSD card. # -echo "[init] looking for microSD..." +echo "[init] Looking for microSD..." if mount -t vfat /dev/mmcsd0 /fs/microsd then - echo "[init] card mounted at /fs/microsd" + echo "[init] microSD card mounted at /fs/microsd" # Start playing the startup tune tone_alarm start else - echo "[init] no microSD card found" + echo "[init] No microSD card found" # Play SOS tone_alarm error fi +# +# Set default values here, can be overriden in rc.txt from SD card +# +set HIL no +set VEHICLE_TYPE none +set FRAME_GEOMETRY none +set OUTPUT_MODE none +set VEHICLE_TYPE none +set PWM_RATE none +set PWM_DISARMED none +set PWM_MIN none +set PWM_MAX none + # # Look for an init script on the microSD card. # @@ -70,27 +83,18 @@ then uorb start # - # Load microSD params + # Load parameters # - #if ramtron start - #then - # 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 ] + param select /fs/microsd/params + if [ -f /fs/microsd/params ] + then + if param load /fs/microsd/params then - if param load /fs/microsd/params - then - echo "[init] Parameters loaded" - else - echo "[init] Parameter file corrupt - ignoring" - fi + echo "[init] Parameters loaded" + else + echo "[init] Parameter file corrupt - ignoring" fi - #fi + fi # # Start system state indicator @@ -104,13 +108,9 @@ then blinkm systemstate fi fi - - # Use FMU PWM output by default - set OUTPUT_MODE fmu_pwm - set IO_PRESENT no # - # Upgrade PX4IO firmware + # Check if PX4IO present and update firmware if needed # if [ -f /etc/extras/px4io-v2_default.bin ] then @@ -119,15 +119,14 @@ then set io_file /etc/extras/px4io-v1_default.bin fi + set IO_PRESENT no + if px4io checkcrc $io_file then echo "[init] PX4IO CRC OK" echo "PX4IO CRC OK" >> $logfile set IO_PRESENT yes - - # If PX4IO present, use it as primary PWM output by default - set OUTPUT_MODE io_pwm else echo "[init] PX4IO CRC failure" echo "PX4IO CRC failure" >> $logfile @@ -142,9 +141,6 @@ then tone_alarm MSPAA set IO_PRESENT yes - - # If PX4IO present, use it as primary PWM output by default - set OUTPUT_MODE io_pwm else echo "[init] PX4IO restart failed" echo "PX4IO restart failed" >> $logfile @@ -164,16 +160,27 @@ then fi fi fi - - set HIL no - set FRAME_TYPE none - set PWM_RATE none - set PWM_DISARMED none - set PWM_MIN none - set PWM_MAX none + + # + # Set default output if it was not defined in rc.txt + # + if [ $OUTPUT_MODE == none ] + then + if [ $IO_PRESENT == yes ] + then + # If PX4IO present, use it as primary PWM output by default + set OUTPUT_MODE io_pwm + else + # Else use PX4FMU PWM output + set OUTPUT_MODE fmu_pwm + fi + fi set EXIT_ON_END no + # + # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts + # if param compare SYS_AUTOCONFIG 1 then set DO_AUTOCONFIG yes @@ -181,41 +188,35 @@ then set DO_AUTOCONFIG no fi - # - # Start the Commander (needs to be this early for in-air-restarts) - # - commander start - # # Set parameters and env variables for selected AUTOSTART # sh /etc/init.d/rc.autostart - # Custom configuration - if [ -f /fs/microsd/etc/rc.conf ] - then - sh /fs/microsd/etc/rc.conf - fi - - if [ $HIL == yes ] - then - set OUTPUT_MODE hil - else - # Try to get an USB console if not in HIL mode - nshterm /dev/ttyACM0 & - fi - - # - # If autoconfig parameter was set, reset it and save parameters - # - if [ $DO_AUTOCONFIG == yes ] - then - param set SYS_AUTOCONFIG 0 - param save - fi - if [ $MODE == autostart ] then + if [ $HIL == yes ] + then + set OUTPUT_MODE hil + else + # Try to get an USB console if not in HIL mode + nshterm /dev/ttyACM0 & + fi + + # + # If autoconfig parameter was set, reset it and save parameters + # + if [ $DO_AUTOCONFIG == yes ] + then + param set SYS_AUTOCONFIG 0 + param save + fi + + # + # Start the Commander (needs to be this early for in-air-restarts) + # + commander start + # # Start primary output # @@ -224,29 +225,47 @@ then echo "[init] Use PX4IO PWM as primary output" if px4io start then - echo "[init] PX4IO OK" - echo "PX4IO OK" >> $logfile + echo "[init] PX4IO started" sh /etc/init.d/rc.io else echo "[init] PX4IO start error" - echo "PX4IO start error" >> $logfile tone_alarm MNGGG fi fi if [ $OUTPUT_MODE == fmu_pwm ] then echo "[init] Use PX4FMU PWM as primary output" - fmu mode_pwm + if fmu mode_pwm + then + echo "[init] PX4FMU PWM output started" + sh /etc/init.d/rc.io + else + echo "[init] PX4FMU PWM output start error" + tone_alarm MNGGG + fi fi if [ $OUTPUT_MODE == mkblctrl ] then echo "[init] Use MKBLCTRL as primary output" - mkblctrl + if mkblctrl + then + echo "[init] MKBLCTRL started" + else + echo "[init] MKBLCTRL start error" + tone_alarm MNGGG + fi + fi if [ $OUTPUT_MODE == hil ] then echo "[init] Use HIL as primary output" - hil mode_pwm + if hil mode_pwm + then + echo "[init] HIL output started" + else + echo "[init] HIL output error" + tone_alarm MNGGG + fi fi # @@ -257,12 +276,10 @@ then echo "[init] Use PX4IO PWM as secondary output" if px4io start then - echo "[init] PX4IO OK" - echo "PX4IO OK" >> $logfile + echo "[init] PX4IO started" sh /etc/init.d/rc.io else echo "[init] PX4IO start error" - echo "PX4IO start error" >> $logfile tone_alarm MNGGG fi fi @@ -270,18 +287,24 @@ then # # MAVLink # - if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] + if [ $HIL == yes ] then - # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output - mavlink start -d /dev/ttyS0 + mavlink start -b 230400 -d /dev/ttyACM0 usleep 5000 - - # Exit from nsh to free port for mavlink - set EXIT_ON_END yes else - # Start MAVLink on default port: ttyS1 - mavlink start - usleep 5000 + if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] + then + # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output + mavlink start -d /dev/ttyS0 + usleep 5000 + + # Exit from nsh to free port for mavlink + set EXIT_ON_END yes + else + # Start MAVLink on default port: ttyS1 + mavlink start + usleep 5000 + fi fi # @@ -294,10 +317,7 @@ then then echo "[init] Start logging" sh /etc/init.d/rc.logging - fi - - if [ $HIL == no ] - then + echo "[init] Start GPS" gps start fi @@ -336,6 +356,7 @@ then if [ $VEHICLE_TYPE == none ] then echo "[init] Vehicle type: GENERIC" + attitude_estimator_ekf start position_estimator_inav start fi -- cgit v1.2.3 From a8d362de13b23a2523dc69d582c68fe672ac236d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 15 Jan 2014 00:02:57 +0100 Subject: Autostart: use MIXER instead of FRAME_GEOMETRY --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 3 +- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 4 +- ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 2 +- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 2 +- ROMFS/px4fmu_common/init.d/2100_mpx_easystar | 2 +- ROMFS/px4fmu_common/init.d/3031_phantom | 2 +- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 2 +- ROMFS/px4fmu_common/init.d/3033_wingwing | 2 +- ROMFS/px4fmu_common/init.d/3034_fx79 | 2 +- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 4 +- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 4 +- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 3 +- ROMFS/px4fmu_common/init.d/rc.fw_interface | 34 ------- ROMFS/px4fmu_common/init.d/rc.interface | 72 ++++++++++++++ ROMFS/px4fmu_common/init.d/rc.io | 10 +- ROMFS/px4fmu_common/init.d/rc.mc_interface | 77 --------------- ROMFS/px4fmu_common/init.d/rc.sensors | 21 ++-- ROMFS/px4fmu_common/init.d/rcS | 131 +++++++++++++++++-------- 18 files changed, 195 insertions(+), 182 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/rc.fw_interface create mode 100644 ROMFS/px4fmu_common/init.d/rc.interface delete mode 100644 ROMFS/px4fmu_common/init.d/rc.mc_interface (limited to 'ROMFS/px4fmu_common/init.d/4011_dji_f450') diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 53140caff..63798bb3c 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -25,6 +25,7 @@ then fi set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_w +set MIXER FMU_quad_w +set PWM_OUTPUTS 1234 set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 6740e2d94..67c24fab3 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -44,10 +44,10 @@ then fi set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_w +set MIXER FMU_quad_w +set PWM_OUTPUTS 1234 set PWM_RATE 400 - set PWM_DISARMED 900 set PWM_MIN 1000 set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil index c5b92d7d4..8c0797d7c 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -43,4 +43,4 @@ fi set HIL yes set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_x +set MIXER FMU_quad_x diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 5ec70043a..bce3015fc 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -43,4 +43,4 @@ fi set HIL yes set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_+ +set MIXER FMU_quad_+ diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index 43f911a78..0e5bf60d6 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -36,4 +36,4 @@ then fi set VEHICLE_TYPE fw -set FRAME_GEOMETRY RET +set MIXER FMU_RET diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 29af48ec6..4ebbe9c61 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -41,4 +41,4 @@ then fi set VEHICLE_TYPE fw -set FRAME_GEOMETRY Q +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index bda05aeb1..03f282237 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -11,4 +11,4 @@ then fi set VEHICLE_TYPE fw -set FRAME_GEOMETRY Q +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index e0340a8d9..e53763278 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -40,4 +40,4 @@ then fi set VEHICLE_TYPE fw -set FRAME_GEOMETRY Q +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 index c4dab7ba6..8d179d1fd 100644 --- a/ROMFS/px4fmu_common/init.d/3034_fx79 +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -40,4 +40,4 @@ then fi set VEHICLE_TYPE fw -set FRAME_GEOMETRY Q +set MIXER FMU_FX79 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 94afe91ae..ab1db94d0 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -41,10 +41,10 @@ then fi set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_x +set MIXER FMU_quad_x +set PWM_OUTPUTS 1234 set PWM_RATE 400 - # DJI ESC range set PWM_DISARMED 900 set PWM_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 21b3088d3..299771c1d 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -27,10 +27,10 @@ then fi set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_x +set MIXER FMU_quad_x +set PWM_OUTPUTS 1234 set PWM_RATE 400 - # DJI ESC range set PWM_DISARMED 900 set PWM_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index 27f73471d..7e020cf59 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -27,6 +27,7 @@ then fi set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_x +set MIXER FMU_quad_x +set PWM_OUTPUTS 1234 set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_interface b/ROMFS/px4fmu_common/init.d/rc.fw_interface deleted file mode 100644 index 133b65218..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.fw_interface +++ /dev/null @@ -1,34 +0,0 @@ -#!nsh -# -# Script to configure fixedwing control interface -# - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Load mixer -# -echo "[init] Frame geometry: $FRAME_GEOMETRY" -set MIXERSD /fs/microsd/etc/mixers/FMU_$FRAME_GEOMETRY.mix - -#Use the mixer file from the sd-card if it exists -if [ -f $MIXERSD ] -then - set MIXER $MIXERSD -else - set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix -fi - - - -if mixer load /dev/pwm_output $MIXER -then - echo "[init] Mixer loaded: $MIXER" -else - echo "[init] Error loading mixer: $MIXER" - tone_alarm $TUNE_OUT_ERROR -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface new file mode 100644 index 000000000..928d3aeeb --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -0,0 +1,72 @@ +#!nsh +# +# Script to configure control interface +# + +if [ $MIXER != none ] +then + # + # Load mixer + # + set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix + + #Use the mixer file from the SD-card if it exists + if [ -f $MIXERSD ] + then + set MIXER_FILE $MIXERSD + else + set MIXER_FILE /etc/mixers/$MIXER.mix + fi + + if [ $OUTPUT_MODE == mkblctrl ] + then + set OUTPUT_DEV /dev/mkblctrl + else + set OUTPUT_DEV /dev/pwm_output + fi + + if mixer load $OUTPUT_DEV $MIXER_FILE + then + echo "[init] Mixer loaded: $MIXER_FILE" + else + echo "[init] Error loading mixer: $MIXER_FILE" + tone_alarm $TUNE_OUT_ERROR + fi +else + echo "[init] Mixer not defined + tone_alarm $TUNE_OUT_ERROR +fi + +if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] +then + if [ $PWM_OUTPUTS != none ] + then + # + # Set PWM output frequency + # + if [ $PWM_RATE != none ] + then + echo "[init] Set PWM rate: $PWM_RATE" + pwm rate -c $PWM_OUTPUTS -r $PWM_RATE + fi + + # + # Set disarmed, min and max PWM values + # + if [ $PWM_DISARMED != none ] + then + echo "[init] Set PWM disarmed: $PWM_DISARMED" + pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED + fi + if [ $PWM_MIN != none ] + then + echo "[init] Set PWM min: $PWM_MIN" + pwm min -c $PWM_OUTPUTS -p $PWM_MIN + fi + if [ $PWM_MAX != none ] + then + echo "[init] Set PWM max: $PWM_MAX" + pwm max -c $PWM_OUTPUTS -p $PWM_MAX + fi + fi +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index 5a010cc9b..c9d964f8e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -9,11 +9,13 @@ px4io recovery # -# Adjust px4io topic limiting +# Adjust PX4IO update rate limit # +set PX4IO_LIMIT 400 if hw_ver compare PX4FMU_V1 then - px4io limit 200 -else - px4io limit 400 + set PX4IO_LIMIT 200 fi + +echo "[init] Set PX4IO update rate limit: $PX4IO_LIMIT Hz" +px4io limit $PX4IO_LIMIT diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface deleted file mode 100644 index 6e4e4ed31..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ /dev/null @@ -1,77 +0,0 @@ -#!nsh -# -# Script to configure multicopter control interface -# - -if [ $FRAME_GEOMETRY == quad_x -o $FRAME_GEOMETRY == quad_+ ] -then - set OUTPUTS 1234 - param set MAV_TYPE 2 -fi -if [ $FRAME_GEOMETRY == quad_w -o $FRAME_GEOMETRY == quad_v ] -then - set OUTPUTS 1234 - param set MAV_TYPE 2 -fi -if [ $FRAME_GEOMETRY == hex_x -o $FRAME_GEOMETRY == hex_+ ] -then - set OUTPUTS 123456 - param set MAV_TYPE 13 -fi -if [ $FRAME_GEOMETRY == octo_x -o $FRAME_GEOMETRY == octo_+ ] -then - set OUTPUTS 12345678 - param set MAV_TYPE 14 -fi - -# -# Load mixer -# -echo "[init] Frame geometry: $FRAME_GEOMETRY" -set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix - -if [ $OUTPUT_MODE == mkblctrl ] -then - set OUTPUT_DEV /dev/mkblctrl -else - set OUTPUT_DEV /dev/pwm_output -fi - -if mixer load $OUTPUT_DEV $MIXER -then - echo "[init] Mixer loaded: $MIXER" -else - echo "[init] Error loading mixer: $MIXER" - tone_alarm $TUNE_OUT_ERROR -fi - -if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] -then - # - # Set PWM output frequency - # - if [ $PWM_RATE != none ] - then - echo "[init] Set PWM rate: $PWM_RATE" - pwm rate -c $OUTPUTS -r $PWM_RATE - fi - - # - # Set disarmed, min and max PWM values - # - if [ $PWM_DISARMED != none ] - then - echo "[init] Set PWM disarmed: $PWM_DISARMED" - pwm disarmed -c $OUTPUTS -p $PWM_DISARMED - fi - if [ $PWM_MIN != none ] - then - echo "[init] Set PWM min: $PWM_MIN" - pwm min -c $OUTPUTS -p $PWM_MIN - fi - if [ $PWM_MAX != none ] - then - echo "[init] Set PWM max: $PWM_MAX" - pwm max -c $OUTPUTS -p $PWM_MAX - fi -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index a2517135f..badbf92c3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -10,39 +10,42 @@ ms5611 start adc start -# mag might be external +# Mag might be external if hmc5883 start then - echo "using HMC5883" + echo "[init] Using HMC5883" fi if mpu6000 start then - echo "using MPU6000" + echo "[init] Using MPU6000" fi if l3gd20 start then - echo "using L3GD20(H)" + echo "[init] Using L3GD20(H)" fi -if lsm303d start +if hw_ver compare PX4FMU_V2 then - echo "using LSM303D" + if lsm303d start + then + echo "[init] Using LSM303D" + fi fi # Start airspeed sensors if meas_airspeed start then - echo "using MEAS airspeed sensor" + echo "[init] Using MEAS airspeed sensor" else if ets_airspeed start then - echo "using ETS airspeed sensor (bus 3)" + echo "[init] Using ETS airspeed sensor (bus 3)" else if ets_airspeed start -b 1 then - echo "Using ETS airspeed sensor (bus 1)" + echo "[init] Using ETS airspeed sensor (bus 1)" fi fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 441d99ecf..92121ac17 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -14,6 +14,7 @@ set CONFIG_FILE /fs/microsd/etc/config.txt set EXTRAS_FILE /fs/microsd/etc/extras.txt set TUNE_OUT_ERROR ML<> $LOG_FILE tone_alarm MLL32CP8MB @@ -193,17 +193,17 @@ then usleep 500000 if px4io checkcrc $IO_FILE then - echo "[init] PX4IO CRC OK after updating" + echo "[init] PX4IO CRC OK, update successful" echo "PX4IO CRC OK after updating" >> $LOG_FILE tone_alarm MLL8CDE set IO_PRESENT yes else - echo "[init] PX4IO update failed" + echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE fi else - echo "[init] PX4IO update failed" + echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE fi fi @@ -220,16 +220,27 @@ then # if [ $OUTPUT_MODE == none ] then - if [ $IO_PRESENT == yes ] + if [ $USE_IO == yes ] then - # If PX4IO present, use it as primary PWM output by default set OUTPUT_MODE io else - # Else use PX4FMU PWM output set OUTPUT_MODE fmu fi fi + if [ $OUTPUT_MODE == io -a $IO_PRESENT != yes ] + then + # Need IO for output but it not present, disable output + set OUTPUT_MODE none + echo "[init] ERROR: PX4IO not found, disabling output" + + # Avoid using ttyS0 for MAVLink on FMUv1 + if hw_ver compare PX4FMU_V1 + then + set FMU_MODE serial + fi + fi + if [ $HIL == yes ] then set OUTPUT_MODE hil @@ -256,7 +267,7 @@ then echo "[init] PX4IO started" sh /etc/init.d/rc.io else - echo "[init] PX4IO start error" + echo "[init] ERROR: PX4IO start failed" tone_alarm $TUNE_OUT_ERROR fi fi @@ -267,7 +278,7 @@ then then echo "[init] FMU mode_$FMU_MODE started" else - echo "[init] FMU mode_$FMU_MODE start error" + echo "[init] ERROR: FMU mode_$FMU_MODE start failed" tone_alarm $TUNE_OUT_ERROR fi @@ -300,7 +311,7 @@ then then echo "[init] MKBLCTRL started" else - echo "[init] MKBLCTRL start error" + echo "[init] ERROR: MKBLCTRL start failed" tone_alarm $TUNE_OUT_ERROR fi @@ -312,7 +323,7 @@ then then echo "[init] HIL output started" else - echo "[init] HIL output error" + echo "[init] ERROR: HIL output start failed" tone_alarm $TUNE_OUT_ERROR fi fi @@ -329,7 +340,7 @@ then echo "[init] PX4IO started" sh /etc/init.d/rc.io else - echo "[init] PX4IO start error" + echo "[init] ERROR: PX4IO start failed" tone_alarm $TUNE_OUT_ERROR fi fi @@ -340,13 +351,17 @@ then then echo "[init] FMU mode_$FMU_MODE started" else - echo "[init] FMU mode_$FMU_MODE start error" + echo "[init] ERROR: FMU mode_$FMU_MODE start failed" tone_alarm $TUNE_OUT_ERROR fi if hw_ver compare PX4FMU_V1 then - if [ $FMU_MODE == pwm -o $FMU_MODE == gpio -o $FMU_MODE == pwm_gpio ] + if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] + then + set TTYS1_BUSY yes + fi + if [ $FMU_MODE == pwm_gpio ] then set TTYS1_BUSY yes fi @@ -401,14 +416,22 @@ then then echo "[init] Vehicle type: FIXED WING" - if [ $FRAME_GEOMETRY == none ] + if [ $MIXER == none ] then - # Set default frame geometry for fixed wing - set FRAME_GEOMETRY AERT + # Set default mixer for fixed wing if not defined + set MIXER FMU_AERT fi + if [ $MAV_TYPE == none ] + then + # Use MAV_TYPE = 1 (fixed wing) if not defined + set MAV_TYPE 1 + fi + + param set MAV_TYPE $MAV_TYPE + # Load mixer and configure outputs - sh /etc/init.d/rc.fw_interface + sh /etc/init.d/rc.interface # Start standard fixedwing apps sh /etc/init.d/rc.fw_apps @@ -420,15 +443,37 @@ then if [ $VEHICLE_TYPE == mc ] then echo "[init] Vehicle type: MULTICOPTER" - - if [ $FRAME_GEOMETRY == none ] + + if [ $MIXER == none ] then - # Set default frame geometry for multicopter - set FRAME_GEOMETRY quad_x + # Set default mixer for multicopter if not defined + set MIXER quad_x + fi + + if [ $MAV_TYPE == none ] + then + # Use MAV_TYPE = 2 (quadcopter) if not defined + set MAV_TYPE 2 + + # Use mixer to detect vehicle type + if [ $MIXER == FMU_hex_x -o $MIXER == FMU_hex_+ ] + then + param set MAV_TYPE 13 + fi + if [ $MIXER == FMU_octo_x -o $MIXER == FMU_octo_+ ] + then + param set MAV_TYPE 14 + fi + if [ $MIXER == FMU_octo_cox ] + then + param set MAV_TYPE 14 + fi fi + + param set MAV_TYPE $MAV_TYPE # Load mixer and configure outputs - sh /etc/init.d/rc.mc_interface + sh /etc/init.d/rc.interface # Start standard multicopter apps sh /etc/init.d/rc.mc_apps @@ -440,9 +485,9 @@ then if [ $VEHICLE_TYPE == none ] then echo "[init] Vehicle type: GENERIC" - - attitude_estimator_ekf start - position_estimator_inav start + + # Load mixer and configure outputs + sh /etc/init.d/rc.interface fi # Start any custom addons -- cgit v1.2.3