diff options
author | tnaegeli <naegelit@student.ethz.ch> | 2012-10-04 09:28:04 +0200 |
---|---|---|
committer | tnaegeli <naegelit@student.ethz.ch> | 2012-10-04 09:28:04 +0200 |
commit | 733975ed2d7b5906e35dbdebad52ee8fa9d92fd6 (patch) | |
tree | c567798e5310693457137bfd0f55fb94bdc8dd53 /apps/multirotor_att_control/multirotor_rate_control.c | |
parent | 147c5bb66429c3d1b7c693d7419ca153ae49336c (diff) | |
download | px4-firmware-733975ed2d7b5906e35dbdebad52ee8fa9d92fd6.tar.gz px4-firmware-733975ed2d7b5906e35dbdebad52ee8fa9d92fd6.tar.bz2 px4-firmware-733975ed2d7b5906e35dbdebad52ee8fa9d92fd6.zip |
fixed Rate controller
Diffstat (limited to 'apps/multirotor_att_control/multirotor_rate_control.c')
-rw-r--r-- | apps/multirotor_att_control/multirotor_rate_control.c | 27 |
1 files changed, 6 insertions, 21 deletions
diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 2a57f93f0..1d400f51b 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -150,11 +150,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; @@ -166,13 +161,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (motor_skip_counter % 500 == 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); } /* calculate current control outputs */ @@ -183,15 +171,12 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, float setPitchRate=rate_sp->pitch; float setYawRate=rate_sp->yaw; - - //x-axis - setpointXrate=p.attrate_p*(setRollRate-gyro_filtered[0]); - //Y-axis - setpointYrate=p.attrate_p*(setPitchRate-gyro_filtered[1]); - //Z-axis - setpointZrate=p.yawrate_p*(setYawRate-gyro_filtered[2]); - - + //x-axis + setpointXrate=p.attrate_p*(setRollRate-rates[0]); + //Y-axis + setpointYrate=p.attrate_p*(setPitchRate-rates[1]); + //Z-axis + setpointZrate=p.yawrate_p*(setYawRate-rates[2]); actuators->control[0] = setpointXrate; //roll actuators->control[1] = setpointYrate; //pitch |