implemented slew-rate

src/modules/px4iofirmware/mixer.cpp

@@ -72,6 +72,8 @@ static bool should_arm_nothrottle = false;
 static bool should_always_enable_pwm = false;
 static volatile bool in_mixer = false;
 
+static uint16_t outputs_prev[4] = {900, 900, 900, 900};
+
 extern int _sbus_fd;
 
 /* selected control values and count for mixing */
@@ -243,6 +245,26 @@ mixer_tick(void)
 		pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed,
 			       r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
 
+		// test slew rate limiting of motor outputs
+		// other option would be low pass filtering
+		float d_pwm_max = 1000.0f / REG_TO_FLOAT(r_setup_slew_max);	// max allowed delta pwm per second
+
+		for (unsigned i = 0; i < 4; i++) {
+			if (d_pwm_max > 0.0f) {
+				float pwm_diff = r_page_servos[i] - outputs_prev[i];
+
+				if (pwm_diff > d_pwm_max * dt) {
+					r_page_servos[i] = outputs_prev[i] + d_pwm_max * dt;
+
+				} else if (pwm_diff < -d_pwm_max * dt) {
+					// XXX might not need this as we won't lose sync on deccelerating
+					r_page_servos[i] = outputs_prev[i] - d_pwm_max * dt;
+				}
+			}
+
+			outputs_prev[i] = r_page_servos[i];
+		}
+
 		/* clamp unused outputs to zero */
 		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
 			r_page_servos[i] = 0;