From 66ddea01d144aff29a6f815c9cedeb29947b7289 Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 11 Aug 2016 08:45:59 +0200 Subject: [PATCH] implemented slew-rate --- src/drivers/px4io/px4io.cpp | 8 +++++++ src/modules/px4iofirmware/mixer.cpp | 22 +++++++++++++++++++ src/modules/px4iofirmware/protocol.h | 2 ++ src/modules/px4iofirmware/px4io.c | 12 ++++++++++ src/modules/px4iofirmware/px4io.h | 2 ++ src/modules/px4iofirmware/registers.c | 5 ++++- src/modules/sensors/sensor_params.c | 15 +++++++++++++ .../systemlib/mixer/mixer_multirotor.cpp | 2 ++ 8 files changed, 67 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 31b56f469e..7a9e9ffb2e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/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, ¶m_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(); } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 36128fc119..c79e4f40ab 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/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; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index a4c9ce235a..9352799f66 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/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 */ diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index d24bbf0ef6..e24b66633f 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/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); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 89dae97710..6b3781495d 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/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 diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 660a8245ce..707b0d424f 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/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; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 5a4ddd3333..90cbc3dfb0 100644 --- a/src/modules/sensors/sensor_params.c +++ b/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); diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 929e0401e4..607daa12c8 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -51,6 +51,7 @@ #include #include +#include #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;