Merged master

11 files changed, 523 insertions, 108 deletions

diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x
new file mode 100644
index 000000000..58a970eba
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x
@@ -0,0 +1,107 @@
+#!nsh
+#
+# Flight startup script for PX4FMU with PWM outputs.
+#
+ 
+# disable USB and autostart
+set USB no
+set MODE custom
+ 
+echo "[init] doing PX4FMU Quad startup..."
+ 
+#
+# Start the ORB
+#
+uorb start
+ 
+#
+# Load microSD params
+#
+echo "[init] loading microSD params"
+param select /fs/microsd/params
+if [ -f /fs/microsd/params ]
+then
+	param load /fs/microsd/params
+fi
+
+#
+# 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 /fs/microsd/params
+fi
+
+#
+# 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
+ 
+#
+# Start MAVLink
+#
+mavlink start -d /dev/ttyS0 -b 57600
+usleep 5000
+ 
+#
+# Start the sensors and test them.
+#
+sh /etc/init.d/rc.sensors
+ 
+#
+# Start the commander.
+#
+commander start
+
+#
+# Start GPS interface (depends on orb)
+#
+gps start
+ 
+#
+# Start the attitude estimator
+#
+attitude_estimator_ekf start
+ 
+echo "[init] starting PWM output"
+fmu mode_pwm
+mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
+pwm -u 400 -m 0xff
+ 
+#
+# Start attitude control
+#
+multirotor_att_control start
+
+#
+# Start logging
+#
+sdlog2 start -r 50 -a -b 14
+ 
+#
+# Start system state
+#
+if blinkm start
+then
+	blinkm systemstate
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.IO_QUAD b/ROMFS/px4fmu_common/init.d/02_io_quad_x
index 5f2de0d7e..131abf8c4 100644
--- a/ROMFS/px4fmu_common/init.d/rc.IO_QUAD
+++ b/ROMFS/px4fmu_common/init.d/02_io_quad_x
@@ -1,8 +1,11 @@
 #!nsh
+#
+# Flight startup script for PX4FMU+PX4IO
+#
  
-# Disable USB and autostart
+# disable USB and autostart
 set USB no
-set MODE quad
+set MODE custom
  
 #
 # Start the ORB (first app to start)
@@ -18,6 +21,16 @@ if [ -f /fs/microsd/params ]
 then
 	param load /fs/microsd/params
 fi
+
+#
+# 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 /fs/microsd/params
+fi
  
 #
 # Force some key parameters to sane values
@@ -68,6 +81,11 @@ px4io start
 # Allow PX4IO to recover from midair restarts.
 # this is very unlikely, but quite safe and robust.
 px4io recovery
+
+#
+# Disable px4io topic limiting
+#
+px4io limit 200
  
 #
 # Start the sensors (depends on orb, px4io)
@@ -87,21 +105,19 @@ attitude_estimator_ekf start
 #
 # Load mixer and start controllers (depends on px4io)
 #
-mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
+mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
+pwm -u 400 -m 0xff
 multirotor_att_control start
  
 #
 # Start logging
 #
-#sdlog start -s 4
+sdlog2 start -r 50 -a -b 14
  
 #
 # Start system state
 #
 if blinkm start
 then
-	echo "using BlinkM for state indication"
 	blinkm systemstate
