aboutsummaryrefslogtreecommitdiff
path: root/ROMFS/px4fmu_common/init.d
diff options
context:
space:
mode:
authorLorenz Meier <lm@inf.ethz.ch>2014-01-20 13:19:49 +0100
committerLorenz Meier <lm@inf.ethz.ch>2014-01-20 13:19:49 +0100
commitf8c5a6cc50cb848a60fd8d359ab4695f18b7d761 (patch)
treeabe5f202816e37ffb8ce8a9868725cf0c0f9b631 /ROMFS/px4fmu_common/init.d
parent13822b35e75cb6a76798159a3f7210d297528243 (diff)
parentd96e63960c9f951dac02f122ac1e067f7dd3316f (diff)
downloadpx4-firmware-f8c5a6cc50cb848a60fd8d359ab4695f18b7d761.tar.gz
px4-firmware-f8c5a6cc50cb848a60fd8d359ab4695f18b7d761.tar.bz2
px4-firmware-f8c5a6cc50cb848a60fd8d359ab4695f18b7d761.zip
Merged master into logging
Diffstat (limited to 'ROMFS/px4fmu_common/init.d')
-rw-r--r--ROMFS/px4fmu_common/init.d/10015_tbs_discovery85
-rw-r--r--ROMFS/px4fmu_common/init.d/10016_3dr_iris106
-rw-r--r--ROMFS/px4fmu_common/init.d/1001_rc_quad.hil105
-rw-r--r--ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil46
-rw-r--r--ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil133
-rw-r--r--ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil68
-rw-r--r--ROMFS/px4fmu_common/init.d/2100_mpx_easystar61
-rw-r--r--ROMFS/px4fmu_common/init.d/3031_io_phantom85
-rw-r--r--ROMFS/px4fmu_common/init.d/3031_phantom44
-rw-r--r--ROMFS/px4fmu_common/init.d/3032_skywalker_x556
-rw-r--r--ROMFS/px4fmu_common/init.d/3033_io_wingwing84
-rw-r--r--ROMFS/px4fmu_common/init.d/3033_wingwing43
-rw-r--r--ROMFS/px4fmu_common/init.d/3034_fx7943
-rw-r--r--ROMFS/px4fmu_common/init.d/3034_io_fx7984
-rw-r--r--ROMFS/px4fmu_common/init.d/4010_dji_f33076
-rw-r--r--ROMFS/px4fmu_common/init.d/4011_dji_f45091
-rw-r--r--ROMFS/px4fmu_common/init.d/4012_hk_x55033
-rw-r--r--ROMFS/px4fmu_common/init.d/40_io_segway51
-rw-r--r--ROMFS/px4fmu_common/init.d/666_fmu_q_x55076
-rw-r--r--ROMFS/px4fmu_common/init.d/800_sdlogger51
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.autostart141
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl113
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.custom_io_esc120
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.fixedwing34
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.fw_apps19
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.hexa94
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.interface72
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.io36
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.logging4
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.mc_apps24
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.mc_interface49
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.multirotor39
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.octo94
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.sensors23
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.standalone13
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.usb33
-rw-r--r--ROMFS/px4fmu_common/init.d/rcS701
37 files changed, 1054 insertions, 1976 deletions
diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
index 81d4b5d57..63798bb3c 100644
--- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
+++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
@@ -1,74 +1,31 @@
#!nsh
-
-echo "[init] Team Blacksheep Discovery Quad"
-
#
-# Load default params for this platform
+# Team Blacksheep Discovery Quadcopter
+#
+# Maintainers: Simon Wilks <sjwilks@gmail.com>
#
-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
+if [ $DO_AUTOCONFIG == yes ]
+then
+ #
+ # 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
+ param set MC_YAWRATE_I 0.0
+ param set MC_YAWRATE_D 0.0
fi
-#
-# Force some key parameters to sane values
-# MAV_TYPE 2 = quadrotor
-#
-param set MAV_TYPE 2
+set VEHICLE_TYPE mc
+set MIXER FMU_quad_w
-#
-# 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
+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 b0f4eda79..67c24fab3 100644
--- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris
+++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris
@@ -1,73 +1,53 @@
#!nsh
-
-echo "[init] 3DR Iris Quad"
-
#
-# Load default params for this platform
+# 3DR Iris Quadcopter
+#
+# Maintainers: Anton Babushkin <anton.babushkin@me.com>
#
-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
+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
+ 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
+
+ param set BAT_V_SCALING 0.00989
+ param set BAT_C_SCALING 0.0124
fi
-#
-# Force some key parameters to sane values
-# MAV_TYPE 2 = quadrotor
-#
-param set MAV_TYPE 2
+set VEHICLE_TYPE mc
+set MIXER FMU_quad_w
-#
-# 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
+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.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil
deleted file mode 100644
index 9b664d63e..000000000
--- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil
+++ /dev/null
@@ -1,105 +0,0 @@
-#!nsh
-#
-# USB HIL start
-#
-
-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/
-#
-param set MAV_TYPE 2
-
-#
-# Check if we got an IO
-#
-if px4io start
-then
- echo "IO started"
-else
- fmu mode_serial
- echo "FMU started"
-fi
-
-#
-# Start the sensors (depends on orb, px4io)
-#
-sh /etc/init.d/rc.sensors
-
-#
-# Start the attitude estimator (depends on orb)
-#
-att_pos_estimator_ekf start
-
-#
-# Load mixer and start controllers (depends on px4io)
-#
-mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
-
-#
-# Start position estimator
-#
-position_estimator_inav start
-
-#
-# Start attitude control
-#
-multirotor_att_control start
-
-#
-# Start position control
-#
-multirotor_pos_control start
-
-echo "[HIL] setup done, running"
-
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..8c0797d7c
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil
@@ -0,0 +1,46 @@
+#!nsh
+#
+# HIL Quadcopter X
+#
+# Maintainers: Anton Babushkin <anton.babushkin@me.com>
+#
+
+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 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 0cc07ad34..bce3015fc 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 <anton.babushkin@me.com>
#
-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 MIXER FMU_quad_+
diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
index 344d78422..75a00a675 100644
--- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
+++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
@@ -1,14 +1,13 @@
#!nsh
#
-# USB HIL start
+# HIL Rascal 110 (Flightgear)
+#
+# Maintainers: Thomas Gubler <thomasgubler@gmail.com>
#
-echo "[HIL] HILStar starting.."
+echo "HIL Rascal 110 starting.."
-#
-# Load default params for this platform
-#
-if param compare SYS_AUTOCONFIG 1
+if [ $DO_AUTOCONFIG == yes ]
then
# Set all params here, then disable autoconfig
@@ -40,59 +39,8 @@ then
param save
fi
-# Allow USB some time to come up
-sleep 1
-# Tell MAVLink that this link is "fast"
-mavlink start -b 230400 -d /dev/ttyACM0
-
-# Create a fake HIL /dev/pwm_output interface
-hil mode_pwm
-
-#
-# Force some key parameters to sane values
-# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
-# see https://pixhawk.ethz.ch/mavlink/
-#
-param set MAV_TYPE 1
-
-#
-# Check if we got an IO
-#
-if px4io start
-then
- echo "IO started"
-else
- fmu mode_serial
- echo "FMU started"
-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)
-#
-set MODE autostart
-mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
-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_AERT.mix"
- mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
-fi
-
-
-fw_pos_control_l1 start
-fw_att_control start
+set HIL yes
-echo "[HIL] setup done, running"
+set VEHICLE_TYPE fw
+set MIXER FMU_AERT
diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar
index 97c2d7c90..0e5bf60d6 100644
--- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar
+++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar
@@ -1,13 +1,15 @@
#!nsh
-
-echo "[init] PX4FMU v1, v2 with or without IO on EasyStar"
-
#
-# Load default params for this platform
+# MPX EasyStar Plane
+#
+# Maintainers: ???
#
-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 +33,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 VEHICLE_TYPE fw
+set MIXER FMU_RET
diff --git a/ROMFS/px4fmu_common/init.d/3031_io_phantom b/ROMFS/px4fmu_common/init.d/3031_io_phantom
deleted file mode 100644
index 0cf6ee39a..000000000
--- a/ROMFS/px4fmu_common/init.d/3031_io_phantom
+++ /dev/null
@@ -1,85 +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
-
-#
-# 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_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom
new file mode 100644
index 000000000..4ebbe9c61
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/3031_phantom
@@ -0,0 +1,44 @@
+#!nsh
+#
+# Phantom FPV Flying Wing
+#
+# Maintainers: Simon Wilks <sjwilks@gmail.com>
+#
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+ #
+ # Default parameters for this platform
+ #
+ 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
+fi
+
+set VEHICLE_TYPE fw
+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 41e041654..03f282237 100644
--- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5
+++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5
@@ -1,58 +1,14 @@
#!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
-
+# Skywalker X5 Flying Wing
#
-# Start and configure PX4IO or FMU interface
+# Maintainers: Thomas Gubler <thomasgubler@gmail.com>, Julian Oes <joes@student.ethz.ch>
#
-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 ]
+if [ $DO_AUTOCONFIG == yes ]
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
+ # TODO
fi
-#
-# Start common fixedwing apps
-#
-sh /etc/init.d/rc.fixedwing
+set VEHICLE_TYPE fw
+set MIXER FMU_Q
diff --git a/ROMFS/px4fmu_common/init.d/3033_io_wingwing b/ROMFS/px4fmu_common/init.d/3033_io_wingwing
deleted file mode 100644
index 82ff425e6..000000000
--- a/ROMFS/px4fmu_common/init.d/3033_io_wingwing
+++ /dev/null
@@ -1,84 +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
-
-#
-# 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_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing
new file mode 100644
index 000000000..e53763278
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/3033_wingwing
@@ -0,0 +1,43 @@
+#!nsh
+#
+# Wing Wing (aka Z-84) Flying Wing
+#
+# Maintainers: Simon Wilks <sjwilks@gmail.com>
+#
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+ #
+ # Default parameters for this platform
+ #
+ 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
+fi
+
+set VEHICLE_TYPE fw
+set MIXER FMU_Q
diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79
new file mode 100644
index 000000000..8d179d1fd
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/3034_fx79
@@ -0,0 +1,43 @@
+#!nsh
+#
+# FX-79 Buffalo Flying Wing
+#
+# Maintainers: Simon Wilks <sjwilks@gmail.com>
+#
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+ #
+ # Default parameters for this platform
+ #
+ 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
+fi
+
+set VEHICLE_TYPE fw
+set MIXER FMU_FX79
diff --git a/ROMFS/px4fmu_common/init.d/3034_io_fx79 b/ROMFS/px4fmu_common/init.d/3034_io_fx79
deleted file mode 100644
index 759c17bb4..000000000
--- a/ROMFS/px4fmu_common/init.d/3034_io_fx79
+++ /dev/null
@@ -1,84 +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
-
-#
-# 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/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330
index 7054210e2..ab1db94d0 100644
--- a/ROMFS/px4fmu_common/init.d/4010_dji_f330
+++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330
@@ -1,31 +1,31 @@
#!nsh
-
-echo "[init] PX4FMU v1, v2 with or without IO on DJI F330"
-
#
-# Load default params for this platform
+# DJI Flame Wheel F330 Quadcopter
+#
+# Maintainers: Anton Babushkin <anton.babushkin@me.com>
#
-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
+if [ $DO_AUTOCONFIG == yes ]
+then
+ #
+ # Default parameters for this platform
+ #
param set MC_ATT_P 7.0
- param set MC_YAWPOS_D 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.8
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 MC_YAWPOS_D 0.0
+ 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.7
- param set MPC_THR_MIN 0.3
+ param set MPC_THR_MAX 0.8
+ param set MPC_THR_MIN 0.2
param set MPC_XY_D 0
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
@@ -38,32 +38,14 @@ then
param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
-
- param save
fi
-#
-# Start and configure PX4IO or FMU interface
-#
-if px4io detect
-then
- # Start MAVLink (depends on orb)
- mavlink start
- usleep 5000
-
- sh /etc/init.d/rc.io
-else
- # Start MAVLink (on UART1 / ttyS0)
- mavlink start -d /dev/ttyS0
- usleep 5000
- fmu mode_pwm
- param set BAT_V_SCALING 0.004593
- set EXIT_ON_END yes
-fi
+set VEHICLE_TYPE mc
+set MIXER FMU_quad_x
-sh /etc/init.d/rc.mc_interface
-
-#
-# Start common for all multirotors apps
-#
-sh /etc/init.d/rc.multirotor
+set PWM_OUTPUTS 1234
+set PWM_RATE 400
+# DJI ESC range
+set PWM_DISARMED 900
+set PWM_MIN 1200
+set PWM_MAX 1900
diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450
index a1d253191..299771c1d 100644
--- a/ROMFS/px4fmu_common/init.d/4011_dji_f450
+++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450
@@ -1,74 +1,37 @@
#!nsh
-
-echo "[init] PX4FMU v1, v2 with or without IO on DJI F450"
-
#
-# Load default params for this platform
+# DJI Flame Wheel F450 Quadcopter
+#
+# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
#
-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
+if [ $DO_AUTOCONFIG == yes ]
+then
+ #
+ # Default parameters for this platform
+ #
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 save
+ param set MC_YAWRATE_I 0.2
+ param set MC_YAWRATE_D 0.005
+
+ # TODO add default MPC parameters
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
+set VEHICLE_TYPE mc
+set MIXER FMU_quad_x
- 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 PWM_OUTPUTS 1234
+set PWM_RATE 400
+# DJI ESC range
+set PWM_DISARMED 900
+set PWM_MIN 1200
+set PWM_MAX 1900
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..7e020cf59
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550
@@ -0,0 +1,33 @@
+#!nsh
+#
+# HobbyKing X550 Quadcopter
+#
+# Maintainers: Todd Stellanova <tstellanova@gmail.com>
+#
+
+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
+
+ # TODO add default MPC parameters
+fi
+
+set VEHICLE_TYPE mc
+set MIXER FMU_quad_x
+
+set PWM_OUTPUTS 1234
+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/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/800_sdlogger b/ROMFS/px4fmu_common/init.d/800_sdlogger
deleted file mode 100644
index 9b90cbdd0..000000000
--- a/ROMFS/px4fmu_common/init.d/800_sdlogger
+++ /dev/null
@@ -1,51 +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 [ $BOARD == fmuv1 ]
- then
- sdlog2 start -r 50 -e -b 16
- else
- sdlog2 start -r 200 -e -b 16
- fi
-fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart
new file mode 100644
index 000000000..325520dd0
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.autostart
@@ -0,0 +1,141 @@
+#
+# Check if auto-setup from one of the standard scripts is wanted
+# SYS_AUTOSTART = 0 means no autostart (default)
+#
+# AUTOSTART PARTITION:
+# 0 .. 999 Reserved (historical)
+# 1000 .. 1999 Simulation setups
+# 2000 .. 2999 Standard planes
+# 3000 .. 3999 Flying wing
+# 4000 .. 4999 Quad X
+# 5000 .. 5999 Quad +
+# 6000 .. 6999 Hexa X
+# 7000 .. 7999 Hexa +
+# 8000 .. 8999 Octo X
+# 9000 .. 9999 Octo +
+# 10000 .. 10999 Wide arm / H frame
+# 11000 .. 11999 Hexa Cox
+# 12000 .. 12999 Octo Cox
+
+#
+# Simulation setups
+#
+
+if param compare SYS_AUTOSTART 1000
+then
+ #sh /etc/init.d/1000_rc_fw_easystar.hil
+fi
+
+if param compare SYS_AUTOSTART 1001
+then
+ sh /etc/init.d/1001_rc_quad_x.hil
+fi
+
+if param compare SYS_AUTOSTART 1002
+then
+ #sh /etc/init.d/1002_rc_fw_state.hil
+fi
+
+if param compare SYS_AUTOSTART 1003
+then
+ sh /etc/init.d/1003_rc_quad_+.hil
+fi
+
+if param compare SYS_AUTOSTART 1004
+then
+ sh /etc/init.d/1004_rc_fw_Rascal110.hil
+fi
+
+#
+# Standard plane
+#
+
+if param compare SYS_AUTOSTART 2100 100
+then
+ #sh /etc/init.d/2100_mpx_easystar
+ #set MODE custom
+fi
+
+if param compare SYS_AUTOSTART 2101 101
+then
+ #sh /etc/init.d/2101_hk_bixler
+ #set MODE custom
+fi
+
+if param compare SYS_AUTOSTART 2102 102
+then
+ #sh /etc/init.d/2102_3dr_skywalker
+ #set MODE custom
+fi
+
+#
+# Flying wing
+#
+
+if param compare SYS_AUTOSTART 3030
+then
+ #sh /etc/init.d/3030_io_camflyer
+fi
+
+if param compare SYS_AUTOSTART 3031
+then
+ sh /etc/init.d/3031_phantom
+fi
+
+if param compare SYS_AUTOSTART 3032
+then
+ sh /etc/init.d/3032_skywalker_x5
+fi
+
+if param compare SYS_AUTOSTART 3033
+then
+ sh /etc/init.d/3033_wingwing
+fi
+
+if param compare SYS_AUTOSTART 3034
+then
+ sh /etc/init.d/3034_fx79
+fi
+
+#
+# Quad X
+#
+
+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
+
+#
+# Wide arm / H frame
+#
+
+if param compare SYS_AUTOSTART 10015
+then
+ 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.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.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.interface b/ROMFS/px4fmu_common/init.d/rc.interface
new file mode 100644
index 000000000..d25f01dde
--- /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 aaf91b316..c9d964f8e 100644
--- a/ROMFS/px4fmu_common/init.d/rc.io
+++ b/ROMFS/px4fmu_common/init.d/rc.io
@@ -1,23 +1,21 @@
#
-# Start PX4IO interface (depends on orb, commander)
+# Init PX4IO interface
#
-if px4io start
-then
- #
- # Allow PX4IO to recover from midair restarts.
- # this is very unlikely, but quite safe and robust.
- px4io recovery
- #
- # Disable px4io topic limiting
- #
- if [ $BOARD == fmuv1 ]
- then
- px4io limit 200
- else
- px4io limit 400
- fi
-else
- # SOS
- tone_alarm error
+#
+# Allow PX4IO to recover from midair restarts.
+# this is very unlikely, but quite safe and robust.
+#
+px4io recovery
+
+#
+# Adjust PX4IO update rate limit
+#
+set PX4IO_LIMIT 400
+if hw_ver compare PX4FMU_V1
+then
+ 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.logging b/ROMFS/px4fmu_common/init.d/rc.logging
index 6c02bf227..ac620844c 100644
--- a/ROMFS/px4fmu_common/init.d/rc.logging
+++ b/ROMFS/px4fmu_common/init.d/rc.logging
@@ -1,11 +1,11 @@
#!nsh
#
-# Initialise logging services.
+# Initialize logging services.
#
if [ -d /fs/microsd ]
then
- if [ $BOARD == fmuv1 ]
+ if hw_ver compare PX4FMU_V1
then
sdlog2 start -r 50 -a -b 16 -t
else
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps
new file mode 100644
index 000000000..8b51d57e5
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps
@@ -0,0 +1,24 @@
+#!nsh
+#
+# Standard apps for multirotors
+#
+
+#
+# Start the attitude estimator
+#
+attitude_estimator_ekf start
+
+#
+# Start position estimator
+#
+position_estimator_inav start
+
+#
+# Start attitude control
+#
+multirotor_att_control start
+
+#
+# Start position control
+#
+multirotor_pos_control start
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 6bb2e84ec..000000000
--- a/ROMFS/px4fmu_common/init.d/rc.mc_interface
+++ /dev/null
@@ -1,49 +0,0 @@
-#!nsh
-#
-# 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
-
-if [ $FRAME_COUNT == 4 ]
-then
- set OUTPUTS 1234
- param set MAV_TYPE 2
-else
- if [ $FRAME_COUNT == 6 ]
- then
- set OUTPUTS 123456
- param set MAV_TYPE 13
- else
- set OUTPUTS 12345678
- fi
-fi
-
-
-#
-# Set PWM output frequency
-#
-pwm rate -c $OUTPUTS -r $PWM_RATE
-
-#
-# Set disarmed, min and max PWM signals (for DJI ESCs)
-#
-pwm disarmed -c $OUTPUTS -p $PWM_DISARMED
-pwm min -c $OUTPUTS -p $PWM_MIN
-pwm max -c $OUTPUTS -p $PWM_MAX
diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor
deleted file mode 100644
index bc550ac5a..000000000
--- a/ROMFS/px4fmu_common/init.d/rc.multirotor
+++ /dev/null
@@ -1,39 +0,0 @@
-#!nsh
-#
-# Standard everything needed for multirotors 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 estimator
-#
-attitude_estimator_ekf start
-
-#
-# Start position estimator
-#
-position_estimator_inav start
-
-#
-# Start attitude control
-#
-multirotor_att_control start
-
-#
-# Start position control
-#
-multirotor_pos_control start
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.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 070a4e7e3..badbf92c3 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -10,41 +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
- set BOARD fmuv2
-else
- set BOARD fmuv1
+ 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/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 ed034877f..a9c5c59ea 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -8,39 +8,39 @@
#
set MODE autostart
-set logfile /fs/microsd/bootlog.txt
+set LOG_FILE /fs/microsd/bootlog.txt
+set RC_FILE /fs/microsd/etc/rc.txt
+set CONFIG_FILE /fs/microsd/etc/config.txt
+set EXTRAS_FILE /fs/microsd/etc/extras.txt
+
+set TUNE_OUT_ERROR ML<<CP4CP4CP4CP4CP4
#
# 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
#
# Look for an init script on the microSD card.
+# Disable autostart if the script found.
#
-# To prevent automatic startup in the current flight mode,
-# the script should set MODE to some other value.
-#
-if [ -f /fs/microsd/etc/rc ]
-then
- echo "[init] reading /fs/microsd/etc/rc"
- sh /fs/microsd/etc/rc
-fi
-# Also consider rc.txt files
-if [ -f /fs/microsd/etc/rc.txt ]
+if [ -f $RC_FILE ]
then
- echo "[init] reading /fs/microsd/etc/rc.txt"
- sh /fs/microsd/etc/rc.txt
+ echo "[init] Executing init script: $RC_FILE"
+ sh $RC_FILE
+ set MODE custom
+else
+ echo "[init] Init script not found: $RC_FILE"
fi
# if this is an APM build then there will be a rc.APM script
@@ -52,20 +52,19 @@ 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
if [ $MODE == autostart ]
then
+ echo "[init] AUTOSTART mode"
+
#
- # Start terminal
+ # Start CDC/ACM serial driver
#
- if sercon
- then
- echo "USB connected"
- fi
+ sercon
#
# Start the ORB (first app to start)
@@ -73,27 +72,20 @@ then
uorb start
#
- # Load microSD params
+ # Load parameters
#
+ set PARAM_FILE /fs/microsd/params
if mtd start
then
- param select /fs/mtd_params
- if param load /fs/mtd_params
- then
- else
- echo "FAILED LOADING PARAMS"
- fi
+ set PARAM_FILE /fs/mtd_params
+ fi
+
+ param select $PARAM_FILE
+ if param load
+ then
+ echo "[init] Parameters loaded: $PARAM_FILE"
else
- param select /fs/microsd/params
- if [ -f /fs/microsd/params ]
- then
- if param load /fs/microsd/params
- then
- echo "Parameters loaded"
- else
- echo "Parameter file corrupt - ignoring"
- fi
- fi
+ echo "[init] ERROR: Parameters loading failed: $PARAM_FILE"
fi
#
@@ -101,355 +93,416 @@ then
#
if rgbled start
then
- echo "Using external RGB Led"
+ echo "[init] Using external RGB Led"
else
if blinkm start
then
+ echo "[init] Using blinkm"
blinkm systemstate
fi
fi
#
- # Start the Commander (needs to be this early for in-air-restarts)
+ # Set default values
#
- commander start
-
- if param compare SYS_AUTOSTART 1000
- then
- sh /etc/init.d/1000_rc_fw_easystar.hil
- set MODE custom
- fi
+ set HIL no
+ set VEHICLE_TYPE none
+ set MIXER none
+ set USE_IO yes
+ set OUTPUT_MODE none
+ set PWM_OUTPUTS none
+ set PWM_RATE none
+ set PWM_DISARMED none
+ set PWM_MIN none
+ set PWM_MAX none
+ set MKBLCTRL_MODE none
+ set FMU_MODE pwm
+ set MAV_TYPE none
- if param compare SYS_AUTOSTART 1001
+ #
+ # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
+ #
+ if param compare SYS_AUTOCONFIG 1
then
- sh /etc/init.d/1001_rc_quad.hil
- set MODE custom
+ set DO_AUTOCONFIG yes
+ else
+ set DO_AUTOCONFIG no
fi
-
- if param compare SYS_AUTOSTART 1002
+
+ #
+ # Set parameters and env variables for selected AUTOSTART
+ #
+ if param compare SYS_AUTOSTART 0
then
- sh /etc/init.d/1002_rc_fw_state.hil
- set MODE custom
+ echo "[init] Don't try to find autostart script"
+ else
+ sh /etc/init.d/rc.autostart
fi
-
- if param compare SYS_AUTOSTART 1003
+
+ #
+ # Override parameters from user configuration file
+ #
+ if [ -f $CONFIG_FILE ]
then
- sh /etc/init.d/1003_rc_quad_+.hil
- set MODE custom
+ echo "[init] Reading config: $CONFIG_FILE"
+ sh $CONFIG_FILE
+ else
+ echo "[init] Config file not found: $CONFIG_FILE"
fi
- if param compare SYS_AUTOSTART 1004
+ #
+ # If autoconfig parameter was set, reset it and save parameters
+ #
+ if [ $DO_AUTOCONFIG == yes ]
then
- sh /etc/init.d/1004_rc_fw_Rascal110.hil
- set MODE custom
+ param set SYS_AUTOCONFIG 0
+ param save
fi
- if [ $MODE != custom ]
+ set IO_PRESENT no
+
+ if [ $USE_IO == yes ]
then
- # Try to get an USB console
- nshterm /dev/ttyACM0 &
+ #
+ # Check if PX4IO present and update firmware if needed
+ #
+ if [ -f /etc/extras/px4io-v2_default.bin ]
+ then
+ set IO_FILE /etc/extras/px4io-v2_default.bin
+ else
+ set IO_FILE /etc/extras/px4io-v1_default.bin
+ fi
+
+ if px4io checkcrc $IO_FILE
+ then
+ echo "[init] PX4IO CRC OK"
+ echo "PX4IO CRC OK" >> $LOG_FILE
+
+ set IO_PRESENT yes
+ else
+ echo "[init] PX4IO CRC failure, trying to update"
+ echo "PX4IO CRC failure" >> $LOG_FILE
+
+ tone_alarm MLL32CP8MB
+
+ if px4io forceupdate 14662 $IO_FILE
+ then
+ usleep 500000
+ if px4io checkcrc $IO_FILE
+ then
+ 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] ERROR: PX4IO update failed"
+ echo "PX4IO update failed" >> $LOG_FILE
+ fi
+ else
+ echo "[init] ERROR: PX4IO update failed"
+ echo "PX4IO update failed" >> $LOG_FILE
+ fi
+ fi
+
+ if [ $IO_PRESENT == no ]
+ then
+ echo "[init] ERROR: PX4IO not found"
+ tone_alarm $TUNE_OUT_ERROR
+ fi
fi
-
+
#
- # Upgrade PX4IO firmware
+ # Set default output if not set
#
+ if [ $OUTPUT_MODE == none ]
+ then
+ if [ $USE_IO == yes ]
+ then
+ set OUTPUT_MODE io
+ else
+ set OUTPUT_MODE fmu
+ fi
+ fi
- if [ -f /etc/extras/px4io-v2_default.bin ]
+ if [ $OUTPUT_MODE == io -a $IO_PRESENT != yes ]
then
- set io_file /etc/extras/px4io-v2_default.bin
- else
- set io_file /etc/extras/px4io-v1_default.bin
+ # 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 px4io start
+ if [ $HIL == yes ]
then
- echo "PX4IO OK"
- echo "PX4IO OK" >> $logfile
+ set OUTPUT_MODE hil
+ if hw_ver compare PX4FMU_V1
+ then
+ set FMU_MODE serial
+ fi
+ else
+ # Try to get an USB console if not in HIL mode
+ nshterm /dev/ttyACM0 &
fi
- if px4io checkcrc $io_file
+ #
+ # Start the Commander (needs to be this early for in-air-restarts)
+ #
+ commander start
+
+ #
+ # Start primary output
+ #
+ set TTYS1_BUSY no
+
+ # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output
+ if [ $OUTPUT_MODE != none ]
then
- echo "PX4IO CRC OK"
- echo "PX4IO CRC OK" >> $logfile
- else
- echo "PX4IO CRC failure"
- echo "PX4IO CRC failure" >> $logfile
- tone_alarm MBABGP
- if px4io forceupdate 14662 $io_file
+ if [ $OUTPUT_MODE == io ]
then
- usleep 500000
+ echo "[init] Use PX4IO PWM as primary output"
if px4io start
then
- echo "PX4IO restart OK"
- echo "PX4IO restart OK" >> $logfile
- tone_alarm MSPAA
+ echo "[init] PX4IO started"
+ sh /etc/init.d/rc.io
else
- echo "PX4IO restart failed"
- echo "PX4IO restart failed" >> $logfile
- tone_alarm MNGGG
- sleep 10
- reboot
+ echo "[init] ERROR: PX4IO start failed"
+ tone_alarm $TUNE_OUT_ERROR
+ fi
+ fi
+ if [ $OUTPUT_MODE == fmu ]
+ then
+ echo "[init] Use FMU PWM as primary output"
+ if fmu mode_$FMU_MODE
+ then
+ echo "[init] FMU mode_$FMU_MODE started"
+ else
+ 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 ]
+ then
+ set TTYS1_BUSY yes
+ fi
+ if [ $FMU_MODE == pwm_gpio ]
+ then
+ set TTYS1_BUSY yes
+ fi
+ fi
+ fi
+ if [ $OUTPUT_MODE == mkblctrl ]
+ then
+ echo "[init] Use MKBLCTRL as primary output"
+ set MKBLCTRL_ARG ""
+ if [ $MKBLCTRL_MODE == x ]
+ then
+ set MKBLCTRL_ARG "-mkmode x"
+ fi
+ if [ $MKBLCTRL_MODE == + ]
+ then
+ set MKBLCTRL_ARG "-mkmode +"
+ fi
+
+ if mkblctrl $MKBLCTRL_ARG
+ then
+ echo "[init] MKBLCTRL started"
+ else
+ echo "[init] ERROR: MKBLCTRL start failed"
+ tone_alarm $TUNE_OUT_ERROR
+ fi
+
+ fi
+ if [ $OUTPUT_MODE == hil ]
+ then
+ echo "[init] Use HIL as primary output"
+ if hil mode_pwm
+ then
+ echo "[init] HIL output started"
+ else
+ echo "[init] ERROR: HIL output start failed"
+ tone_alarm $TUNE_OUT_ERROR
+ fi
+ fi
+
+ #
+ # Start IO or FMU for RC PPM input if needed
+ #
+ if [ $IO_PRESENT == yes ]
+ then
+ if [ $OUTPUT_MODE != io ]
+ then
+ if px4io start
+ then
+ echo "[init] PX4IO started"
+ sh /etc/init.d/rc.io
+ else
+ echo "[init] ERROR: PX4IO start failed"
+ tone_alarm $TUNE_OUT_ERROR
+ fi
fi
else
- echo "PX4IO update failed"
- echo "PX4IO update failed" >> $logfile
- tone_alarm MNGGG
+ if [ $OUTPUT_MODE != fmu ]
+ then
+ if fmu mode_$FMU_MODE
+ then
+ echo "[init] FMU mode_$FMU_MODE started"
+ else
+ 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 ]
+ then
+ set TTYS1_BUSY yes
+ fi
+ if [ $FMU_MODE == pwm_gpio ]
+ then
+ set TTYS1_BUSY yes
+ fi
+ fi
+ fi
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 .. 10999 Wide arm / H frame
- # 11000 .. 11999 Hexa Cox
- # 12000 .. 12999 Octo Cox
-
- if param compare SYS_AUTOSTART 4008 8
- then
- sh /etc/init.d/4008_ardrone
- set MODE custom
- fi
+ # MAVLink
+ #
+ set EXIT_ON_END no
- if param compare SYS_AUTOSTART 4009 9
+ if [ $HIL == yes ]
then
- sh /etc/init.d/4009_ardrone_flow
- set MODE custom
+ sleep 1
+ mavlink start -b 230400 -d /dev/ttyACM0
+ usleep 5000
+ else
+ if [ $TTYS1_BUSY == yes ]
+ then
+ # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
+ 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
- if param compare SYS_AUTOSTART 4010 10
- then
- set FRAME_GEOMETRY x
- set FRAME_COUNT 4
- set PWM_MIN 1200
- set PWM_MAX 1900
- set PWM_DISARMED 900
- sh /etc/init.d/4010_dji_f330
- set MODE custom
- fi
-
- if param compare SYS_AUTOSTART 4011 11
- then
- sh /etc/init.d/4011_dji_f450
- set MODE custom
- fi
-
- if param compare SYS_AUTOSTART 4012
- then
- sh /etc/init.d/666_fmu_q_x550
- set MODE custom
- fi
-
- if param compare SYS_AUTOSTART 6012 12
- then
- set MIXER /etc/mixers/FMU_hex_x.mix
- sh /etc/init.d/rc.hexa
- set MODE custom
- fi
-
- if param compare SYS_AUTOSTART 7013 13
- then
- set MIXER /etc/mixers/FMU_hex_+.mix
- sh /etc/init.d/rc.hexa
- set MODE custom
- fi
-
- if param compare SYS_AUTOSTART 8001
- then
- set MIXER /etc/mixers/FMU_octo_x.mix
- sh /etc/init.d/rc.octo
- set MODE custom
- fi
-
- if param compare SYS_AUTOSTART 9001
- then
- set MIXER /etc/mixers/FMU_octo_+.mix
- sh /etc/init.d/rc.octo
- set MODE custom
- fi
+ #
+ # Sensors, Logging, GPS
+ #
+ echo "[init] Start sensors"
+ sh /etc/init.d/rc.sensors
- if param compare SYS_AUTOSTART 12001
+ if [ $HIL == no ]
then
- set MIXER /etc/mixers/FMU_octo_cox.mix
- sh /etc/init.d/rc.octo
- set MODE custom
+ echo "[init] Start logging"
+ sh /etc/init.d/rc.logging
+
+ echo "[init] Start GPS"
+ gps start
fi
- if param compare SYS_AUTOSTART 10015 15
- then
- sh /etc/init.d/10015_tbs_discovery
- set MODE custom
- fi
-
- if param compare SYS_AUTOSTART 10016 16
- then
- sh /etc/init.d/10016_3dr_iris
- set MODE custom
- fi
-
- # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame
- if param compare SYS_AUTOSTART 4017 17
- then
- set MKBLCTRL_MODE no
- set MKBLCTRL_FRAME x
- sh /etc/init.d/rc.custom_dji_f330_mkblctrl
- set MODE custom
- fi
-
- # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame
- if param compare SYS_AUTOSTART 5018 18
- then
- set MKBLCTRL_MODE no
- set MKBLCTRL_FRAME +
- sh /etc/init.d/rc.custom_dji_f330_mkblctrl
- set MODE custom
- fi
-
- # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing
- if param compare SYS_AUTOSTART 4019 19
- then
- set MKBLCTRL_MODE yes
- set MKBLCTRL_FRAME x
- sh /etc/init.d/rc.custom_dji_f330_mkblctrl
- set MODE custom
- fi
-
- # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing
- if param compare SYS_AUTOSTART 5020 20
+ #
+ # Fixed wing setup
+ #
+ if [ $VEHICLE_TYPE == fw ]
then
- set MKBLCTRL_MODE yes
- set MKBLCTRL_FRAME +
- sh /etc/init.d/rc.custom_dji_f330_mkblctrl
- set MODE custom
+ echo "[init] Vehicle type: FIXED WING"
+
+ if [ $MIXER == none ]
+ then
+ # 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.interface
+
+ # Start standard fixedwing apps
+ sh /etc/init.d/rc.fw_apps
fi
- # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame
- if param compare SYS_AUTOSTART 4021 21
+ #
+ # Multicopters setup
+ #
+ if [ $VEHICLE_TYPE == mc ]
then
- set FRAME_GEOMETRY x
- set ESC_MAKER afro
- sh /etc/init.d/rc.custom_io_esc
- set MODE custom
- fi
+ echo "[init] Vehicle type: MULTICOPTER"
- # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame
- if param compare SYS_AUTOSTART 10022 22
- then
- set FRAME_GEOMETRY w
- sh /etc/init.d/rc.custom_io_esc
- set MODE custom
- fi
-
- if param compare SYS_AUTOSTART 3030 30
- then
- sh /etc/init.d/3030_io_camflyer
- set MODE custom
- fi
+ if [ $MIXER == none ]
+ then
+ # Set default mixer for multicopter if not defined
+ set MIXER quad_x
+ fi
- if param compare SYS_AUTOSTART 3031 31
- then
- sh /etc/init.d/3031_io_phantom
- set MODE custom
- fi
-
- if param compare SYS_AUTOSTART 3032 32
- then
- sh /etc/init.d/3032_skywalker_x5
- set MODE custom
- 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
- if param compare SYS_AUTOSTART 3033 33
- then
- sh /etc/init.d/3033_io_wingwing
- set MODE custom
+ param set MAV_TYPE $MAV_TYPE
+
+ # Load mixer and configure outputs
+ sh /etc/init.d/rc.interface
+
+ # Start standard multicopter apps
+ sh /etc/init.d/rc.mc_apps
fi
- if param compare SYS_AUTOSTART 3034 34
- then
- sh /etc/init.d/3034_io_fx79
- set MODE custom
- fi
-
- if param compare SYS_AUTOSTART 40
- then
- sh /etc/init.d/40_io_segway
- set MODE custom
- fi
-
- if param compare SYS_AUTOSTART 2100 100
+ #
+ # Generic setup (autostart ID not found)
+ #
+ if [ $VEHICLE_TYPE == none ]
then
- sh /etc/init.d/2100_mpx_easystar
- set MODE custom
- fi
+ echo "[init] Vehicle type: GENERIC"
- if param compare SYS_AUTOSTART 2101 101
- then
- sh /etc/init.d/2101_hk_bixler
- set MODE custom
+ # Load mixer and configure outputs
+ sh /etc/init.d/rc.interface
fi
- if param compare SYS_AUTOSTART 2102 102
- then
- sh /etc/init.d/2102_3dr_skywalker
- set MODE custom
- fi
-
- if param compare SYS_AUTOSTART 800
- then
- sh /etc/init.d/800_sdlogger
- set MODE custom
- fi
-
- # Start any custom extensions that might be missing
- if [ -f /fs/microsd/etc/rc.local ]
- then
- sh /fs/microsd/etc/rc.local
- fi
-
- # If none of the autostart scripts triggered, get a minimal setup
- if [ $MODE == autostart ]
+ # Start any custom addons
+ if [ -f $EXTRAS_FILE ]
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
-
+ echo "[init] Starting addons script: $EXTRAS_FILE"
+ sh $EXTRAS_FILE
+ else
+ echo "[init] Addons script not found: $EXTRAS_FILE"
fi
if [ $EXIT_ON_END == yes ]