fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 20:33:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

fixed Rate controller

Browse Source
This commit is contained in:
tnaegeli
2012-10-04 09:28:04 +02:00
parent 147c5bb664
commit 733975ed2d
4 changed files with 63 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/multirotor_att_control/multirotor_rate_control.c
+6 -21
View File
@@ -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