fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 15:13:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

slew rate limiting: moved logic to multirotor mixer

Browse Source 
Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman
2016-09-30 13:55:53 +02:00
committed by Julian Oes
parent 66ddea01d1
commit 2607769470
4 changed files with 73 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/px4iofirmware/mixer.cpp
+10 -22
View File
@@ -72,8 +72,6 @@ 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 */
@@ -234,6 +232,11 @@ mixer_tick(void)
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
// update slew rate value
float slew_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT(
r_setup_slew_max);
mixer_group.update_slew_rate(slew_max);
/* mix */
/* poor mans mutex */
@@ -245,26 +248,6 @@ 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;
@@ -535,6 +518,11 @@ mixer_set_failsafe()
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
// update slew rate value
float slew_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT(
r_setup_slew_max);
mixer_group.update_slew_rate(slew_max);
/* mix */
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits);