Merge branch 'multirotor' of github.com:cvg/Firmware_Private into multirotor

3 files changed, 80 insertions, 57 deletions

diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x
index c1f3187f9..8223b3ea5 100644
--- a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x
+++ b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x
@@ -62,44 +62,21 @@ param set MAV_TYPE 2
 #
 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)
+# Start common for all multirotors apps
 #
-gps start
- 
+sh /etc/init.d/rc.multirotor
+
 #
-# Start the attitude estimator
+# Start PWM output
 #
-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
-
 # Try to get an USB console
 nshterm /dev/ttyACM0 &
 
 # Exit, because /dev/ttyS0 is needed for MAVLink
-exit
+#exit
diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor
new file mode 100644
index 000000000..81184f363
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.multirotor
@@ -0,0 +1,49 @@
+#!nsh
+#
+# Standard everything needed for multirotors except mixer, output and mavlink
+#
+
+#
+# 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
+ 
+#
+# Start position estimator
+#
+position_estimator_inav start
+
+#
+# Start attitude control
+#
+multirotor_att_control start
+
+#
+# Start position control
+#
+multirotor_pos_control start
+
+#
+# Start logging
+#
+if [ $BOARD == fmuv1 ]
+then
+	sdlog2 start -r 50 -a -b 16
+else
+	sdlog2 start -r 200 -a -b 16
+fi
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index e3d314881..f7ac24341 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1023,46 +1023,42 @@ int commander_thread_main(int argc, char *argv[])
 				/* arm/disarm by RC */
 				res = TRANSITION_NOT_CHANGED;
 
-				/* check if left stick is in lower left position and we are in MANUAL or AUTO mode -> disarm
+				/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
 				 * do it only for rotary wings */
 				if (status.is_rotary_wing &&
 				    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
-				    (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY)) {
-					if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
-						if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-							/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
-							arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-							res = arming_state_transition(&status, &safety, new_arming_state, &armed);
-							stick_off_counter = 0;
-
-						} else {
-							stick_off_counter++;
-						}
-
-						stick_on_counter = 0;
+				    (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY ||
+				    	(status.condition_landed && (
+				   			status.navigation_state == NAVIGATION_STATE_ALTHOLD ||
+			 				status.navigation_state == NAVIGATION_STATE_VECTOR
+			    		))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+					if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+						/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
+						arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
+						res = arming_state_transition(&status, &safety, new_arming_state, &armed);
+						stick_off_counter = 0;
 
 					} else {
-						stick_off_counter = 0;
+						stick_off_counter++;
 					}
+				} else {
+					stick_off_counter = 0;
 				}
 
-				/* check if left stick is in lower right position and we're in manual mode -> arm */
+				/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
 				if (status.arming_state == ARMING_STATE_STANDBY &&
-				    status.main_state == MAIN_STATE_MANUAL) {
-					if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
-						if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-							res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
-							stick_on_counter = 0;
-
-						} else {
-							stick_on_counter++;
-						}
-
-						stick_off_counter = 0;
+				    status.main_state == MAIN_STATE_MANUAL &&
+				    sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+					if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+						res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+						stick_on_counter = 0;
 
 					} else {
-						stick_on_counter = 0;
+						stick_on_counter++;
 					}
+
+				} else {
+					stick_on_counter = 0;
 				}
 
 				if (res == TRANSITION_CHANGED) {
@@ -1708,7 +1704,8 @@ void *commander_low_prio_loop(void *arg)
 
 		/* ignore commands the high-prio loop handles */
 		if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
-		    cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM)
+		    cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
+		    cmd.command == VEHICLE_CMD_NAV_TAKEOFF)
 			continue;
 
 		/* only handle low-priority commands here */