From 501c5ff49f2ba05d21cfe10775c1ebc3bc6af2c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Dec 2013 22:18:07 +0100 Subject: Cleaned up startup, should be completely compatible, but allows clean QGC indices --- ROMFS/px4fmu_common/init.d/#!nsh | 4 ++ ROMFS/px4fmu_common/init.d/08_ardrone | 57 --------------- ROMFS/px4fmu_common/init.d/09_ardrone_flow | 88 ----------------------- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 74 ++++++++++++++++++++ ROMFS/px4fmu_common/init.d/10016_3dr_iris | 73 +++++++++++++++++++ ROMFS/px4fmu_common/init.d/100_mpx_easystar | 87 ----------------------- ROMFS/px4fmu_common/init.d/101_hk_bixler | 87 ----------------------- ROMFS/px4fmu_common/init.d/102_3dr_skywalker | 89 ----------------------- ROMFS/px4fmu_common/init.d/10_dji_f330 | 97 -------------------------- ROMFS/px4fmu_common/init.d/11_dji_f450 | 83 ---------------------- ROMFS/px4fmu_common/init.d/15_tbs_discovery | 81 --------------------- ROMFS/px4fmu_common/init.d/16_3dr_iris | 80 --------------------- ROMFS/px4fmu_common/init.d/2100_mpx_easystar | 80 +++++++++++++++++++++ ROMFS/px4fmu_common/init.d/2101_hk_bixler | 80 +++++++++++++++++++++ ROMFS/px4fmu_common/init.d/2102_3dr_skywalker | 82 ++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/3030_io_camflyer | 58 +++++++++++++++ ROMFS/px4fmu_common/init.d/3031_io_phantom | 85 ++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 58 +++++++++++++++ ROMFS/px4fmu_common/init.d/3033_io_wingwing | 84 ++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/3034_io_fx79 | 84 ++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/30_io_camflyer | 65 ----------------- ROMFS/px4fmu_common/init.d/31_io_phantom | 92 ------------------------ ROMFS/px4fmu_common/init.d/32_skywalker_x5 | 65 ----------------- ROMFS/px4fmu_common/init.d/33_io_wingwing | 91 ------------------------ ROMFS/px4fmu_common/init.d/34_io_fx79 | 91 ------------------------ ROMFS/px4fmu_common/init.d/4008_ardrone | 57 +++++++++++++++ ROMFS/px4fmu_common/init.d/4009_ardrone_flow | 88 +++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/4010_dji_f330 | 69 ++++++++++++++++++ ROMFS/px4fmu_common/init.d/4011_dji_f450 | 76 ++++++++++++++++++++ ROMFS/px4fmu_common/init.d/666_fmu_q_x550 | 7 -- ROMFS/px4fmu_common/init.d/800_sdlogger | 7 -- ROMFS/px4fmu_common/init.d/rcS | 96 +++++++++++++++---------- 32 files changed, 1112 insertions(+), 1203 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/#!nsh delete mode 100644 ROMFS/px4fmu_common/init.d/08_ardrone delete mode 100644 ROMFS/px4fmu_common/init.d/09_ardrone_flow create mode 100644 ROMFS/px4fmu_common/init.d/10015_tbs_discovery create mode 100644 ROMFS/px4fmu_common/init.d/10016_3dr_iris delete mode 100644 ROMFS/px4fmu_common/init.d/100_mpx_easystar delete mode 100644 ROMFS/px4fmu_common/init.d/101_hk_bixler delete mode 100644 ROMFS/px4fmu_common/init.d/102_3dr_skywalker delete mode 100644 ROMFS/px4fmu_common/init.d/10_dji_f330 delete mode 100644 ROMFS/px4fmu_common/init.d/11_dji_f450 delete mode 100644 ROMFS/px4fmu_common/init.d/15_tbs_discovery delete mode 100644 ROMFS/px4fmu_common/init.d/16_3dr_iris create mode 100644 ROMFS/px4fmu_common/init.d/2100_mpx_easystar create mode 100644 ROMFS/px4fmu_common/init.d/2101_hk_bixler create mode 100644 ROMFS/px4fmu_common/init.d/2102_3dr_skywalker create mode 100644 ROMFS/px4fmu_common/init.d/3030_io_camflyer create mode 100644 ROMFS/px4fmu_common/init.d/3031_io_phantom create mode 100644 ROMFS/px4fmu_common/init.d/3032_skywalker_x5 create mode 100644 ROMFS/px4fmu_common/init.d/3033_io_wingwing create mode 100644 ROMFS/px4fmu_common/init.d/3034_io_fx79 delete mode 100644 ROMFS/px4fmu_common/init.d/30_io_camflyer delete mode 100644 ROMFS/px4fmu_common/init.d/31_io_phantom delete mode 100644 ROMFS/px4fmu_common/init.d/32_skywalker_x5 delete mode 100644 ROMFS/px4fmu_common/init.d/33_io_wingwing delete mode 100644 ROMFS/px4fmu_common/init.d/34_io_fx79 create mode 100644 ROMFS/px4fmu_common/init.d/4008_ardrone create mode 100644 ROMFS/px4fmu_common/init.d/4009_ardrone_flow create mode 100644 ROMFS/px4fmu_common/init.d/4010_dji_f330 create mode 100644 ROMFS/px4fmu_common/init.d/4011_dji_f450 diff --git a/ROMFS/px4fmu_common/init.d/#!nsh b/ROMFS/px4fmu_common/init.d/#!nsh new file mode 100644 index 000000000..69b718282 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/#!nsh @@ -0,0 +1,4 @@ +#!nsh +# +# Script to set PWM min / max limits and mixer +# \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone deleted file mode 100644 index f6f09ac22..000000000 --- a/ROMFS/px4fmu_common/init.d/08_ardrone +++ /dev/null @@ -1,57 +0,0 @@ -#!nsh - -echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set MC_ATTRATE_D 0 - param set MC_ATTRATE_I 0 - param set MC_ATTRATE_P 0.13 - param set MC_ATT_D 0 - param set MC_ATT_I 0.3 - param set MC_ATT_P 5 - param set MC_YAWPOS_D 0.1 - param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_P 1 - param set MC_YAWRATE_D 0 - param set MC_YAWRATE_I 0 - param set MC_YAWRATE_P 0.15 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 -param set BAT_V_SCALING 0.008381 - -# -# Start MAVLink -# -mavlink start -d /dev/ttyS0 -b 57600 -usleep 5000 - -# -# Configure PX4FMU for operation with PX4IOAR -# -fmu mode_gpio_serial - -# -# Fire up the AR.Drone interface. -# -ardrone_interface start -d /dev/ttyS1 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -# Exit, because /dev/ttyS0 is needed for MAVLink -exit diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow deleted file mode 100644 index 9b739f245..000000000 --- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow +++ /dev/null @@ -1,88 +0,0 @@ -#!nsh - -echo "[init] 09_ardrone_flow: PX4FMU on PX4IOAR carrier board with PX4FLOW" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set MC_ATTRATE_D 0 - param set MC_ATTRATE_I 0 - param set MC_ATTRATE_P 0.13 - param set MC_ATT_D 0 - param set MC_ATT_I 0.3 - param set MC_ATT_P 5 - param set MC_YAWPOS_D 0.1 - param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_P 1 - param set MC_YAWRATE_D 0 - param set MC_YAWRATE_I 0 - param set MC_YAWRATE_P 0.15 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 -param set BAT_V_SCALING 0.008381 - -# -# Start MAVLink and MAVLink Onboard (PX4FLOW Sensor) -# -mavlink start -d /dev/ttyS0 -b 57600 -mavlink_onboard start -d /dev/ttyS3 -b 115200 -usleep 5000 - -# -# Configure PX4FMU for operation with PX4IOAR -# -fmu mode_gpio_serial - -# -# Fire up the AR.Drone interface. -# -ardrone_interface start -d /dev/ttyS1 - -# -# Start the sensors. -# -sh /etc/init.d/rc.sensors - -# -# Start the commander. -# -commander start - -# -# Start the attitude estimator -# -attitude_estimator_ekf start - -# -# Start the position estimator -# -flow_position_estimator start - -# -# Fire up the multi rotor attitude controller -# -multirotor_att_control start - -# -# Fire up the flow position controller -# -flow_position_control start - -# -# Fire up the flow speed controller -# -flow_speed_control start - -# Exit, because /dev/ttyS0 is needed for MAVLink -exit diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery new file mode 100644 index 000000000..81d4b5d57 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -0,0 +1,74 @@ +#!nsh + +echo "[init] Team Blacksheep Discovery 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 + + param set MC_ATTRATE_D 0.006 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.17 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 5.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 0.5 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.2 + + param save +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 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 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris new file mode 100644 index 000000000..b0f4eda79 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -0,0 +1,73 @@ +#!nsh + +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 + + 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 + param set MC_ATT_P 9.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 0.5 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.2 + + param save +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 +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/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar deleted file mode 100644 index abe378b22..000000000 --- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar +++ /dev/null @@ -1,87 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on EasyStar" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 100 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_Y_ROLLFF 1.1 - 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 - -set EXIT_ON_END no - -# -# 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 - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler deleted file mode 100644 index c616da988..000000000 --- a/ROMFS/px4fmu_common/init.d/101_hk_bixler +++ /dev/null @@ -1,87 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on HK Bixler" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 100 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_Y_ROLLFF 1.1 - 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 - -set EXIT_ON_END no - -# -# 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_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/102_3dr_skywalker deleted file mode 100644 index e5d21c321..000000000 --- a/ROMFS/px4fmu_common/init.d/102_3dr_skywalker +++ /dev/null @@ -1,89 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 100 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_Y_ROLLFF 1.1 - 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 - -set EXIT_ON_END no - -# -# 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 - -pwm disarmed -c 3 -p 1056 - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AETR.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AETR.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 deleted file mode 100644 index 467b56bbf..000000000 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ /dev/null @@ -1,97 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" - -# -# 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 2 = quadrotor -# -param set MAV_TYPE 2 - -set EXIT_ON_END no - -# -# 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 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/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 deleted file mode 100644 index 818f9375e..000000000 --- a/ROMFS/px4fmu_common/init.d/11_dji_f450 +++ /dev/null @@ -1,83 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" - -# -# 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 save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -set EXIT_ON_END no - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - commander start - - 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 - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery deleted file mode 100644 index c79e9d283..000000000 --- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery +++ /dev/null @@ -1,81 +0,0 @@ -#!nsh - -echo "[init] Team Blacksheep Discovery 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 - - param set MC_ATTRATE_D 0.006 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.17 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 5.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_P 0.5 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.2 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -set EXIT_ON_END no - -# -# 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 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 - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris deleted file mode 100644 index f6b071cf1..000000000 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ /dev/null @@ -1,80 +0,0 @@ -#!nsh - -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 - - 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 - param set MC_ATT_P 9.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_P 0.5 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.2 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -set EXIT_ON_END no - -# -# 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 -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 - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar new file mode 100644 index 000000000..97c2d7c90 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -0,0 +1,80 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on EasyStar" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + 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 diff --git a/ROMFS/px4fmu_common/init.d/2101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_hk_bixler new file mode 100644 index 000000000..995d3ba07 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/2101_hk_bixler @@ -0,0 +1,80 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on HK Bixler" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + 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_AERT.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix +fi + +# +# Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker new file mode 100644 index 000000000..a6d2ace96 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker @@ -0,0 +1,82 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + 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 + +pwm disarmed -c 3 -p 1056 + +# +# Load mixer and start controllers (depends on px4io) +# +if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AETR.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_AETR.mix +fi + +# +# Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer new file mode 100644 index 000000000..65f01c974 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -0,0 +1,58 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on Camflyer" + +# +# 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 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_Q.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +fi + +# +# Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/3031_io_phantom b/ROMFS/px4fmu_common/init.d/3031_io_phantom new file mode 100644 index 000000000..0cf6ee39a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3031_io_phantom @@ -0,0 +1,85 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set FW_AIRSPD_MIN 11.4 + param set FW_AIRSPD_TRIM 14 + param set FW_AIRSPD_MAX 22 + param set FW_L1_PERIOD 15 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 45 + param set FW_P_LIM_MIN -45 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 15 + param set FW_R_P 80 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.8 + param set FW_THR_LND_MAX 0 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0.5 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 2.0 + param set FW_Y_ROLLFF 1.0 + param set RC_SCALE_ROLL 0.6 + param set RC_SCALE_PITCH 0.6 + param set TRIM_PITCH 0.1 + + 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_Q.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +fi + +# +# Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 new file mode 100644 index 000000000..41e041654 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -0,0 +1,58 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on Skywalker X5" + +# +# 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 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_Q.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +fi + +# +# Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/3033_io_wingwing b/ROMFS/px4fmu_common/init.d/3033_io_wingwing new file mode 100644 index 000000000..82ff425e6 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3033_io_wingwing @@ -0,0 +1,84 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on the Wing Wing (aka Z-84)" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 9 + param set FW_AIRSPD_MAX 14 + param set FW_L1_PERIOD 10 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 20 + param set FW_P_LIM_MAX 30 + param set FW_P_LIM_MIN -20 + param set FW_P_P 30 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 60 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 0.7 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 2.0 + 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_Q.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +fi + +# +# Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/3034_io_fx79 b/ROMFS/px4fmu_common/init.d/3034_io_fx79 new file mode 100644 index 000000000..759c17bb4 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3034_io_fx79 @@ -0,0 +1,84 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on FX-79 Buffalo" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set FW_AIRSPD_MAX 20 + param set FW_AIRSPD_TRIM 12 + param set FW_AIRSPD_MIN 15 + param set FW_L1_PERIOD 12 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 80 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.75 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 1.1 + 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_FX79.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_FX79.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_FX79.mix +else + echo "Using /etc/mixers/FMU_FX79.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_FX79.mix +fi + +# +# Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer deleted file mode 100644 index 8a8bc1590..000000000 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ /dev/null @@ -1,65 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Camflyer" - -# -# 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 1 = fixed wing -# -param set MAV_TYPE 1 - -set EXIT_ON_END no - -# -# 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_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing - -if [ $EXIT_ON_END == yes ] -then - exit -fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom deleted file mode 100644 index 62cfe1a9c..000000000 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ /dev/null @@ -1,92 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MIN 11.4 - param set FW_AIRSPD_TRIM 14 - param set FW_AIRSPD_MAX 22 - param set FW_L1_PERIOD 15 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 15 - param set FW_R_P 80 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.8 - param set FW_THR_LND_MAX 0 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0.5 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 2.0 - param set FW_Y_ROLLFF 1.0 - param set RC_SCALE_ROLL 0.6 - param set RC_SCALE_PITCH 0.6 - param set TRIM_PITCH 0.1 - - 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 - -set EXIT_ON_END no - -# -# 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_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing - -if [ $EXIT_ON_END == yes ] -then - exit -fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 b/ROMFS/px4fmu_common/init.d/32_skywalker_x5 deleted file mode 100644 index 1e752f13a..000000000 --- a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 +++ /dev/null @@ -1,65 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Skywalker X5" - -# -# 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 1 = fixed wing -# -param set MAV_TYPE 1 - -set EXIT_ON_END no - -# -# 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_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/33_io_wingwing b/ROMFS/px4fmu_common/init.d/33_io_wingwing deleted file mode 100644 index 538c69711..000000000 --- a/ROMFS/px4fmu_common/init.d/33_io_wingwing +++ /dev/null @@ -1,91 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on the Wing Wing (aka Z-84)" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MIN 7 - param set FW_AIRSPD_TRIM 9 - param set FW_AIRSPD_MAX 14 - param set FW_L1_PERIOD 10 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 20 - param set FW_P_LIM_MAX 30 - param set FW_P_LIM_MIN -20 - param set FW_P_P 30 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 60 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 0.7 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5 - param set FW_T_SINK_MIN 2 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 2.0 - 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 - -set EXIT_ON_END no - -# -# 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_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing - -if [ $EXIT_ON_END == yes ] -then - exit -fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/34_io_fx79 b/ROMFS/px4fmu_common/init.d/34_io_fx79 deleted file mode 100644 index 989204952..000000000 --- a/ROMFS/px4fmu_common/init.d/34_io_fx79 +++ /dev/null @@ -1,91 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on FX-79 Buffalo" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MAX 20 - param set FW_AIRSPD_TRIM 12 - param set FW_AIRSPD_MIN 15 - param set FW_L1_PERIOD 12 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 80 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.75 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 1.1 - 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 - -set EXIT_ON_END no - -# -# 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_FX79.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_FX79.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_FX79.mix -else - echo "Using /etc/mixers/FMU_FX79.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_FX79.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone new file mode 100644 index 000000000..f6f09ac22 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -0,0 +1,57 @@ +#!nsh + +echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set MC_ATTRATE_D 0 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_P 0.13 + param set MC_ATT_D 0 + param set MC_ATT_I 0.3 + param set MC_ATT_P 5 + param set MC_YAWPOS_D 0.1 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 1 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.15 + + param set SYS_AUTOCONFIG 0 + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 +param set BAT_V_SCALING 0.008381 + +# +# Start MAVLink +# +mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 + +# +# Configure PX4FMU for operation with PX4IOAR +# +fmu mode_gpio_serial + +# +# Fire up the AR.Drone interface. +# +ardrone_interface start -d /dev/ttyS1 + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +# Exit, because /dev/ttyS0 is needed for MAVLink +exit diff --git a/ROMFS/px4fmu_common/init.d/4009_ardrone_flow b/ROMFS/px4fmu_common/init.d/4009_ardrone_flow new file mode 100644 index 000000000..9b739f245 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4009_ardrone_flow @@ -0,0 +1,88 @@ +#!nsh + +echo "[init] 09_ardrone_flow: PX4FMU on PX4IOAR carrier board with PX4FLOW" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set MC_ATTRATE_D 0 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_P 0.13 + param set MC_ATT_D 0 + param set MC_ATT_I 0.3 + param set MC_ATT_P 5 + param set MC_YAWPOS_D 0.1 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 1 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.15 + + param set SYS_AUTOCONFIG 0 + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 +param set BAT_V_SCALING 0.008381 + +# +# Start MAVLink and MAVLink Onboard (PX4FLOW Sensor) +# +mavlink start -d /dev/ttyS0 -b 57600 +mavlink_onboard start -d /dev/ttyS3 -b 115200 +usleep 5000 + +# +# Configure PX4FMU for operation with PX4IOAR +# +fmu mode_gpio_serial + +# +# Fire up the AR.Drone interface. +# +ardrone_interface start -d /dev/ttyS1 + +# +# Start the sensors. +# +sh /etc/init.d/rc.sensors + +# +# Start the commander. +# +commander start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +# +# Start the position estimator +# +flow_position_estimator start + +# +# Fire up the multi rotor attitude controller +# +multirotor_att_control start + +# +# Fire up the flow position controller +# +flow_position_control start + +# +# Fire up the flow speed controller +# +flow_speed_control start + +# Exit, because /dev/ttyS0 is needed for MAVLink +exit diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 new file mode 100644 index 000000000..7054210e2 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -0,0 +1,69 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" + +# +# 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 + +# +# 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 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 new file mode 100644 index 000000000..84e48696a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -0,0 +1,76 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" + +# +# 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 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 + + commander start + + 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 diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 index 1ee84b9b0..acd8027fb 100644 --- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 +++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 @@ -34,8 +34,6 @@ fi # param set MAV_TYPE 2 -set EXIT_ON_END no - # # Start and configure PX4IO or FMU interface # @@ -76,8 +74,3 @@ pwm max -c 1234 -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/800_sdlogger b/ROMFS/px4fmu_common/init.d/800_sdlogger index 955fe0e2a..1198b42cd 100644 --- a/ROMFS/px4fmu_common/init.d/800_sdlogger +++ b/ROMFS/px4fmu_common/init.d/800_sdlogger @@ -13,8 +13,6 @@ then param save fi -set EXIT_ON_END no - # # Start and configure PX4IO or FMU interface # @@ -53,8 +51,3 @@ then sdlog2 start -r 200 -e -b 16 fi fi - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index e6a748f8f..8bab93fbc 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -194,64 +194,83 @@ then tone_alarm MNGGG fi fi + + set EXIT_ON_END no # # 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 .. 19999 Wide arm / H frame - if param compare SYS_AUTOSTART 8 + if param compare SYS_AUTOSTART 4008 8 then - sh /etc/init.d/08_ardrone + sh /etc/init.d/4008_ardrone set MODE custom fi - if param compare SYS_AUTOSTART 9 + if param compare SYS_AUTOSTART 4009 9 then - sh /etc/init.d/09_ardrone_flow + sh /etc/init.d/4009_ardrone_flow set MODE custom fi - if param compare SYS_AUTOSTART 10 + if param compare SYS_AUTOSTART 4010 10 then - sh /etc/init.d/10_dji_f330 + 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 11 + if param compare SYS_AUTOSTART 4011 11 then - sh /etc/init.d/11_dji_f450 + sh /etc/init.d/4011_dji_f450 set MODE custom fi - if param compare SYS_AUTOSTART 12 + if param compare SYS_AUTOSTART 6012 12 then set MIXER /etc/mixers/FMU_hex_x.mix sh /etc/init.d/12-13_hex set MODE custom fi - if param compare SYS_AUTOSTART 13 + if param compare SYS_AUTOSTART 6013 13 then set MIXER /etc/mixers/FMU_hex_+.mix sh /etc/init.d/12-13_hex set MODE custom fi - if param compare SYS_AUTOSTART 15 + if param compare SYS_AUTOSTART 10015 15 then - sh /etc/init.d/15_tbs_discovery + sh /etc/init.d/10015_tbs_discovery set MODE custom fi - if param compare SYS_AUTOSTART 16 + if param compare SYS_AUTOSTART 10016 16 then - sh /etc/init.d/16_3dr_iris + 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 17 + if param compare SYS_AUTOSTART 4017 17 then set MKBLCTRL_MODE no set MKBLCTRL_FRAME x @@ -260,7 +279,7 @@ then fi # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame - if param compare SYS_AUTOSTART 18 + if param compare SYS_AUTOSTART 5018 18 then set MKBLCTRL_MODE no set MKBLCTRL_FRAME + @@ -269,7 +288,7 @@ then fi # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing - if param compare SYS_AUTOSTART 19 + if param compare SYS_AUTOSTART 4019 19 then set MKBLCTRL_MODE yes set MKBLCTRL_FRAME x @@ -278,7 +297,7 @@ then fi # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing - if param compare SYS_AUTOSTART 20 + if param compare SYS_AUTOSTART 5020 20 then set MKBLCTRL_MODE yes set MKBLCTRL_FRAME + @@ -287,7 +306,7 @@ then fi # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 21 + if param compare SYS_AUTOSTART 4021 21 then set FRAME_GEOMETRY x set ESC_MAKER afro @@ -296,40 +315,40 @@ then fi # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 22 + 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 30 + if param compare SYS_AUTOSTART 3030 30 then - sh /etc/init.d/30_io_camflyer + sh /etc/init.d/3030_io_camflyer set MODE custom fi - if param compare SYS_AUTOSTART 31 + if param compare SYS_AUTOSTART 3031 31 then - sh /etc/init.d/31_io_phantom + sh /etc/init.d/3031_io_phantom set MODE custom fi - if param compare SYS_AUTOSTART 32 + if param compare SYS_AUTOSTART 3032 32 then - sh /etc/init.d/32_skywalker_x5 + sh /etc/init.d/3032_skywalker_x5 set MODE custom fi - if param compare SYS_AUTOSTART 33 + if param compare SYS_AUTOSTART 3033 33 then - sh /etc/init.d/33_io_wingwing + sh /etc/init.d/3033_io_wingwing set MODE custom fi - if param compare SYS_AUTOSTART 34 + if param compare SYS_AUTOSTART 3034 34 then - sh /etc/init.d/34_io_fx79 + sh /etc/init.d/3034_io_fx79 set MODE custom fi @@ -339,21 +358,21 @@ then set MODE custom fi - if param compare SYS_AUTOSTART 100 + if param compare SYS_AUTOSTART 2100 100 then - sh /etc/init.d/100_mpx_easystar + sh /etc/init.d/2100_mpx_easystar set MODE custom fi - if param compare SYS_AUTOSTART 101 + if param compare SYS_AUTOSTART 2101 101 then - sh /etc/init.d/101_hk_bixler + sh /etc/init.d/2101_hk_bixler set MODE custom fi - if param compare SYS_AUTOSTART 102 + if param compare SYS_AUTOSTART 2102 102 then - sh /etc/init.d/102_3dr_skywalker + sh /etc/init.d/2102_3dr_skywalker set MODE custom fi @@ -402,5 +421,10 @@ then fi + if [ $EXIT_ON_END == yes ] + then + exit + fi + # End of autostart fi -- cgit v1.2.3