Merge pull request #515 from julianoes/startup_fixes

Startup script update

ROMFS/px4fmu_common/init.d/100_mpx_easystar

@@ -52,8 +52,6 @@ then
 	# Start MAVLink (depends on orb)
 	mavlink start
 
-	commander start
-
 	sh /etc/init.d/rc.io
 	# Limit to 100 Hz updates and (implicit) 50 Hz PWM
 	px4io limit 100
@@ -61,46 +59,27 @@ else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
 
-	commander start
-
 	fmu mode_pwm
 	param set BAT_V_SCALING 0.004593
 	set EXIT_ON_END yes
 fi
 
-#
-# 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
- 
 #
 # Load mixer and start controllers (depends on px4io)
 #
 if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ]
 then
-	echo "Using FMU_RET mixer from sd card"
+	echo "Using /fs/microsd/etc/mixers/FMU_RET.mix"
 	mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix
 else
-	echo "Using standard FMU_RET mixer"
+	echo "Using /etc/mixers/FMU_RET.mix"
 	mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix
 fi
-fw_att_control start
-fw_pos_control_l1 start
+
+#
+Start common fixedwing apps
+#
+sh /etc/init.d/rc.fixedwing
 
 if [ $EXIT_ON_END == yes ]
 then

ROMFS/px4fmu_common/init.d/101_hk_bixler

@@ -52,8 +52,6 @@ then
 	# Start MAVLink (depends on orb)
 	mavlink start
 
-	commander start
-
 	sh /etc/init.d/rc.io
 	# Limit to 100 Hz updates and (implicit) 50 Hz PWM
 	px4io limit 100
@@ -61,39 +59,27 @@ else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
 
-	commander start
-
 	fmu mode_pwm
 	param set BAT_V_SCALING 0.004593
 	set EXIT_ON_END yes
 fi
- 
-#
-# 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
- 
 #
 # Load mixer and start controllers (depends on px4io)
 #
-mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
-fw_att_control start
-fw_pos_control_l1 start
+if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ]
+then
+	echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix"
+	mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix
+else
+	echo "Using /etc/mixers/FMU_Q.mix"
+	mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
+fi
+
+#
+Start common fixedwing apps
+#
+sh /etc/init.d/rc.fixedwing
 
 if [ $EXIT_ON_END == yes ]
 then

ROMFS/px4fmu_common/init.d/10_dji_f330

@@ -59,10 +59,7 @@ then
 	mavlink start
 	usleep 5000
 
-	commander start
-
 	sh /etc/init.d/rc.io
-	# Set PWM values for DJI ESCs
 else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
@@ -83,7 +80,7 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
 pwm rate -c 1234 -r 400
 
 #
-# Set disarmed, min and max PWM signals
+# Set disarmed, min and max PWM signals (for DJI ESCs)
 #
 pwm disarmed -c 1234 -p 900
 pwm min -c 1234 -p 1200

ROMFS/px4fmu_common/init.d/11_dji_f450

@@ -73,7 +73,7 @@ pwm min -c 1234 -p 1200
 pwm max -c 1234 -p 1800
 
 #
-# Start common for all multirotors apps
+# Start common multirotor apps
 #
 sh /etc/init.d/rc.multirotor
 

ROMFS/px4fmu_common/init.d/12-13_hex

@@ -61,10 +61,6 @@ then
 	usleep 5000
 
 	sh /etc/init.d/rc.io
-	# Set PWM values for DJI ESCs
-	px4io idle 900 900 900 900 900 900
-	px4io min 1200 1200 1200 1200 1200 1200
-	px4io max 1900 1900 1900 1900 1900 1900
 else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
@@ -77,12 +73,19 @@ fi
 #
 # Load mixer
 #
-mixer load /dev/pwm_output $MIXER
+mixer load /dev/pwm_output /etc/mixers/FMU_hex_x.mix
 
 #
 # Set PWM output frequency to 400 Hz
 #
-pwm -u 400 -m 0xff
+pwm rate -c 123456 -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

