From 6697ffb668ecf74448417d3ca410f97f10276161 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 30 Jun 2015 09:49:52 +0200
Subject: [PATCH] IO driver: Set throttle to zero if in PWM ramp mode

---
 src/modules/px4iofirmware/mixer.cpp | 44 +++++++++++++++++++----------
 1 file changed, 29 insertions(+), 15 deletions(-)

diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 5e6a3a585a..b5d93daea7 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -49,6 +49,7 @@
 
 #include <systemlib/pwm_limit/pwm_limit.h>
 #include <systemlib/mixer/mixer.h>
+#include <uORB/topics/actuator_controls.h>
 
 extern "C" {
 //#define DEBUG
@@ -318,13 +319,6 @@ mixer_callback(uintptr_t handle,
 	case MIX_OVERRIDE:
 		if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
 			control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
-			if (control_group == 0 && control_index == 0) {
-				control += REG_TO_FLOAT(r_setup_trim_roll);
-			} else if (control_group == 0 && control_index == 1) {
-				control += REG_TO_FLOAT(r_setup_trim_pitch);
-			} else if (control_group == 0 && control_index == 2) {
-				control += REG_TO_FLOAT(r_setup_trim_yaw);
-			}
 			break;
 		}
 		return -1;
@@ -333,13 +327,6 @@ mixer_callback(uintptr_t handle,
 		/* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */
 		if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
 			control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
-			if (control_group == 0 && control_index == 0) {
-				control += REG_TO_FLOAT(r_setup_trim_roll);
-			} else if (control_group == 0 && control_index == 1) {
-				control += REG_TO_FLOAT(r_setup_trim_pitch);
-			} else if (control_group == 0 && control_index == 2) {
-				control += REG_TO_FLOAT(r_setup_trim_yaw);
-			}
 			break;
 		} else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
 			control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
@@ -353,6 +340,33 @@ mixer_callback(uintptr_t handle,
 		return -1;
 	}
 
+	/* apply trim offsets for override channels */
+	if (source == MIX_OVERRIDE || source == MIX_OVERRIDE_FMU_OK) {
+		if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
+			control_index == actuator_controls_s::INDEX_ROLL) {
+			control += REG_TO_FLOAT(r_setup_trim_roll);
+
+		} else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
+			control_index == actuator_controls_s::INDEX_PITCH) {
+			control += REG_TO_FLOAT(r_setup_trim_pitch);
+
+		} else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
+			control_index == actuator_controls_s::INDEX_YAW) {
+			control += REG_TO_FLOAT(r_setup_trim_yaw);
+		}
+	}
+
+	/* motor spinup phase - lock throttle to zero */
+	if (pwm_limit.state == PWM_LIMIT_STATE_RAMP) {
+		if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
+			control_index == actuator_controls_s::INDEX_THROTTLE) {
+			/* limit the throttle output to zero during motor spinup,
+			 * as the motors cannot follow any demand yet
+			 */
+			control = 0.0f;
+		}
+	}
+
 	/* limit output */
 	if (control > 1.0f) {
 		control = 1.0f;