implemented slew-rate

src/drivers/px4io/px4io.cpp

@@ -1245,6 +1245,14 @@ PX4IO::task_main()
 					}
 				}
 
+				/* maximum motor pwm slew rate */
+				parm_handle = param_find("MOT_SLEW_MAX");
+
+				if (parm_handle != PARAM_INVALID) {
+					param_get(parm_handle, &param_val);
+					(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_MOTOR_SLEW_MAX, FLOAT_TO_REG(param_val));
+				}
+
 				// Also trigger param update in Battery instance.
 				_battery.updateParams();
 			}

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;
 

src/modules/sensors/sensor_params.c

@@ -3198,3 +3198,18 @@ PARAM_DEFINE_INT32(PWM_AUX_MAX, 2000);
  * @group PWM Outputs
  */
 PARAM_DEFINE_INT32(PWM_AUX_DISARMED, 1000);
+
+/**
+ * Minimum motor rise time (slew rate limit).
+ *
+ * Minimum time allowed for the motor input signal to pass through
+ * a range of 1000 PWM units. A value x means that the motor signal
+ * can only go from 1000 to 2000 PWM in maximum x seconds.
+ *
+ * Zero means that slew rate limiting is disabled.
+ *
+ * @min 0.0
+ * @unit s/(1000*PWM)
+ * @group PWM Outputs
+ */
+PARAM_DEFINE_FLOAT(MOT_SLEW_MAX, 0.0f);

src/modules/systemlib/mixer/mixer_multirotor.cpp

@@ -51,6 +51,7 @@
 #include <math.h>
 
 #include <px4iofirmware/protocol.h>
+#include <drivers/drv_pwm_output.h>
 
 #include "mixer.h"
 
@@ -366,6 +367,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 			     thrust + boost;
 
 		outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f);
+
 	}
 
 	return _rotor_count;