-else
-	echo "no BlinkM found, OK."
-fi
\ No newline at end of file
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR b/ROMFS/px4fmu_common/init.d/08_ardrone
index f55ac2ae3..f55ac2ae3 100644
--- a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR
+++ b/ROMFS/px4fmu_common/init.d/08_ardrone
diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR_PX4FLOW_example b/ROMFS/px4fmu_common/init.d/09_ardrone_flow
index e7173f6e6..e7173f6e6 100644
--- a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR_PX4FLOW_example
+++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow
diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330
new file mode 100644
index 000000000..4107fab4f
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/10_io_f330
@@ -0,0 +1,139 @@
+#!nsh
+#
+# Flight startup script for PX4FMU+PX4IO
+#
+ 
+# disable USB and autostart
+set USB no
+set MODE custom
+
+#
+# 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.007
+	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	         7.0
+	param set MC_POS_P	         0.1
+	param set MC_RCLOSS_THR	         0.0
+	param set MC_YAWPOS_D	         0.0
+	param set MC_YAWPOS_I	         0.5
+	param set MC_YAWPOS_P	         1.0
+	param set MC_YAWRATE_D	         0.0
+	param set MC_YAWRATE_I	         0.0
+	param set MC_YAWRATE_P	         0.2
+
+	param save /fs/microsd/params
+fi
+ 
+#
+# 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 PX4IO Firmware should be upgraded (from Andrew Tridgell)
+#
+if [ -f /fs/microsd/px4io2.bin ]
+then
+	echo "PX4IO Firmware found. Checking Upgrade.."
+	if cmp /fs/microsd/px4io2.bin /fs/microsd/px4io2.cur
+	then
+		echo "No newer version, skipping upgrade."
+	else
+		echo "Loading /fs/microsd/px4io2.bin"
+		if px4io update /fs/microsd/px4io2.bin > /fs/microsd/px4io2.log
+		then
+			cp /fs/microsd/px4io2.bin /fs/microsd/px4io2.cur
+			echo "Flashed /fs/microsd/px4io2.bin OK" >> /fs/microsd/px4io2.log
+		else
+			echo "Failed flashing /fs/microsd/px4io2.bin" >> /fs/microsd/px4io2.log
+			echo "Failed to upgrade PX4IO2 firmware - check if PX4IO2 is in bootloader mode"
+		fi
+	fi
+fi
+ 
+#
+# Start MAVLink (depends on orb)
+#
+mavlink start
+usleep 5000
+ 
+#
+# Start PX4IO interface (depends on orb, commander)
+#
+px4io start
+pwm -u 400 -m 0xff
+ 
+#
+# Allow PX4IO to recover from midair restarts.
+# this is very unlikely, but quite safe and robust.
+px4io recovery
+
+#
+# Disable px4io topic limiting
+#
+px4io limit 200
+
+#
+# This sets a PWM right after startup (regardless of safety button)
+#
+px4io idle 900 900 900 900
+
+#
+# The values are for spinning motors when armed using DJI ESCs
+#
+px4io min 1200 1200 1200 1200
+
+#
+# Upper limits could be higher, this is on the safe side
+#
+px4io max 1800 1800 1800 1800
+ 
+#
+# Start the sensors (depends on orb, px4io)
+#
+sh /etc/init.d/rc.sensors
+
+#
+# Start the commander (depends on orb, mavlink)
+#
+commander start
+ 
+#
+# 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)
+#
+mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
+multirotor_att_control start
+ 
+#
+# Start logging
+#
+sdlog2 start -r 20 -a -b 14
+ 
+#
+# Start system state
+#
+if blinkm start
+then
+	blinkm systemstate
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IO b/ROMFS/px4fmu_common/init.d/30_io_camflyer
index 925a5703e..5090b98a4 100644
--- a/ROMFS/px4fmu_common/init.d/rc.PX4IO
+++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer
@@ -1,8 +1,11 @@
 #!nsh
+#
+# Flight startup script for PX4FMU+PX4IO
+#
  
-# Disable USB and autostart
+# disable USB and autostart
 set USB no
-set MODE camflyer
+set MODE custom
  
 #
 # Start the ORB (first app to start)
@@ -18,6 +21,16 @@ if [ -f /fs/microsd/params ]
 then
 	param load /fs/microsd/params
 fi
+
+#
+# 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 /fs/microsd/params
+fi
  
 #
 # Force some key parameters to sane values
@@ -68,6 +81,10 @@ px4io start
 # Allow PX4IO to recover from midair restarts.
 # this is very unlikely, but quite safe and robust.
 px4io recovery
+
+#
+# Set actuator limit to 100 Hz update (50 Hz PWM)
+px4io limit 100
  
 #
 # Start the sensors (depends on orb, px4io)
@@ -93,15 +110,12 @@ control_demo start
 #
 # Start logging
 #
-#sdlog start -s 4
+sdlog2 start -r 50 -a -b 14
  
 #
 # Start system state
 #
 if blinkm start
 then
-	echo "using BlinkM for state indication"
 	blinkm systemstate
-else
-	echo "no BlinkM found, OK."
 fi
diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom
new file mode 100644
index 000000000..5090b98a4
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/31_io_phantom
@@ -0,0 +1,121 @@
+#!nsh
+#
+# Flight startup script for PX4FMU+PX4IO
+#
+ 
+# disable USB and autostart
+set USB no
+set MODE custom
+ 
+#
+# Start the ORB (first app to start)
+#
+uorb start
+ 
+#
+# Load microSD params
+#
+echo "[init] loading microSD params"
+param select /fs/microsd/params
+if [ -f /fs/microsd/params ]
+then
+	param load /fs/microsd/params
+fi
+
+#
+# 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 /fs/microsd/params
+fi
+ 
+#
+# 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 PX4IO Firmware should be upgraded (from Andrew Tridgell)
+#
+if [ -f /fs/microsd/px4io.bin ]
+then
+	echo "PX4IO Firmware found. Checking Upgrade.."
+	if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
+	then
+		echo "No newer version, skipping upgrade."
+	else
+		echo "Loading /fs/microsd/px4io.bin"
+		if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log
+		then
+			cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
+			echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log
+		else
+			echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log
+			echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode"
+		fi
+	fi
+fi
+ 
+#
+# Start MAVLink (depends on orb)
+#
+mavlink start -d /dev/ttyS1 -b 57600
+usleep 5000
+ 
+#
+# Start the commander (depends on orb, mavlink)
+#
+commander start
+ 
+#
+# Start PX4IO interface (depends on orb, commander)
+#
+px4io start
+ 
+#
+# Allow PX4IO to recover from midair restarts.
+# this is very unlikely, but quite safe and robust.
+px4io recovery
+
+#
+# Set actuator limit to 100 Hz update (50 Hz PWM)
+px4io limit 100
+ 
+#
+# 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)
+#
+kalman_demo start
+ 
+#
+# Load mixer and start controllers (depends on px4io)
+#
+mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
+control_demo start
+ 
+#
+# Start logging
+#
+sdlog2 start -r 50 -a -b 14
+ 
+#
+# Start system state
+#
+if blinkm start
+then
+	blinkm systemstate
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x b/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x
deleted file mode 100644
index 980197d68..000000000
--- a/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x
+++ /dev/null
@@ -1,67 +0,0 @@
-#!nsh
-#
-# Flight startup script for PX4FMU with PWM outputs.
-#
- 
-# Disable the USB interface
-set USB no
- 
-# Disable autostarting other apps
-set MODE custom
- 
-echo "[init] doing PX4FMU Quad startup..."
- 
-#
-# Start the ORB
-#
-uorb start
- 
-#
-# Load microSD params
-#
-echo "[init] loading microSD params"
-param select /fs/microsd/params
-if [ -f /fs/microsd/params ]
-then
-	param load /fs/microsd/params
-fi
- 
-#
-# 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
- 
-#
-# Start MAVLink
-#
-mavlink start -d /dev/ttyS0 -b 57600
-usleep 5000
- 
-#
-# Start the sensors and test them.
-#
-sh /etc/init.d/rc.sensors
- 
-#
-# Start the commander.
-#
-commander start
- 
-#
-# Start the attitude estimator
-#
-attitude_estimator_ekf start
- 
-echo "[init] starting PWM output"
-fmu mode_pwm
-mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
- 
-#
-# Start attitude control
-#
-multirotor_att_control start
- 
-echo "[init] startup done, exiting"
-exit
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 62c7184b8..73f40c503 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -34,9 +34,10 @@ fi
 # ALWAYS start this task before the
 # preflight_check.
 #
