dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

multirotor_pos_control: limit xy velocity integral output to tilt_max / 2

This commit is contained in:
Anton Babushkin 2013-07-25 20:05:45 +04:00
parent 8dd5130d99
commit a013fc5d0b
1 changed files with 10 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/multirotor_pos_control/multirotor_pos_control.c
View File

@ -248,9 +248,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_status), state_sub, &status);
/* check parameters at 1 Hz*/
paramcheck_counter++;
if (paramcheck_counter == 50) {
if (--paramcheck_counter <= 0) {
paramcheck_counter = 50;
bool param_updated;
orb_check(param_sub, &param_updated);
@ -259,15 +258,19 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
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);
// TODO 1000.0 is hotfix
pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1000.0f, params.tilt_max);
/* use integral_limit_out = tilt_max / 2 */
float i_limit;
if (params.xy_vel_i == 0.0) {
i_limit = params.tilt_max / params.xy_vel_i / 2.0;
} else {
i_limit = 1.0f; // not used really
}
pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
}
pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min);
}
paramcheck_counter = 0;
}
/* only check global position setpoint updates but not read */