Autostart: generic quad, hexa and octo added, WIP

13 files changed, 196 insertions, 234 deletions

diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm b/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm
new file mode 100644
index 000000000..9bee414dc
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm
@@ -0,0 +1,17 @@
+#!nsh
+
+# Maintainers: Anton Babushkin <anton.babushkin@me.com>, Lorenz Meier <lm@inf.ethz.ch>
+
+echo "[init] PX4FMU v1, v2 with or without IO on generic quadcopter X with PWM ESCs"
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	#
+	# Default parameters for this platform
+	#
+fi
+ 
+set FRAME_TYPE mc
+set FRAME_GEOMETRY x
+set FRAME_COUNT 4
+set PWM_RATE 400
diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330
index 58c6d99bb..93edb0005 100644
--- a/ROMFS/px4fmu_common/init.d/4010_dji_f330
+++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330
@@ -1,24 +1,29 @@
 #!nsh
 
+# Maintainers: Anton Babushkin <anton.babushkin@me.com>
+
 echo "[init] PX4FMU v1, v2 with or without IO on DJI F330"
 
+# Use generic quad X PWM as base
+sh /etc/init.d/4001_quad_x_pwm
+
 if [ $DO_AUTOCONFIG == yes ]
 then
 	#
 	# Default parameters for this platform
 	#
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
 	param set MC_ATT_P 7.0
 	param set MC_ATT_I 0.0
 	param set MC_ATT_D 0.0
-	param set MC_YAWPOS_P 2.0
+	param set MC_ATTRATE_P 0.12
+	param set MC_ATTRATE_I 0.0
+	param set MC_ATTRATE_D 0.004
+	param set MC_YAWPOS_P 2.8
 	param set MC_YAWPOS_I 0.0
 	param set MC_YAWPOS_D 0.0
-	param set MC_YAWRATE_P 0.3
-	param set MC_YAWRATE_I 0.2
-	param set MC_YAWRATE_D 0.005
+	param set MC_YAWRATE_P 0.2
+	param set MC_YAWRATE_I 0.05
+	param set MC_YAWRATE_D 0.0
 	
 	param set MPC_TILT_MAX 0.5
 	param set MPC_THR_MAX 0.8
@@ -37,10 +42,7 @@ then
 	param set MPC_Z_VEL_P 0.20
 fi
 
-set FRAME_TYPE mc
-set FRAME_GEOMETRY x
-set FRAME_COUNT 4
-set PWM_RATE 400
+# DJI ESC range
+set PWM_DISARMED 900
 set PWM_MIN 1200
 set PWM_MAX 1900
-set PWM_DISARMED 900
diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450
index ca055dfcb..cc0ddccc8 100644
--- a/ROMFS/px4fmu_common/init.d/4011_dji_f450
+++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450
@@ -1,30 +1,34 @@
 #!nsh
 
+# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
+
 echo "[init] PX4FMU v1, v2 with or without IO on DJI F450"
 
+# Use generic quad X PWM as base
+sh /etc/init.d/4001_quad_x_pwm
+
 if [ $DO_AUTOCONFIG == yes ]
 then
 	#
 	# Default parameters for this platform
 	#
-	param set MC_ATTRATE_D 0.004
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATT_D 0.0
-	param set MC_ATT_I 0.0
 	param set MC_ATT_P 7.0
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWPOS_I 0.0
+	param set MC_ATT_I 0.0
+	param set MC_ATT_D 0.0
+	param set MC_ATTRATE_P 0.12
+	param set MC_ATTRATE_I 0.0
+	param set MC_ATTRATE_D 0.004
 	param set MC_YAWPOS_P 2.0
-	param set MC_YAWRATE_D 0.005
-	param set MC_YAWRATE_I 0.2
+	param set MC_YAWPOS_I 0.0
+	param set MC_YAWPOS_D 0.0
 	param set MC_YAWRATE_P 0.3
+	param set MC_YAWRATE_I 0.2
+	param set MC_YAWRATE_D 0.005
+	
+	# TODO add default MPC parameters
 fi
  
-set FRAME_TYPE mc
-set FRAME_GEOMETRY x
-set FRAME_COUNT 4
-set PWM_RATE 400
+# DJI ESC range
+set PWM_DISARMED 900
 set PWM_MIN 1200
 set PWM_MAX 1900
