可以在crtp_commander_rpyt.c中调整如下参数选择
static RPYType stabilizationModeRoll = ANGLE; // Current stabilization type of roll (rate or angle)
static RPYType stabilizationModePitch = ANGLE; // Current stabilization type of pitch (rate or angle)
static RPYType stabilizationModeYaw = RATE; // Current stabilization type of yaw (rate or angle)
1.2 实现代码
void controllerPid(control_t *control, setpoint_t *setpoint,
const sensorData_t *sensors,
const state_t *state,
const uint32_t tick)
{
if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) { //该宏定义用于控制PID的计算频率,时间基准来自MPU6050触发的中断
// Rate-controled YAW is moving YAW angle setpoint
if (setpoint->mode.yaw == modeVelocity) { //rata模式,对yaw做修正
attitudeDesired.yaw += setpoint->attitudeRate.yaw * ATTITUDE_UPDATE_DT;
while (attitudeDesired.yaw > 180.0f)
attitudeDesired.yaw -= 360.0f;
while (attitudeDesired.yaw < -180.0f)
attitudeDesired.yaw += 360.0f;
} else { //attitude模式
attitudeDesired.yaw = setpoint->attitude.yaw;
}
}
if (RATE_DO_EXECUTE(POSITION_RATE, tick)) { //位置控制
positionController(&actuatorThrust, &attitudeDesired, setpoint, state);
}
if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
// Switch between manual and automatic position control
if (setpoint->mode.z == modeDisable) {
actuatorThrust = setpoint->thrust;
}
if (setpoint->mode.x == modeDisable || setpoint->mode.y == modeDisable) {
attitudeDesired.roll = setpoint->attitude.roll;
attitudeDesired.pitch = setpoint->attitude.pitch;
}
attitudeControllerCorrectAttitudePID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw,
attitudeDesired.roll, attitudeDesired.pitch, attitudeDesired.yaw,
&rateDesired.roll, &rateDesired.pitch, &rateDesired.yaw);
// For roll and pitch, if velocity mode, overwrite rateDesired with the setpoint
// value. Also reset the PID to avoid error buildup, which can lead to unstable
// behavior if level mode is engaged later
if (setpoint->mode.roll == modeVelocity) {
rateDesired.roll = setpoint->attitudeRate.roll;
attitudeControllerResetRollAttitudePID();
}
if (setpoint->mode.pitch == modeVelocity) {
rateDesired.pitch = setpoint->attitudeRate.pitch;
attitudeControllerResetPitchAttitudePID();
}
// TODO: Investigate possibility to subtract gyro drift.
attitudeControllerCorrectRatePID(sensors->gyro.x, -sensors->gyro.y, sensors->gyro.z,
rateDesired.roll, rateDesired.pitch, rateDesired.yaw);
attitudeControllerGetActuatorOutput(&control->roll,
&control->pitch,
&control->yaw);
control->yaw = -control->yaw;
}
if (tiltCompensationEnabled)
{
control->thrust = actuatorThrust / sensfusion6GetInvThrustCompensationForTilt();
}
else
{
control->thrust = actuatorThrust;
}
if (control->thrust == 0)
{
control->thrust = 0;
control->roll = 0;
control->pitch = 0;
control->yaw = 0;
attitudeControllerResetAllPID();
positionControllerResetAllPID();
// Reset the calculated YAW angle for rate control
attitudeDesired.yaw = state->attitude.yaw;
}
}
参数调整大致思路 PID coefficients can be optimized using Particle Swarm Optimization, or many other algorithms. This is called automatic tuning, and is my second goal for the refactoring I'm trying to do. For manual tuning, tune the Kp gain first, with Ki and Kd at 0. The Kp tuning will give you the majority of the stabilization. If you want smooth motion, a smaller Kp is the goal. For aerobatic maneuvers you'll want a higher Kp to force a faster return to the setpoint. After finding a stable Kp (Too high and you'll have marginal stability or oscillations), tune the Ki to help prevent drift and hold your heading. Finally tune the Kd to adjust the snappiness of returning to the setpoint. By snappiness, I mean how quickly it eliminates overshoot from the adjustment. 链接
二、Mellinger控制器
/* This controller is based on the following publication: Daniel Mellinger, Vijay Kumar: Minimum snap trajectory generation and control for quadrotors. IEEE International Conference on Robotics and Automation (ICRA), 2011.
Crazyflie project added the following:
Integral terms (compensates for: battery voltage drop over time, unbalanced center of mass due to asymmmetries, and uneven wear on propellers and motors)
D-term for angular velocityer
Support to use this controller as an attitude-only controller for manual flight
