From 3883b7113213b6065103e7e32e8ccb519a0405e0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 23 May 2015 13:15:48 +0200
Subject: [PATCH] pwm handler: Support channel reversal

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

diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c
index c733a53d7a..adcfb703c0 100644
--- a/src/modules/systemlib/pwm_limit/pwm_limit.c
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.c
@@ -53,7 +53,9 @@ void pwm_limit_init(pwm_limit_t *limit)
 	return;
 }
 
-void pwm_limit_calc(const bool armed, const unsigned num_channels, 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)
+void pwm_limit_calc(const bool 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)
 {
 
 	/* first evaluate state changes */
@@ -134,7 +136,13 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
 						ramp_min_pwm = min_pwm[i];
 					}
 
-					effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2;
+					float control_value = output[i];
+
+					if (reverse_mask & (1 << i)) {
+						control_value = -1.0f * control_value;
+					}
+
+					effective_pwm[i] = control_value * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2;
 
 					/* last line of defense against invalid inputs */
 					if (effective_pwm[i] < ramp_min_pwm) {
@@ -147,7 +155,14 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
 			break;
 		case PWM_LIMIT_STATE_ON:
 			for (unsigned i=0; i<num_channels; i++) {
-				effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
+
+				float control_value = output[i];
+
+				if (reverse_mask & (1 << i)) {
+					control_value = -1.0f * control_value;
+				}
+
+				effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
 
 				/* last line of defense against invalid inputs */
 				if (effective_pwm[i] < min_pwm[i]) {
diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h
index 6ec9f4b7e2..6ad3d75599 100644
--- a/src/modules/systemlib/pwm_limit/pwm_limit.h
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.h
@@ -72,7 +72,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 *disarmed_pwm,
+__EXPORT void pwm_limit_calc(const bool 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