-set PWM_DISARMED 900
diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550
new file mode 100644
index 000000000..a749b7699
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550
@@ -0,0 +1,27 @@
+#!nsh
+
+# Maintainers: Todd Stellanova <tstellanova@gmail.com>
+
+echo "[init] PX4FMU v1, v2 with or without IO on HobbyKing X550"
+
+# Use generic quad X PWM as base
+sh /etc/init.d/4001_quad_x_pwm
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	#
+	# Default parameters for this platform
+	#
+	param set MC_ATT_P 5.5
+	param set MC_ATT_I 0
+	param set MC_ATT_D 0
+	param set MC_ATTRATE_P 0.14
+	param set MC_ATTRATE_I 0
+	param set MC_ATTRATE_D 0.006
+	param set MC_YAWPOS_P 0.6
+	param set MC_YAWPOS_I 0
+	param set MC_YAWPOS_D 0
+	param set MC_YAWRATE_P 0.08
+	param set MC_YAWRATE_I 0
+	param set MC_YAWRATE_D 0
+fi
diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm
new file mode 100644
index 000000000..7d0748668
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm
@@ -0,0 +1,17 @@
+#!nsh
+
+# Maintainers: Anton Babushkin <anton.babushkin@me.com>, Lorenz Meier <lm@inf.ethz.ch>
+
+echo "[init] PX4FMU v1, v2 with or without IO on generic quadcopter + with PWM ESCs"
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	#
+	# Default parameters for this platform
+	#
+fi
+ 
+set FRAME_TYPE mc
+set FRAME_GEOMETRY +
+set FRAME_COUNT 4
+set PWM_RATE 400
diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm
new file mode 100644
index 000000000..3ea5479cb
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm
@@ -0,0 +1,17 @@
+#!nsh
+
+# Maintainers: Anton Babushkin <anton.babushkin@me.com>, Lorenz Meier <lm@inf.ethz.ch>
+
+echo "[init] PX4FMU v1, v2 with or without IO on generic hexacopter X with PWM ESCs"
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	#
+	# Default parameters for this platform
+	#
+fi
+ 
+set FRAME_TYPE mc
+set FRAME_GEOMETRY x
+set FRAME_COUNT 6
+set PWM_RATE 400
diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550
deleted file mode 100644
index acd8027fb..000000000
--- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550
+++ /dev/null
@@ -1,76 +0,0 @@
-#!nsh
-
-echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with or without IO"
-
-#
-# Load default params for this platform
-#
-if param compare SYS_AUTOCONFIG 1
-then
-	# Set all params here, then disable autoconfig
-	param set MC_ATTRATE_P 0.14
-	param set MC_ATTRATE_I 0
-	param set MC_ATTRATE_D 0.006
-	param set MC_ATT_P 5.5
-	param set MC_ATT_I 0
-	param set MC_ATT_D 0
-	param set MC_YAWPOS_D 0
-	param set MC_YAWPOS_I 0
-	param set MC_YAWPOS_P 0.6
-	param set MC_YAWRATE_D 0
-	param set MC_YAWRATE_I 0
-	param set MC_YAWRATE_P 0.08
-	param set RC_SCALE_PITCH 1
-	param set RC_SCALE_ROLL 1
-	param set RC_SCALE_YAW 3
-
-	param set SYS_AUTOCONFIG 0
-	param save
-fi
-
-#
-# Force some key parameters to sane values
-# MAV_TYPE     2 = quadrotor
-#
-param set MAV_TYPE 2
-
-#
-# Start and configure PX4IO or FMU interface
-#
-if px4io detect
-then
-	# Start MAVLink (depends on orb)
-	mavlink start
-	usleep 5000
-
-	sh /etc/init.d/rc.io
-else
-	# Start MAVLink (on UART1 / ttyS0)
-	mavlink start -d /dev/ttyS0
-	usleep 5000
-	fmu mode_pwm
-	param set BAT_V_SCALING 0.004593
-	set EXIT_ON_END yes
-fi
-
-#
-# Load mixer
-#
-mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
-
-#
-# Set PWM output frequency
-#
-pwm rate -c 1234 -r 400
-
-#
-# Set disarmed, min and max PWM signals
-#
-pwm disarmed -c 1234 -p 900
-pwm min -c 1234 -p 1100
-pwm max -c 1234 -p 1900
-
-#
-# Start common for all multirotors apps
-#
-sh /etc/init.d/rc.multirotor
diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm
new file mode 100644
index 000000000..f747618b8
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm
@@ -0,0 +1,17 @@
+#!nsh
+
+# Maintainers: Anton Babushkin <anton.babushkin@me.com>, Lorenz Meier <lm@inf.ethz.ch>
+
+echo "[init] PX4FMU v1, v2 with or without IO on generic hexacopter + with PWM ESCs"
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	#
+	# Default parameters for this platform
+	#
+fi
+ 
+set FRAME_TYPE mc
+set FRAME_GEOMETRY +
+set FRAME_COUNT 4
+set PWM_RATE 400
diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm
new file mode 100644
index 000000000..f8f459185
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm
@@ -0,0 +1,17 @@
+#!nsh
+
+# Maintainers: Anton Babushkin <anton.babushkin@me.com>, Lorenz Meier <lm@inf.ethz.ch>
+
+echo "[init] PX4FMU v1, v2 with or without IO on generic octocopter X with PWM ESCs"
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	#
+	# Default parameters for this platform
+	#
+fi
+ 
+set FRAME_TYPE mc
+set FRAME_GEOMETRY x
+set FRAME_COUNT 8
+set PWM_RATE 400
diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm
new file mode 100644
index 000000000..f8bcd13a9
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm
@@ -0,0 +1,17 @@
+#!nsh
+
+# Maintainers: Anton Babushkin <anton.babushkin@me.com>, Lorenz Meier <lm@inf.ethz.ch>
+
+echo "[init] PX4FMU v1, v2 with or without IO on generic octocopter + with PWM ESCs"
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	#
+	# Default parameters for this platform
+	#
+fi
+ 
+set FRAME_TYPE mc
+set FRAME_GEOMETRY +
+set FRAME_COUNT 8
+set PWM_RATE 400
diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart
index 10b7bc424..153fbb66b 100644
--- a/ROMFS/px4fmu_common/init.d/rc.autostart
+++ b/ROMFS/px4fmu_common/init.d/rc.autostart
@@ -17,53 +17,54 @@
 # 11000	..	11999		Hexa Cox
 # 12000	..	12999		Octo Cox
 
