fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 01:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed pid bug, attitude was not controlled

Browse Source
This commit is contained in:
Julian Oes
2013-06-16 09:55:28 +02:00
parent 263b60c200
commit 68fb200f0b
2 changed files with 8 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/multirotor_att_control/multirotor_attitude_control.c
+6 -4
View File
@@ -155,8 +155,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
parameters_init(&h); parameters_init(&h);
parameters_update(&h, &p); parameters_update(&h, &p);
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET);
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET);
initialized = true; initialized = true;
} }
@@ -167,8 +167,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
parameters_update(&h, &p); parameters_update(&h, &p);
/* apply parameters */ /* apply parameters */
pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f);
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f);
} }
/* reset integral if on ground */ /* reset integral if on ground */
@@ -188,6 +188,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body ,
att->roll, att->rollspeed, deltaT); att->roll, att->rollspeed, deltaT);
// printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT);
if (control_yaw_position) { if (control_yaw_position) {
/* control yaw rate */ /* control yaw rate */
src/modules/systemlib/pid/pid.c
+2 -3
View File
@@ -101,7 +101,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
} }
if (isfinite(diff_filter_factor)) { if (isfinite(diff_filter_factor)) {
pid->limit = diff_filter_factor; pid->diff_filter_factor = diff_filter_factor;
} else { } else {
ret = 1; ret = 1;
@@ -179,8 +179,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
} }
// Calculate the output. Limit output magnitude to pid->limit // Calculate the output. Limit output magnitude to pid->limit
float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd); 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 (output < -pid->limit) output = -pid->limit; if (output < -pid->limit) output = -pid->limit;