Merge branch 'master' of https://github.com/julianoes/Firmware into io

13 files changed, 90 insertions, 12 deletions

diff --git a/ROMFS/Makefile b/ROMFS/Makefile
index 56ae63d27..ec4221b93 100644
--- a/ROMFS/Makefile
+++ b/ROMFS/Makefile
@@ -29,6 +29,10 @@ ROMFS_FSSPEC	:= $(SRCROOT)/scripts/rcS~init.d/rcS \
 		   $(SRCROOT)/mixers/FMU_RET.mix~mixers/FMU_ERT.mix \
 		   $(SRCROOT)/mixers/FMU_quad_x.mix~mixers/FMU_quad_x.mix \
 		   $(SRCROOT)/mixers/FMU_quad_+.mix~mixers/FMU_quad_+.mix \
+		   $(SRCROOT)/mixers/FMU_hex_x.mix~mixers/FMU_hex_x.mix \
+		   $(SRCROOT)/mixers/FMU_hex_+.mix~mixers/FMU_hex_+.mix \
+		   $(SRCROOT)/mixers/FMU_octo_x.mix~mixers/FMU_octo_x.mix \
+		   $(SRCROOT)/mixers/FMU_octo_+.mix~mixers/FMU_octo_+.mix \
 		   $(SRCROOT)/logging/logconv.m~logging/logconv.m
 
 #
diff --git a/ROMFS/mixers/FMU_hex_+.mix b/ROMFS/mixers/FMU_hex_+.mix
new file mode 100644
index 000000000..b5e38ce9e
--- /dev/null
+++ b/ROMFS/mixers/FMU_hex_+.mix
@@ -0,0 +1,7 @@
+Multirotor mixer for PX4FMU
+===========================
+
+This file defines a single mixer for a hexacopter in the + configuration. All controls
+are mixed 100%.
+
+R: 6+ 10000 10000 10000 0
diff --git a/ROMFS/mixers/FMU_hex_x.mix b/ROMFS/mixers/FMU_hex_x.mix
new file mode 100644
index 000000000..8e8d122ad
--- /dev/null
+++ b/ROMFS/mixers/FMU_hex_x.mix
@@ -0,0 +1,7 @@
+Multirotor mixer for PX4FMU
+===========================
+
+This file defines a single mixer for a hexacopter in the X configuration.  All controls
+are mixed 100%.
+
+R: 6x 10000 10000 10000 0
diff --git a/ROMFS/mixers/FMU_octo_+.mix b/ROMFS/mixers/FMU_octo_+.mix
new file mode 100644
index 000000000..2cb70e814
--- /dev/null
+++ b/ROMFS/mixers/FMU_octo_+.mix
@@ -0,0 +1,7 @@
+Multirotor mixer for PX4FMU
+===========================
+
+This file defines a single mixer for a octocopter in the + configuration. All controls
+are mixed 100%.
+
+R: 8+ 10000 10000 10000 0
diff --git a/ROMFS/mixers/FMU_octo_x.mix b/ROMFS/mixers/FMU_octo_x.mix
new file mode 100644
index 000000000..edc71f013
--- /dev/null
+++ b/ROMFS/mixers/FMU_octo_x.mix
@@ -0,0 +1,7 @@
+Multirotor mixer for PX4FMU
+===========================
+
+This file defines a single mixer for a octocopter in the X configuration.  All controls
+are mixed 100%.
+
+R: 8x 10000 10000 10000 0
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 72c7e34a1..cd8783673 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -37,7 +37,6 @@
  *
  * PX4IO is connected via serial (or possibly some other interface at a later
  * point).
-
  */
 
 #include <nuttx/config.h>
@@ -384,7 +383,7 @@ PX4IO::task_main()
 
 			if (fds[2].revents & POLLIN) {
 
-				orb_copy(ORB_ID(actuator_armed), _t_armed, &_controls);
+				orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed);
 				_send_needed = true;
 			}
 		}
@@ -538,7 +537,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 
 	case PWM_SERVO_DISARM:
 		/* fake a disarmed transition */
-		_armed.armed = true;
+		_armed.armed = false;
 		_send_needed = true;
 		break;
 
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 29463d744..6a8387741 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -68,10 +68,13 @@
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
 
 #include "multirotor_attitude_control.h"
 #include "multirotor_rate_control.h"
 
+PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost.
+
 __EXPORT int multirotor_att_control_main(int argc, char *argv[]);
 
 static bool thread_should_exit;
@@ -142,6 +145,10 @@ mc_thread_main(int argc, char *argv[])
 	bool flag_system_armed = false;
 	bool man_yaw_zero_once = false;
 
+	/* prepare the handle for the failsafe throttle */
+	param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
+	float failsafe_throttle = 0.0f;
+
 	while (!thread_should_exit) {
 
 		/* wait for a sensor update, check for exit condition every 500 ms */
@@ -226,6 +233,17 @@ mc_thread_main(int argc, char *argv[])
 					att_sp.yaw_body = att.yaw;
 				}
 				att_sp.thrust = manual.throttle;