-if param compare SYS_AUTOSTART 4008 8
+if param compare SYS_AUTOSTART 4001
+then
+	sh /etc/init.d/4001_quad_x_pwm
+fi
+
+if param compare SYS_AUTOSTART 4008
 then
 	#sh /etc/init.d/4008_ardrone
 fi
 
-if param compare SYS_AUTOSTART 4009 9
+if param compare SYS_AUTOSTART 4009
 then
 	#sh /etc/init.d/4009_ardrone_flow
 fi
 
-if param compare SYS_AUTOSTART 4010 10
+if param compare SYS_AUTOSTART 4010
 then
 	sh /etc/init.d/4010_dji_f330
 fi
 
-if param compare SYS_AUTOSTART 4011 11
+if param compare SYS_AUTOSTART 4011
 then
 	sh /etc/init.d/4011_dji_f450
 fi
 
 if param compare SYS_AUTOSTART 4012
 then
-	#sh /etc/init.d/666_fmu_q_x550
+	sh /etc/init.d/4012_hk_x550
 fi
 
-if param compare SYS_AUTOSTART 6012 12
+if param compare SYS_AUTOSTART 6001
 then
-	#set MIXER /etc/mixers/FMU_hex_x.mix
-	#sh /etc/init.d/rc.hexa
+	sh /etc/init.d/6001_hexa_x_pwm
 fi
 
-if param compare SYS_AUTOSTART 7013 13
+if param compare SYS_AUTOSTART 7001
 then
-	#set MIXER /etc/mixers/FMU_hex_+.mix
-	#sh /etc/init.d/rc.hexa
+	sh /etc/init.d/7001_hexa_+_pwm
 fi
 
 if param compare SYS_AUTOSTART 8001
 then
-	#set MIXER /etc/mixers/FMU_octo_x.mix
-	#sh /etc/init.d/rc.octo
+	sh /etc/init.d/8001_octo_x_pwm
 fi
 
 if param compare SYS_AUTOSTART 9001
 then
-	#set MIXER /etc/mixers/FMU_octo_+.mix
-	#sh /etc/init.d/rc.octo
+	sh /etc/init.d/9001_octo_+_pwm
 fi
 
 if param compare SYS_AUTOSTART 12001