ROMFS/px4fmu_common/init.d/15_tbs_discovery

@@ -31,7 +31,7 @@ fi
 # MAV_TYPE     2 = quadrotor
 #
 param set MAV_TYPE 2
- 
+
 set EXIT_ON_END no
 
 #
@@ -43,8 +43,6 @@ then
 	mavlink start
 	usleep 5000
 
-	commander start
-
 	sh /etc/init.d/rc.io
 else
 	# Start MAVLink (on UART1 / ttyS0)
@@ -66,11 +64,11 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
 pwm rate -c 1234 -r 400
 
 #
-# Set disarmed, min and max PWM signals (for DJI ESCs)
+# Set disarmed, min and max PWM signals
 #
 pwm disarmed -c 1234 -p 900
-pwm min -c 1234 -p 1200
-pwm max -c 1234 -p 1800
+pwm min -c 1234 -p 1100
+pwm max -c 1234 -p 1900
 
 #
 # Start common for all multirotors apps

ROMFS/px4fmu_common/init.d/16_3dr_iris

@@ -31,7 +31,7 @@ fi
 # MAV_TYPE     2 = quadrotor
 #
 param set MAV_TYPE 2
- 
+
 set EXIT_ON_END no
 
 #
@@ -43,8 +43,6 @@ then
 	mavlink start
 	usleep 5000
 
-	commander start
-
 	sh /etc/init.d/rc.io
 else
 	# Start MAVLink (on UART1 / ttyS0)

ROMFS/px4fmu_common/init.d/30_io_camflyer

@@ -30,8 +30,6 @@ then
 	# Start MAVLink (depends on orb)
 	mavlink start
 
-	commander start
-
 	sh /etc/init.d/rc.io
 	# Limit to 100 Hz updates and (implicit) 50 Hz PWM
 	px4io limit 100
@@ -39,41 +37,29 @@ else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
 
-	commander start
-
 	fmu mode_pwm
 	param set BAT_V_SCALING 0.004593
 	set EXIT_ON_END yes
 fi
- 
-#
-# 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
- 
 #
 # Load mixer and start controllers (depends on px4io)
 #
-mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
-fw_att_control start
-fw_pos_control_l1 start
+if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
+then
+	echo "Using /fs/microsd/etc/mixers/FMU_Q.mix"
+	mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix
+else
+	echo "Using /etc/mixers/FMU_Q.mix"
+	mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
+fi
+
+#
+Start common fixedwing apps
+#
+sh /etc/init.d/rc.fixedwing
 
 if [ $EXIT_ON_END == yes ]
 then
 	exit
-fi
+fi

ROMFS/px4fmu_common/init.d/31_io_phantom

@@ -52,8 +52,6 @@ then
 	# Start MAVLink (depends on orb)
 	mavlink start
 
-	commander start
-
 	sh /etc/init.d/rc.io
 	# Limit to 100 Hz updates and (implicit) 50 Hz PWM
 	px4io limit 100
@@ -61,41 +59,29 @@ else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
 
-	commander start
-
 	fmu mode_pwm
 	param set BAT_V_SCALING 0.004593
 	set EXIT_ON_END yes
 fi
- 
-#
-# 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
- 
 #
 # Load mixer and start controllers (depends on px4io)
 #
-mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
-fw_att_control start
-fw_pos_control_l1 start
+if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
+then
+	echo "Using /fs/microsd/etc/mixers/FMU_Q.mix"
+	mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix
+else
+	echo "Using /etc/mixers/FMU_Q.mix"
+	mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
+fi
+
+#
+Start common fixedwing apps
+#
+sh /etc/init.d/rc.fixedwing
 
 if [ $EXIT_ON_END == yes ]
 then
 	exit
-fi
+fi

ROMFS/px4fmu_common/init.d/32_skywalker_x5

@@ -30,8 +30,6 @@ then
 	# Start MAVLink (depends on orb)
 	mavlink start
 
