Changed pwm_limit interface a bit

3 files changed, 14 insertions, 17 deletions

diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 5232f9b96..05897b4ce 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -197,9 +197,7 @@ mixer_tick(void)
 		/* mix */
 		mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
 
-		pwm_limit.nchannels = mixed;
-
-		pwm_limit_calc(should_arm, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
+		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
 
 		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
 			r_page_servos[i] = 0;
diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c
index f67b5edf2..3187a4fa2 100644
--- a/src/modules/systemlib/pwm_limit/pwm_limit.c
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.c
@@ -45,15 +45,14 @@
 #include <stdbool.h>
 #include <drivers/drv_hrt.h>
 
-__EXPORT void pwm_limit_init(pwm_limit_t *limit)
+void pwm_limit_init(pwm_limit_t *limit)
 {
-	limit->nchannels = 0;
 	limit->state = LIMIT_STATE_OFF;
 	limit->time_armed = 0;
 	return;
 }
 
-__EXPORT void pwm_limit_calc(const bool armed, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output_requested, uint16_t *effective_pwm, pwm_limit_t *limit)
+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, float *output, uint16_t *effective_pwm, pwm_limit_t *limit)
 {
 	/* first evaluate state changes */
 	switch (limit->state) {
@@ -89,24 +88,26 @@ __EXPORT void pwm_limit_calc(const bool armed, const uint16_t *disarmed_pwm, con
 	switch (limit->state) {
 		case LIMIT_STATE_OFF:
 		case LIMIT_STATE_INIT:
-			for (unsigned i=0; i<limit->nchannels; i++) {
+			for (unsigned i=0; i<num_channels; i++) {
 				effective_pwm[i] = disarmed_pwm[i];
+				output[i] = 0.0f;
 			}
 			break;
 		case LIMIT_STATE_RAMP:
 
 			progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US;
-			for (unsigned i=0; i<limit->nchannels; i++) {
+			for (unsigned i=0; i<num_channels; i++) {
 
-				temp_pwm = output_requested[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
+				temp_pwm = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
 				/* already follow user/controller input if higher than min_pwm */
 				effective_pwm[i] = (disarmed_pwm[i]*(10000-progress) + (temp_pwm > min_pwm[i] ? temp_pwm : min_pwm[i])*progress)/10000;
-
+				output[i] = (float)progress/10000.0f * output[i];
 			}
 			break;
 		case LIMIT_STATE_ON:
-			for (unsigned i=0; i<limit->nchannels; i++) {
-				effective_pwm[i] = output_requested[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
+			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;
+				/* effective_output stays the same */
 			}
 			break;
 		default:
diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h
index f8c6370e8..9974770be 100644
--- a/src/modules/systemlib/pwm_limit/pwm_limit.h
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.h
@@ -46,8 +46,6 @@
 #include <stdint.h>
 #include <stdbool.h>
 
-__BEGIN_DECLS
-
 /*
  * time for the ESCs to initialize
  * (this is not actually needed if PWM is sent right after boot)
@@ -66,13 +64,13 @@ typedef struct {
 		LIMIT_STATE_ON
 	} state;
 	uint64_t time_armed;
-	unsigned nchannels;
 } pwm_limit_t;
 
-__EXPORT void pwm_limit_init(pwm_limit_t *limit);
+__BEGIN_DECLS
 
-__EXPORT void pwm_limit_calc(const bool armed, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output_requested, uint16_t *effective_pwm, pwm_limit_t *limit);
+__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, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit);
 
 __END_DECLS