diff --git a/ROMFS/px4fmu_common/init.d/rc.hexa b/ROMFS/px4fmu_common/init.d/rc.hexa
deleted file mode 100644
index 097db28e4..000000000
--- a/ROMFS/px4fmu_common/init.d/rc.hexa
+++ /dev/null
@@ -1,94 +0,0 @@
-#!nsh
-
-echo "[init] PX4FMU v1, v2 with or without IO on Hex"
-
-#
-# Load default params for this platform
-#
-if param compare SYS_AUTOCONFIG 1
-then
-	# Set all params here, then disable autoconfig
-	param set SYS_AUTOCONFIG 0
-
-	param set MC_ATTRATE_D 0.004
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATT_D 0.0
-	param set MC_ATT_I 0.0
-	param set MC_ATT_P 7.0
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWPOS_I 0.0
-	param set MC_YAWPOS_P 2.0
-	param set MC_YAWRATE_D 0.005
-	param set MC_YAWRATE_I 0.2
-	param set MC_YAWRATE_P 0.3
-	param set NAV_TAKEOFF_ALT 3.0
-	param set MPC_TILT_MAX 0.5
-	param set MPC_THR_MAX 0.7
-	param set MPC_THR_MIN 0.3
-	param set MPC_XY_D 0
-	param set MPC_XY_P 0.5
-	param set MPC_XY_VEL_D 0
-	param set MPC_XY_VEL_I 0
-	param set MPC_XY_VEL_MAX 3
-	param set MPC_XY_VEL_P 0.2
-	param set MPC_Z_D 0
-	param set MPC_Z_P 1
-	param set MPC_Z_VEL_D 0
-	param set MPC_Z_VEL_I 0.1
-	param set MPC_Z_VEL_MAX 2
-	param set MPC_Z_VEL_P 0.20
-
-	param save
-fi
- 
-#
-# Force some key parameters to sane values
-# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/
-# 13 = hexarotor
-#
-param set MAV_TYPE 13
-
-set EXIT_ON_END no
-
-#
-# Start and configure PX4IO interface
-#
-if px4io detect
-then
-	# Start MAVLink (depends on orb)
-	mavlink start
-	usleep 5000
-
-	sh /etc/init.d/rc.io
-else
-	# This is not possible on a hexa
-	tone_alarm error
-fi
-
-#
-# Load mixer
-#
-mixer load /dev/pwm_output $MIXER
-
-#
-# Set PWM output frequency to 400 Hz
-#
-pwm rate -a -r 400
-
-#
-# Set disarmed, min and max PWM signals
-#
-pwm disarmed -c 123456 -p 900
-pwm min -c 123456 -p 1100
-pwm max -c 123456 -p 1900
-
-#
-# Start common for all multirotors apps
-#
-sh /etc/init.d/rc.multirotor
-
-if [ $EXIT_ON_END == yes ]
-then
-	exit
-fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface
index 2a05012a6..9e3d678bf 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_interface
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface
@@ -3,39 +3,35 @@
 # Script to set PWM min / max limits and mixer
 #
 
-#
-# Load mixer
-#
-if [ $FRAME_GEOMETRY == x ]
-then
-    echo "Frame geometry X"
-    mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
-else
-    if [ $FRAME_GEOMETRY == w ]
-    then
-	echo "Frame geometry W"
-	mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
-    else
-	echo "Frame geometry +"
-    	mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
-    fi
-fi
-
+echo "Rotors count: $FRAME_COUNT"
 if [ $FRAME_COUNT == 4 ]
 then
+	set FRAME_COUNT_STR quad
     set OUTPUTS 1234
     param set MAV_TYPE 2
-else
-    if [ $FRAME_COUNT == 6 ]
-    then
+fi
+if [ $FRAME_COUNT == 6 ]
+then
+	set FRAME_COUNT_STR hex
 	set OUTPUTS 123456
 	param set MAV_TYPE 13
-    else
+fi
+if [ $FRAME_COUNT == 8 ]
+then
+	set FRAME_COUNT_STR octo
 	set OUTPUTS 12345678
-    fi
+	param set MAV_TYPE 14
 fi
 
 #
+# Load mixer
+#
+echo "Frame geometry: ${FRAME_GEOMETRY}"
+set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY_STR}_${FRAME_GEOMETRY}.mix
+echo "Loading mixer: ${MIXER}"
+mixer load /dev/pwm_output ${MIXER}
+
+#
 # Set PWM output frequency
 #
 if [ $PWM_RATE != none ]