From 433c9bf42d55165c9e45616514cda2765217af63 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 4 Jul 2015 09:52:15 +0200
Subject: [PATCH] PWM limit lib: Support pre-arming

---
 src/modules/systemlib/pwm_limit/pwm_limit.c | 28 ++++++++++++++++++---
 src/modules/systemlib/pwm_limit/pwm_limit.h |  2 +-
 2 files changed, 26 insertions(+), 4 deletions(-)

diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c
index 2f72d347c6..09965b96b9 100644
--- a/src/modules/systemlib/pwm_limit/pwm_limit.c
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.c
@@ -54,7 +54,7 @@ void pwm_limit_init(pwm_limit_t *limit)
 	return;
 }
 
-void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t reverse_mask,
+void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask,
 	const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm,
 	const float *output, uint16_t *effective_pwm, pwm_limit_t *limit)
 {
@@ -99,6 +99,16 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
 			break;
 	}
 
+	/* if the system is pre-armed, the limit state is temporarily on,
+	 * as some outputs are valid and the non-valid outputs have been
+	 * set to NaN. This is not stored in the state machine though,
+	 * as the throttle channels need to go through the ramp at
+	 * regular arming time.
+	 */
+	if (pre_armed) {
+		limit->state = PWM_LIMIT_STATE_ON;
+	}
+
 	unsigned progress;
 
 	/* then set effective_pwm based on state */
@@ -120,6 +130,14 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
 				}
 
 				for (unsigned i=0; i<num_channels; i++) {
+
+					float control_value = output[i];
+
+					/* check for invalid / disabled channels */
+					if (!isfinite(control_value)) {
+						effective_pwm[i] = disarmed_pwm[i];
+						continue;
+					}
 	                
 					uint16_t ramp_min_pwm;
 	                
@@ -141,8 +159,6 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
 						ramp_min_pwm = min_pwm[i];
 					}
 
-					float control_value = output[i];
-
 					if (reverse_mask & (1 << i)) {
 						control_value = -1.0f * control_value;
 					}
@@ -163,6 +179,12 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
 
 				float control_value = output[i];
 
+				/* check for invalid / disabled channels */
+				if (!isfinite(control_value)) {
+					effective_pwm[i] = disarmed_pwm[i];
+					continue;
+				}
+
 				if (reverse_mask & (1 << i)) {
 					control_value = -1.0f * control_value;
 				}
diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h
index 4e2ae374fd..48cf686e8e 100644
--- a/src/modules/systemlib/pwm_limit/pwm_limit.h
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.h
@@ -71,7 +71,7 @@ typedef struct {
 
 __EXPORT void pwm_limit_init(pwm_limit_t *limit);
 
-__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t reverse_mask, const uint16_t *disarmed_pwm,
+__EXPORT void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, const uint16_t *disarmed_pwm,
 			     const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit);
 
 __END_DECLS