diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-04 10:56:55 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-04 10:56:55 +0200 |
commit | 2a06b66845542b05e3cad3d21099e33adc213227 (patch) | |
tree | 3b10d28cba955525c8ac37c7d1d348d0a91ac16a /apps/multirotor_att_control/multirotor_rate_control.c | |
parent | dfae108e6aff6e77eb05def50d99fb5c6d2c28c8 (diff) | |
download | px4-firmware-2a06b66845542b05e3cad3d21099e33adc213227.tar.gz px4-firmware-2a06b66845542b05e3cad3d21099e33adc213227.tar.bz2 px4-firmware-2a06b66845542b05e3cad3d21099e33adc213227.zip |
Fixed inner yaw rate loop
Diffstat (limited to 'apps/multirotor_att_control/multirotor_rate_control.c')
-rw-r--r-- | apps/multirotor_att_control/multirotor_rate_control.c | 79 |
1 files changed, 31 insertions, 48 deletions
diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index b4eb3469b..d332660f6 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -141,11 +141,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static int motor_skip_counter = 0; - // static PID_t yaw_pos_controller; - static PID_t yaw_speed_controller; - static PID_t pitch_controller; - static PID_t roll_controller; - static struct mc_rate_control_params p; static struct mc_rate_control_param_handles h; @@ -155,37 +150,25 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (initialized == false) { parameters_init(&h); parameters_update(&h, &p); - - pid_init(&yaw_speed_controller, p.yawrate_p, 0, p.yawrate_i, p.yawrate_awu, - PID_MODE_DERIVATIV_SET); - pid_init(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu, - PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu, - PID_MODE_DERIVATIV_SET); - initialized = true; } /* load new parameters with lower rate */ - if (motor_skip_counter % 250 == 0) { + if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - /* apply parameters */ - pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu); - pid_set_parameters(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu); - pid_set_parameters(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu); + printf("p.yawrate_p: %8.4f", (double)p.yawrate_p); } /* calculate current control outputs */ /* control pitch (forward) output */ - float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch, - rates[1], 0.0f, deltaT); + float pitch_control = 5 * deltaT * (rates[1] - rate_sp->pitch); /* control roll (left/right) output */ - float roll_control = pid_calculate(&roll_controller, rate_sp->roll, - rates[0], 0.0f, deltaT); + float roll_control = p.attrate_p * deltaT * (rates[0] - rate_sp->roll); + /* control yaw rate */ - float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); + float yaw_rate_control = p.yawrate_p * deltaT * (rates[2] - rate_sp->yaw); /* * compensate the vertical loss of thrust @@ -215,37 +198,37 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* compensate thrust vector for roll / pitch contributions */ motor_thrust *= zcompensation; - /* limit yaw rate output */ - if (yaw_rate_control > p.yawrate_lim) { - yaw_rate_control = p.yawrate_lim; - yaw_speed_controller.saturated = 1; - } + // /* limit yaw rate output */ + // if (yaw_rate_control > p.yawrate_lim) { + // yaw_rate_control = p.yawrate_lim; + // yaw_speed_controller.saturated = 1; + // } - if (yaw_rate_control < -p.yawrate_lim) { - yaw_rate_control = -p.yawrate_lim; - yaw_speed_controller.saturated = 1; - } + // if (yaw_rate_control < -p.yawrate_lim) { + // yaw_rate_control = -p.yawrate_lim; + // yaw_speed_controller.saturated = 1; + // } - if (pitch_control > p.attrate_lim) { - pitch_control = p.attrate_lim; - pitch_controller.saturated = 1; - } + // if (pitch_control > p.attrate_lim) { + // pitch_control = p.attrate_lim; + // pitch_controller.saturated = 1; + // } - if (pitch_control < -p.attrate_lim) { - pitch_control = -p.attrate_lim; - pitch_controller.saturated = 1; - } + // if (pitch_control < -p.attrate_lim) { + // pitch_control = -p.attrate_lim; + // pitch_controller.saturated = 1; + // } - if (roll_control > p.attrate_lim) { - roll_control = p.attrate_lim; - roll_controller.saturated = 1; - } + // if (roll_control > p.attrate_lim) { + // roll_control = p.attrate_lim; + // roll_controller.saturated = 1; + // } - if (roll_control < -p.attrate_lim) { - roll_control = -p.attrate_lim; - roll_controller.saturated = 1; - } + // if (roll_control < -p.attrate_lim) { + // roll_control = -p.attrate_lim; + // roll_controller.saturated = 1; + // } actuators->control[0] = roll_control; actuators->control[1] = pitch_control; |