fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 03:31:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

Cleaned up PI wrapping code, still subject to testing

Browse Source
This commit is contained in:
Lorenz Meier
2012-11-20 15:19:51 +01:00
parent 129e6d73de
commit 6ff4520904
10 changed files with 121 additions and 174 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/multirotor_att_control/multirotor_attitude_control.c
+12 -10
View File
@@ -158,7 +158,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
}
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators, bool control_yaw_position)
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position)
{
static uint64_t last_run = 0;
static uint64_t last_input = 0;
@@ -201,8 +201,6 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
/* update parameters from storage */
parameters_update(&h, &p);
//printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
/* apply parameters */
pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
@@ -220,15 +218,19 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
if(control_yaw_position) {
/* control yaw rate */
//rates_sp->yaw = p.yaw_p * atan2f(cosf(att->yaw - att_sp->yaw_tait_bryan), sinf(att->yaw - att_sp->yaw_tait_bryan)) - (p.yaw_d * att->yawspeed);
yaw_error = att->yaw - att_sp->yaw_tait_bryan;
if ((double)yaw_error > M_PI) {
yaw_error -= M_PI;
} else if ((double)yaw_error < -M_PI) {
yaw_error += M_PI;
/* positive error: rotate to right, negative error, rotate to left (NED frame) */
// yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw);
yaw_error = att_sp->yaw_body - att->yaw;
if (yaw_error > M_PI_F) {
yaw_error -= M_TWOPI_F;
} else if (yaw_error < -M_PI_F) {
yaw_error += M_TWOPI_F;
}
rates_sp->yaw = - p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed);
rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed);
}
rates_sp->thrust = att_sp->thrust;