-sensors start
-
-#
-# Check sensors - run AFTER 'sensors start'
-#
-preflight_check
\ No newline at end of file
+if sensors start
+then
+	#
+	# Check sensors - run AFTER 'sensors start'
+	#
+	preflight_check &
+fi
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index 31af3991a..c89932bb5 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -5,8 +5,44 @@
 
 echo "Starting MAVLink on this USB console"
 
+# Stop tone alarm
+tone_alarm stop
+
 # Tell MAVLink that this link is "fast"
-mavlink start -b 230400 -d /dev/console
+if mavlink stop
+then
+	echo "stopped other MAVLink instance"
+fi
+mavlink start -b 230400 -d /dev/ttyACM0
+
+if [ $MODE == autostart ]
+then
+
+	# Start the commander
+	commander start
+
+	# 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
+fi
 
 echo "MAVLink started, exiting shell.."
 
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 498c93f28..6d141f1b6 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -7,7 +7,6 @@
 # can change this to prevent automatic startup of the flight script.
 #
 set MODE autostart
-set USB autoconnect
 
 #
 # Try to mount the microSD card.
@@ -46,28 +45,77 @@ fi
 # from an EXTERNAL_SCRIPTS build option
 if [ -f /etc/init.d/rc.APM ]
 then
-
-	#
-	# Check for USB host
-	#
-	if [ $USB != autoconnect ]
+	if sercon
 	then
-		echo "[init] not connecting USB"
-	else
-		if sercon
-		then
-			echo "[init] USB interface connected"
-		else
-			if [ -f /dev/ttyACM0 ]
-				echo "[init] NSH via USB"
-			then
-			else
-				echo "[init] No USB connected"
-			fi
-		fi
+		echo "[init] USB interface connected"
 	fi
 
-	echo Running rc.APM
+	echo "Running rc.APM"
 	# if APM startup is successful then nsh will exit
 	sh /etc/init.d/rc.APM
 fi
+
+if [ $MODE == autostart ]
+then
+
+#
+# Start the ORB (first app to start)
+#
+uorb start
+
+#
+# Load microSD params
+#
+if ramtron start
+then
+	param select /ramtron/params
+	if [ -f /ramtron/params ]
+	then
+		param load /ramtron/params
+	fi
+else
+	param select /fs/microsd/params
+	if [ -f /fs/microsd/params ]
+	then
+		param load /fs/microsd/params
+	fi
+fi
+
+#
+# Check if auto-setup from one of the standard scripts is wanted
+# SYS_AUTOSTART = 0 means no autostart (default)
+#
+if param compare SYS_AUTOSTART 1
+then
+	sh /etc/init.d/01_fmu_quad_x
+fi
+
+if param compare SYS_AUTOSTART 2
+then
+	sh /etc/init.d/02_io_quad_x
+fi
+
+if param compare SYS_AUTOSTART 8
+then
+	sh /etc/init.d/08_ardrone
+fi
+
+if param compare SYS_AUTOSTART 9
+then
+	sh /etc/init.d/09_ardrone_flow
+fi
+
+if param compare SYS_AUTOSTART 10
+then
+	sh /etc/init.d/10_io_f330
+fi
+
+if param compare SYS_AUTOSTART 30
+then
+	sh /etc/init.d/30_io_camflyer
+fi
+
+if param compare SYS_AUTOSTART 31
+then
+	sh /etc/init.d/31_io_phantom
+fi