diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-27 17:13:52 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-27 17:13:52 +0100 |
commit | a6f2c6022ec039fa6ccee03bc139794faa290265 (patch) | |
tree | 874f2eb7bfdc58d7ca23ac82cc73e462ab685fd9 /apps/multirotor_att_control | |
parent | b2068b4e0e43b8ab5088bba3e84cff18178dfafa (diff) | |
parent | d4edf2e85c4238387872eb5ee6bc1187117a280d (diff) | |
download | px4-firmware-a6f2c6022ec039fa6ccee03bc139794faa290265.tar.gz px4-firmware-a6f2c6022ec039fa6ccee03bc139794faa290265.tar.bz2 px4-firmware-a6f2c6022ec039fa6ccee03bc139794faa290265.zip |
Merge branch 'fixedwing_outdoor' of https://github.com/julianoes/Firmware into fixedwing_outdoor
Diffstat (limited to 'apps/multirotor_att_control')
-rw-r--r-- | apps/multirotor_att_control/multirotor_attitude_control.c | 9 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_rate_control.c | 44 |
2 files changed, 26 insertions, 27 deletions
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index e94d7c55d..b98736cee 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -197,7 +197,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s } /* load new parameters with lower rate */ - if (motor_skip_counter % 1000 == 0) { + if (motor_skip_counter % 500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); @@ -206,6 +206,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); } + /* reset integral if on ground */ + if(att_sp->thrust < 0.1f) { + pid_reset_integral(&pitch_controller); + pid_reset_integral(&roll_controller); + } + + /* calculate current control outputs */ /* control pitch (forward) output */ diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 93f7085ae..edb50a96e 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -150,16 +150,13 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, const float rates[], struct actuator_controls_s *actuators) { - static float roll_control_last = 0; - static float pitch_control_last = 0; static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; static uint64_t last_input = 0; - float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; - if (last_input != rate_sp->timestamp) { - last_input = rate_sp->timestamp; - } + static PID_t pitch_rate_controller; + static PID_t roll_rate_controller; + last_run = hrt_absolute_time(); @@ -175,37 +172,32 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_init(&h); parameters_update(&h, &p); initialized = true; + + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f,1000.0f, PID_MODE_DERIVATIV_CALC); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f,1000.0f, PID_MODE_DERIVATIV_CALC); + } /* load new parameters with lower rate */ - if (motor_skip_counter % 2500 == 0) { + if (motor_skip_counter % 500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); // warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz", // (double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input)); - } - /* calculate current control outputs */ - - /* control pitch (forward) output */ - float pitch_control = p.attrate_p * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last); - /* increase resilience to faulty control inputs */ - if (isfinite(pitch_control)) { - pitch_control_last = pitch_control; - } else { - pitch_control = 0.0f; - warnx("rej. NaN ctrl pitch"); + /* load new parameters with lower rate */ + parameters_update(&h, &p); + + /* apply parameters */ + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f); } + /* control roll (left/right) output */ - float roll_control = p.attrate_p * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last); - /* increase resilience to faulty control inputs */ - if (isfinite(roll_control)) { - roll_control_last = roll_control; - } else { - roll_control = 0.0f; - warnx("rej. NaN ctrl roll"); - } + float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , rates[0], 0.0f, deltaT); + float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , rates[1], 0.0f, deltaT); + /* control yaw rate */ float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); |