diff options
author | Julian Oes <julian@oes.ch> | 2013-07-08 10:31:32 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-07-08 10:31:32 +0200 |
commit | 88389ea2554c6f56a4fdd86cdd86a1e7b6affc21 (patch) | |
tree | 3ef98ded43e5f3a2cdc30a836a3a7329fe5757bb /src/modules/systemlib/pid/pid.c | |
parent | 76346bfe19c816491a6982abfa10f48cd9d258f6 (diff) | |
parent | cf2dbdf9a1ae06c7d0e0a7963916a3709a1bc075 (diff) | |
download | px4-firmware-88389ea2554c6f56a4fdd86cdd86a1e7b6affc21.tar.gz px4-firmware-88389ea2554c6f56a4fdd86cdd86a1e7b6affc21.tar.bz2 px4-firmware-88389ea2554c6f56a4fdd86cdd86a1e7b6affc21.zip |
Merge branch 'master' into new_state_machine
compiling again
Conflicts:
src/modules/fixedwing_att_control/fixedwing_att_control_att.c
src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
src/modules/mavlink/orb_listener.c
src/modules/multirotor_att_control/multirotor_attitude_control.c
src/modules/multirotor_att_control/multirotor_rate_control.c
src/modules/systemlib/pid/pid.c
src/modules/systemlib/pid/pid.h
src/modules/uORB/objects_common.cpp
Diffstat (limited to 'src/modules/systemlib/pid/pid.c')
-rw-r--r-- | src/modules/systemlib/pid/pid.c | 70 |
1 files changed, 34 insertions, 36 deletions
diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 2aafe0636..9308100b0 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -1,9 +1,11 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@student.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> + * Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,14 +38,21 @@ /** * @file pid.c - * Implementation of generic PID control interface + * + * Implementation of generic PID control interface. + * + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Julian Oes <joes@student.ethz.ch> */ #include "pid.h" #include <math.h> __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - float limit, float diff_filter_factor, uint8_t mode) + float limit, uint8_t mode, float dt_min) { pid->kp = kp; pid->ki = ki; @@ -51,17 +60,15 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->intmax = intmax; pid->limit = limit; pid->mode = mode; - pid->diff_filter_factor = diff_filter_factor; + pid->dt_min = dt_min; pid->count = 0.0f; pid->saturated = 0.0f; pid->last_output = 0.0f; - pid->sp = 0.0f; - pid->error_previous_filtered = 0.0f; - pid->control_previous = 0.0f; + pid->error_previous = 0.0f; pid->integral = 0.0f; } -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor) +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) { int ret = 0; @@ -100,13 +107,6 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float ret = 1; } - if (isfinite(diff_filter_factor)) { - pid->diff_filter_factor = diff_filter_factor; - - } else { - ret = 1; - } - return ret; } @@ -144,20 +144,16 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculated current error value float error = pid->sp - val; - float error_filtered; // Calculate or measured current error derivative - if (pid->mode == PID_MODE_DERIVATIV_CALC) { + d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = error; - error_filtered = pid->error_previous_filtered + (error - pid->error_previous_filtered) * pid->diff_filter_factor; - d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt - pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) { + d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = -val; - error_filtered = pid->error_previous_filtered + (- val - pid->error_previous_filtered) * pid->diff_filter_factor; - d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt - pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; @@ -165,6 +161,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = 0.0f; } + if (!isfinite(d)) { + d = 0.0f; + } + // Calculate the error integral and check for saturation i = pid->integral + (error * dt); @@ -175,7 +175,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo } else { if (!isfinite(i)) { - i = 0; + i = 0.0f; } pid->integral = i; @@ -184,19 +184,17 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculate the output. Limit output magnitude to pid->limit float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); - if (output > pid->limit) output = pid->limit; - - if (output < -pid->limit) output = -pid->limit; if (isfinite(output)) { - pid->last_output = output; - } + if (output > pid->limit) { + output = pid->limit; - pid->control_previous = pid->last_output; + } else if (output < -pid->limit) { + output = -pid->limit; + } - // if (isfinite(error)) { // Why is this necessary? DEW - // pid->error_previous = error; - // } + pid->last_output = output; + } *ctrl_p = (error * pid->kp); *ctrl_i = (i * pid->ki); |