diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-07-15 09:11:52 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-07-15 09:11:52 +0400 |
commit | b174a6051527dacc858c6ed54b5d113888c5d4de (patch) | |
tree | dc277ea32263c4bb24c1f2b1d421c871f99186c8 /src/modules/multirotor_pos_control/multirotor_pos_control.c | |
parent | 87782300509572ea593f309cbbf0a1ca64a53775 (diff) | |
download | px4-firmware-b174a6051527dacc858c6ed54b5d113888c5d4de.tar.gz px4-firmware-b174a6051527dacc858c6ed54b5d113888c5d4de.tar.bz2 px4-firmware-b174a6051527dacc858c6ed54b5d113888c5d4de.zip |
multirotor_pos_control: PID ontroller derivative mode fixed
Diffstat (limited to 'src/modules/multirotor_pos_control/multirotor_pos_control.c')
-rw-r--r-- | src/modules/multirotor_pos_control/multirotor_pos_control.c | 7 |
1 files changed, 4 insertions, 3 deletions
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 1d2dd384a..4bae7efa4 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -234,11 +234,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) parameters_update(¶ms_h, ¶ms); for (int i = 0; i < 2; i++) { - pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } - pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, 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); int paramcheck_counter = 0; @@ -401,7 +401,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; local_pos_sp.x += sp_move_rate[0] * dt; local_pos_sp.y += sp_move_rate[1] * dt; - /* limit maximum setpoint from position offset and preserve direction */ + /* limit maximum setpoint from position offset and preserve direction + * fail safe, should not happen in normal operation */ float pos_vec_x = local_pos_sp.x - local_pos.x; float pos_vec_y = local_pos_sp.y - local_pos.y; float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; |