diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-11-26 10:33:10 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-11-26 10:33:29 +0400 |
commit | 3a6be42265921de536f950e2fb5ea25b06945549 (patch) | |
tree | 1401df15c6abb7563f239fb77a228dec2e945278 /src | |
parent | d8e95de9bf3249290524ae21204e87e2b75a3bd9 (diff) | |
download | px4-firmware-3a6be42265921de536f950e2fb5ea25b06945549.tar.gz px4-firmware-3a6be42265921de536f950e2fb5ea25b06945549.tar.bz2 px4-firmware-3a6be42265921de536f950e2fb5ea25b06945549.zip |
multirotor_pos_control: cleanup and fixes
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/multirotor_pos_control/multirotor_pos_control.c | 21 | ||||
-rw-r--r-- | src/modules/multirotor_pos_control/thrust_pid.c | 101 | ||||
-rw-r--r-- | src/modules/multirotor_pos_control/thrust_pid.h | 17 |
3 files changed, 58 insertions, 81 deletions
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index c0e1eb523..ebee0b7a8 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -251,7 +251,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } pid_init(&z_pos_pid, PID_MODE_DERIVATIV_SET, 0.02f); - thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + thrust_pid_init(&z_vel_pid, 0.02f); while (!thread_should_exit) { @@ -265,18 +265,18 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* update params */ parameters_update(¶ms_h, ¶ms); - for (int i = 0; i < 2; i++) { - pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - /* use integral_limit_out = tilt_max / 2 */ - float i_limit; + /* integral_limit * ki = tilt_max / 2 */ + float i_limit; - if (params.xy_vel_i > 0.0f) { - i_limit = params.tilt_max / params.xy_vel_i / 2.0f; + if (params.xy_vel_i > 0.0f) { + i_limit = params.tilt_max / params.xy_vel_i / 2.0f; - } else { - i_limit = 0.0f; // not used - } + } else { + i_limit = 0.0f; // not used + } + for (int i = 0; i < 2; i++) { + pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 0.0f, 0.0f); pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); } @@ -471,6 +471,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } + /* update yaw setpoint only if value is valid */ if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) { att_sp.yaw_body = global_pos_sp.yaw; diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c index b985630ae..39a715322 100644 --- a/src/modules/multirotor_pos_control/thrust_pid.c +++ b/src/modules/multirotor_pos_control/thrust_pid.c @@ -43,22 +43,22 @@ #include "thrust_pid.h" #include <math.h> -__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min) +#define COS_TILT_MAX 0.7f + +__EXPORT void thrust_pid_init(thrust_pid_t *pid, float dt_min) { - pid->kp = kp; - pid->ki = ki; - pid->kd = kd; - pid->limit_min = limit_min; - pid->limit_max = limit_max; - pid->mode = mode; pid->dt_min = dt_min; - pid->last_output = 0.0f; - pid->sp = 0.0f; - pid->error_previous = 0.0f; + pid->kp = 0.0f; + pid->ki = 0.0f; + pid->kd = 0.0f; pid->integral = 0.0f; + pid->output_min = 0.0f; + pid->output_max = 0.0f; + pid->error_previous = 0.0f; + pid->last_output = 0.0f; } -__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max) +__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float output_min, float output_max) { int ret = 0; @@ -83,15 +83,15 @@ __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, fl ret = 1; } - if (isfinite(limit_min)) { - pid->limit_min = limit_min; + if (isfinite(output_min)) { + pid->output_min = output_min; } else { ret = 1; } - if (isfinite(limit_max)) { - pid->limit_max = limit_max; + if (isfinite(output_max)) { + pid->output_max = output_max; } else { ret = 1; @@ -102,40 +102,18 @@ __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, fl __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22) { - /* Alternative integral component calculation - * - * start: - * error = setpoint - current_value - * integral = integral + (Ki * error * dt) - * derivative = (error - previous_error) / dt - * previous_error = error - * output = (Kp * error) + integral + (Kd * derivative) - * wait(dt) - * goto start - */ - if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) { return pid->last_output; } float i, d; - pid->sp = sp; - - // Calculated current error value - float error = pid->sp - val; - // Calculate or measured current error derivative - if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) { - d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); - pid->error_previous = error; + /* error value */ + float error = sp - val; - } else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) { - d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); - pid->error_previous = -val; - - } else { - d = 0.0f; - } + /* error derivative */ + d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = -val; if (!isfinite(d)) { d = 0.0f; @@ -145,36 +123,41 @@ __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, floa i = pid->integral + (pid->ki * error * dt); /* attitude-thrust compensation - * r22 is (2, 2) componet of rotation matrix for current attitude */ + * r22 is (2, 2) component of rotation matrix for current attitude */ float att_comp; - if (r22 > 0.8f) + if (r22 > COS_TILT_MAX) { att_comp = 1.0f / r22; - else if (r22 > 0.0f) - att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f; - else + + } else if (r22 > 0.0f) { + att_comp = ((1.0f / COS_TILT_MAX - 1.0f) / COS_TILT_MAX) * r22 + 1.0f; + + } else { att_comp = 1.0f; + } - /* calculate output */ - float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp; + /* calculate PD output */ + float output = ((error * pid->kp) + (d * pid->kd)) * att_comp; /* check for saturation */ - if (output < pid->limit_min || output > pid->limit_max) { - /* saturated, recalculate output with old integral */ - output = (error * pid->kp) + pid->integral + (d * pid->kd); - - } else { - if (isfinite(i)) { + if (isfinite(i)) { + float i_comp = i * att_comp; + if ((output + i_comp) >= pid->output_min || (output + i_comp) <= pid->output_max) { + /* not saturated, use new integral value */ pid->integral = i; } } + /* add I component to output */ + output += pid->integral * att_comp; + + /* limit output */ if (isfinite(output)) { - if (output > pid->limit_max) { - output = pid->limit_max; + if (output > pid->output_max) { + output = pid->output_max; - } else if (output < pid->limit_min) { - output = pid->limit_min; + } else if (output < pid->output_min) { + output = pid->output_min; } pid->last_output = output; diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h index 5e169c1ba..71c3704d0 100644 --- a/src/modules/multirotor_pos_control/thrust_pid.h +++ b/src/modules/multirotor_pos_control/thrust_pid.h @@ -47,27 +47,20 @@ __BEGIN_DECLS -/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */ -#define THRUST_PID_MODE_DERIVATIV_CALC 0 -/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */ -#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP 1 - typedef struct { + float dt_min; float kp; float ki; float kd; - float sp; float integral; + float output_min; + float output_max; float error_previous; float last_output; - float limit_min; - float limit_max; - float dt_min; - uint8_t mode; } thrust_pid_t; -__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min); -__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max); +__EXPORT void thrust_pid_init(thrust_pid_t *pid, float dt_min); +__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float output_min, float output_max); __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22); __EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i); |