pwm handler: Support channel reversal

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]) {

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