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;

src/modules/px4iofirmware/protocol.h

@@ -242,6 +242,8 @@ enum {							/* DSM bind states */
 
 #define PX4IO_P_SETUP_SBUS_RATE			22	/* frame rate of SBUS1 output in Hz */
 
+#define PX4IO_P_SETUP_MOTOR_SLEW_MAX 	24 	/* max motor slew rate */
+
 /* autopilot control values, -10000..10000 */
 #define PX4IO_PAGE_CONTROLS			51	/**< actuator control groups, one after the other, 8 wide */
 #define PX4IO_P_CONTROLS_GROUP_0		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */

src/modules/px4iofirmware/px4io.c

@@ -69,6 +69,8 @@ static struct hrt_call serial_dma_call;
 
 pwm_limit_t pwm_limit;
 
+float dt;
+
 /*
  * a set of debug buffers to allow us to send debug information from ISRs
  */
@@ -347,8 +349,18 @@ user_start(int argc, char *argv[])
 
 	uint64_t last_debug_time = 0;
 	uint64_t last_heartbeat_time = 0;
+	uint64_t last_loop_time = 0;
 
 	for (;;) {
+		dt = (hrt_absolute_time() - last_loop_time) / 1000000.0f;
+		last_loop_time = hrt_absolute_time();
+
+		if (dt < 0.0001f) {
+			dt = 0.0001f;
+
+		} else if (dt > 0.02f) {
+			dt = 0.02f;
+		}
 
 		/* track the rate at which the loop is running */
 		perf_count(loop_perf);

src/modules/px4iofirmware/px4io.h

@@ -126,6 +126,7 @@ extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */
 #define r_setup_scale_pitch	r_page_setup[PX4IO_P_SETUP_SCALE_PITCH]
 #define r_setup_scale_yaw	r_page_setup[PX4IO_P_SETUP_SCALE_YAW]
 #define r_setup_sbus_rate	r_page_setup[PX4IO_P_SETUP_SBUS_RATE]
+#define r_setup_slew_max	r_page_setup[PX4IO_P_SETUP_MOTOR_SLEW_MAX]
 
 #define r_control_values	(&r_page_controls[0])
 
@@ -145,6 +146,7 @@ struct sys_state_s {
 };
 
 extern struct sys_state_s system_state;
+extern float dt;
 
 /*
  * PWM limit structure

src/modules/px4iofirmware/registers.c

@@ -180,7 +180,8 @@ volatile uint16_t	r_page_setup[] = {
 	[PX4IO_P_SETUP_TRIM_YAW] = 0,
 	[PX4IO_P_SETUP_SCALE_ROLL] = 10000,
 	[PX4IO_P_SETUP_SCALE_PITCH] = 10000,
-	[PX4IO_P_SETUP_SCALE_YAW] = 10000
+	[PX4IO_P_SETUP_SCALE_YAW] = 10000,
+	[PX4IO_P_SETUP_MOTOR_SLEW_MAX] = 0
 };
 
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V2
@@ -685,6 +686,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		case PX4IO_P_SETUP_SCALE_ROLL:
 		case PX4IO_P_SETUP_SCALE_PITCH:
 		case PX4IO_P_SETUP_SCALE_YAW:
+		case PX4IO_P_SETUP_MOTOR_SLEW_MAX:
+
 			r_page_setup[offset] = value;
 			break;