-	commander start
-
 	sh /etc/init.d/rc.io
 	# Limit to 100 Hz updates and (implicit) 50 Hz PWM
 	px4io limit 100
@@ -39,32 +37,10 @@ else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
 
-	commander start
-
 	fmu mode_pwm
 	param set BAT_V_SCALING 0.004593
 	set EXIT_ON_END yes
 fi
- 
-#
-# 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
 
 #
 # Load mixer and start controllers (depends on px4io)
@@ -77,8 +53,11 @@ else
 	echo "Using /etc/mixers/FMU_Q.mix"
 	mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
 fi
-fw_att_control start
-fw_pos_control_l1 start
+
+#
+Start common fixedwing apps
+#
+sh /etc/init.d/rc.fixedwing
 
 if [ $EXIT_ON_END == yes ]
 then

ROMFS/px4fmu_common/init.d/666_fmu_q_x550

@@ -1,6 +1,6 @@
 #!nsh
 
-echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with PWM outputs"
+echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with or without IO"
 
 #
 # Load default params for this platform
@@ -33,17 +33,27 @@ fi
 # MAV_TYPE     2 = quadrotor
 #
 param set MAV_TYPE 2
- 
-#
-# Start MAVLink
-#
-mavlink start -d /dev/ttyS0 -b 57600
-usleep 5000
+
+set EXIT_ON_END no
 
 #
-# Start PWM output
+# Start and configure PX4IO or FMU interface
 #
-fmu mode_pwm
+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
@@ -55,10 +65,19 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
 #
 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
- 
-# Exit, because /dev/ttyS0 is needed for MAVLink
-exit
+
+if [ $EXIT_ON_END == yes ]
+then
+	exit
+fi

ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl

@@ -84,10 +84,10 @@ then
 
 	sh /etc/init.d/rc.io
 else
-	fmu mode_pwm
 	# 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

ROMFS/px4fmu_common/init.d/rc.custom_io_esc

@@ -60,9 +60,7 @@ then
 	# Start MAVLink (depends on orb)
 	mavlink start -d /dev/ttyS1 -b 57600
 	usleep 5000
-
-	commander start
-
+	
 	sh /etc/init.d/rc.io
 else
 	fmu mode_pwm

ROMFS/px4fmu_common/init.d/rc.fixedwing

@@ -0,0 +1,34 @@
+#!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

ROMFS/px4fmu_common/init.d/rc.multirotor

@@ -1,6 +1,6 @@
 #!nsh
 #
-# Standard everything needed for multirotors except mixer, output and mavlink
+# Standard everything needed for multirotors except mixer, actuator output and mavlink
 #
 
 #

ROMFS/px4fmu_common/init.d/rcS

@@ -105,6 +105,11 @@ then
 			blinkm systemstate
 		fi
 	fi
+	
+	#
+	# Start the Commander (needs to be this early for in-air-restarts)
+	#
+	commander start
 
 	if param compare SYS_AUTOSTART 1000
 	then

src/modules/commander/commander.cpp

@@ -116,6 +116,8 @@ extern struct system_load_s system_load;
 #define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 #define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 
+#define MAVLINK_OPEN_INTERVAL 50000
+
 #define STICK_ON_OFF_LIMIT 0.75f
 #define STICK_THRUST_RANGE 1.0f
 #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
@@ -582,16 +584,6 @@ int commander_thread_main(int argc, char *argv[])
 
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
-	if (mavlink_fd < 0) {
-		/* try again later */
-		usleep(20000);
-		mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-
-		if (mavlink_fd < 0) {
-			warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first.");
-		}
-	}
-
 	/* Main state machine */
 	/* make sure we are in preflight state */
 	memset(&status, 0, sizeof(status));
@@ -770,6 +762,11 @@ int commander_thread_main(int argc, char *argv[])
 
 	while (!thread_should_exit) {
 
+		if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) {
+			/* try to open the mavlink log device every once in a while */
+			mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+		}
+
 		/* update parameters */
 		orb_check(param_changed_sub, &updated);