1 files changed, 48 insertions, 7 deletions

diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index b654bdec4..e3ec2fa76 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -59,6 +59,11 @@ extern "C" {
  */
 #define FMU_INPUT_DROP_LIMIT_US		200000
 
+/*
+ * Time that the ESCs need to initialize
+ */
+ #define ESC_INIT_TIME_US		1000000
+
 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
 #define ROLL     0
 #define PITCH    1
@@ -69,6 +74,10 @@ extern "C" {
 /* current servo arm/disarm state */
 static bool mixer_servos_armed = false;
 
+static uint64_t esc_init_time;
+static bool esc_init_active = false;
+static bool esc_init_done = false;
+
 /* selected control values and count for mixing */
 enum mixer_source {
 	MIX_NONE,
@@ -166,6 +175,20 @@ mixer_tick(void)
 		float	outputs[IO_SERVO_COUNT];
 		unsigned mixed;
 
+		/* after arming, some ESCs need an initalization period, count the time from here */
+		if (mixer_servos_armed && !esc_init_done && !esc_init_active) {
+			esc_init_time = hrt_absolute_time();
+			esc_init_active = true;
+			isr_debug(1, "start counting now");
+		}
+
+		/* after waiting long enough for the ESC initialization, we can disable the ESC initialization phase */
+		if (!esc_init_done && esc_init_active && mixer_servos_armed && (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US)) {
+			esc_init_active = false;
+			esc_init_done = true;
+			isr_debug(1, "time is up");
+		}
+
 		/* mix */
 		mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
 
@@ -175,8 +198,20 @@ mixer_tick(void)
 			/* save actuator values for FMU readback */
 			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
 
-			/* scale to servo output */
-			r_page_servos[i] = (outputs[i] * 600.0f) + 1500;
+			/* XXX maybe this check for an armed FMU could be achieved a little less messy */
+			if (source == MIX_FMU && !(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) {
+				r_page_servos[i] = r_page_servo_failsafe[i];
+			}
+			/* during ESC initialization, use low PWM */
+			else if (esc_init_active) {
+				r_page_servos[i] = (outputs[i] * 600 + 1500);
+			
+			/* afterwards use min and max values */
+			} else {
+				r_page_servos[i] = (outputs[i]
+					* (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2
+					+ (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2);
+			}
 
 		}
 		for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
@@ -193,13 +228,15 @@ mixer_tick(void)
 	 * here.
 	 */
 	bool should_arm = (
-	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
-	 	/* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
-		/* there is valid input via direct PWM or mixer */ ((r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) ||
-		/* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) && 
-		/* IO initialised without error */  (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+		/* IO initialised without error */  (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
+		/* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && 
+			/* and either FMU is armed */ ( ( (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && 
+				/* and there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK))) ||
+			
+			/* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) )
 	);
 
+
 	if (should_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
@@ -213,6 +250,9 @@ mixer_tick(void)
 		mixer_servos_armed = false;
 		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
 		isr_debug(5, "> disarmed");
+
+		esc_init_active = false;
+		isr_debug(1, "disarming, and init aborted");
 	}
 
 	if (mixer_servos_armed) {
@@ -345,6 +385,7 @@ mixer_set_failsafe()
 	 * Check if a custom failsafe value has been written,
 	 * or if the mixer is not ok and bail out.
 	 */
+
 	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
 		!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
 		return;