+
+				/* if the RC signal is lost, try to stay level and go slowly back down to ground */
+				if(state.rc_signal_lost) {
+					/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
+					param_get(failsafe_throttle_handle, &failsafe_throttle);
+					att_sp.roll_body = 0.0f;
+					att_sp.pitch_body = 0.0f;
+					att_sp.yaw_body = 0.0f;
+					att_sp.thrust = failsafe_throttle;
+				}
+
 				att_sp.timestamp = hrt_absolute_time();
 			}
 			/* STEP 2: publish the result to the vehicle actuators */
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index 3964d7503..f1dac433f 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -239,13 +239,19 @@ comms_handle_command(const void *buffer, size_t length)
 	for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++)
 		system_state.fmu_channel_data[i] = cmd->servo_command[i];
 
+	/* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
+	if(system_state.arm_ok && !cmd->arm_ok) {
+		system_state.armed = false;
+	}
+
 	system_state.arm_ok = cmd->arm_ok;
 	system_state.mixer_use_fmu = true;
 	system_state.fmu_data_received = true;
 
+
 	/* handle changes signalled by FMU */
-	if (!system_state.arm_ok && system_state.armed)
-		system_state.armed = false;
+//	if (!system_state.arm_ok && system_state.armed)
+//		system_state.armed = false;
 
 	/* XXX do relay changes here */	
 	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
index b614f3fa4..6cf499c39 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.c
@@ -82,7 +82,7 @@ static void	mixer_get_rc_input(void);
 static void	mixer_update(int mixer, uint16_t *inputs, int input_count);
 
 /* current servo arm/disarm state */
-bool mixer_servos_armed;
+bool mixer_servos_armed = false;
 
 /*
  * Each mixer consumes a set of inputs and produces a single output.
@@ -159,7 +159,7 @@ mixer_tick(void *arg)
 			/*
 			 * If we are armed, update the servo output.
 			 */
-			if (system_state.armed)
+			if (system_state.armed && system_state.arm_ok)
 				up_pwm_servo_set(i, mixers[i].current_value);
 		}
 	}
@@ -167,7 +167,7 @@ mixer_tick(void *arg)
 	/*
 	 * Decide whether the servos should be armed right now.
 	 */
-	should_arm = system_state.armed && (control_count > 0);
+	should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
 	if (should_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index e02d865d5..dec2cdde6 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -133,8 +133,9 @@ int user_start(int argc, char *argv[])
 		/* print some simple status */
 		if (timers[TIMER_STATUS_PRINT] == 0) {
 			timers[TIMER_STATUS_PRINT] = 1000;
-			lib_lowprintf("%c %s | %s | %s | C=%d F=%d B=%d   \r",
+			lib_lowprintf("%c %s | %s | %s | %s | C=%d F=%d B=%d   \r",
 				cursor[cycle++ & 3],
+				(system_state.arm_ok        ? "FMU_ARMED"  : "FMU_SAFE"),
 				(system_state.armed         ? "ARMED"  : "SAFE"),
 				(system_state.rc_channels   ? "RC OK"  : "NO RC"),
 				(system_state.mixer_use_fmu ? "FMU OK" : "NO FMU"),
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 536dbebf1..f50e29252 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -68,7 +68,7 @@
 struct sys_state_s 
 {
 
-	bool		armed;		/* actually armed */
+	bool		armed;		/* IO armed */
 	bool		arm_ok;		/* FMU says OK to arm */
 
 	/*
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index f27432664..0ad636c0b 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -76,9 +76,9 @@ safety_check_button(void *arg)
 	/* 
 	 * Debounce the safety button, change state if it has been held for long enough.
 	 *
-	 * Ignore the button if FMU has not said it's OK to arm yet.
 	 */
-	if (BUTTON_SAFETY && system_state.arm_ok) {
+
+	if (BUTTON_SAFETY) {
 		if (arm_counter < ARM_COUNTER_THRESHOLD) {
 			arm_counter++;
 		} else if (arm_counter == ARM_COUNTER_THRESHOLD) {
diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp
index 0e714ed50..52d79e369 100644
--- a/apps/systemlib/mixer/mixer_multirotor.cpp
+++ b/apps/systemlib/mixer/mixer_multirotor.cpp
@@ -161,6 +161,28 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 	float		max     = 0.0f;
 	float		fixup_scale;
 
+	/* use an output factor to prevent too strong control signals at low throttle */
+	float min_thrust = 0.05f;
+	float max_thrust = 1.0f;
+	float startpoint_full_control = 0.20f;
+	float output_factor;
+
+	/* keep roll, pitch and yaw control to 0 below min thrust */
+	if (thrust <= min_thrust) {
+		output_factor = 0.0f;
+	/* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */
+	} else if (thrust < startpoint_full_control && thrust > min_thrust) {
+		output_factor = (thrust/max_thrust)/(startpoint_full_control-min_thrust);
+	/* and then stay at full control */
+	} else {
+		output_factor = max_thrust;
+	}
+
+	roll *= output_factor;
+	pitch *= output_factor;
+	yaw *= output_factor;
+
+
 	/* perform initial mix pass yielding un-bounded outputs */
 	for (unsigned i = 0; i < _rotor_count; i++) {
 		float tmp = roll  * _rotors[i].roll_scale +