diff options
| author | Julian Oes <julian@oes.ch> | 2013-06-16 09:55:28 +0200 |
|---|---|---|
| committer | Julian Oes <julian@oes.ch> | 2013-06-16 15:22:10 +0200 |
| commit | 303694f5f7b7a03488c6075ff2bd67014badfacb (patch) | |
| tree | 25205ebaf97182ea2210f7227c1eced3f86b973f | |
| parent | 8559315f4f8b6515f2ba9ceadf2aa3b73af41bdc (diff) | |
| download | px4-firmware-303694f5f7b7a03488c6075ff2bd67014badfacb.tar.gz px4-firmware-303694f5f7b7a03488c6075ff2bd67014badfacb.tar.bz2 px4-firmware-303694f5f7b7a03488c6075ff2bd67014badfacb.zip | |
Fixed pid bug, attitude was not controlled
| -rw-r--r-- | src/modules/multirotor_att_control/multirotor_attitude_control.c | 10 | ||||
| -rw-r--r-- | src/modules/systemlib/pid/pid.c | 5 |
2 files changed, 8 insertions, 7 deletions
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index d7e1a3adc..4c8f72b8d 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -155,8 +155,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); 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(&roll_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.2f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -167,8 +167,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_update(&h, &p); /* 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(&roll_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.2f); } /* 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 , 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) { /* control yaw rate */ diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index d0e67d3ea..0188a51c4 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -101,7 +101,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float } if (isfinite(diff_filter_factor)) { - pid->limit = diff_filter_factor; + pid->diff_filter_factor = diff_filter_factor; } else { 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 - 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; |