|
楼主 |
发表于 2013-12-25 16:57:09
|
显示全部楼层
请看软件部分\bitcraze-crazyflie-firmware-26d661941c7e\modules\src\controller.c
void controllerCorrectRatePID(
float rollRateActual, float pitchRateActual, float yawRateActual,
float rollRateDesired, float pitchRateDesired, float yawRateDesired)
{
pidSetDesired(&pidRollRate, rollRateDesired);
TRUNCATE_SINT16(rollOutput, pidUpdate(&pidRollRate, rollRateActual, TRUE));
pidSetDesired(&pidPitchRate, pitchRateDesired);
TRUNCATE_SINT16(pitchOutput, pidUpdate(&pidPitchRate, pitchRateActual, TRUE));
pidSetDesired(&pidYawRate, yawRateDesired);
TRUNCATE_SINT16(yawOutput, pidUpdate(&pidYawRate, yawRateActual, TRUE));
}
void controllerCorrectAttitudePID(
float eulerRollActual, float eulerPitchActual, float eulerYawActual,
float eulerRollDesired, float eulerPitchDesired, float eulerYawDesired,
float* rollRateDesired, float* pitchRateDesired, float* yawRateDesired)
{
pidSetDesired(&pidRoll, eulerRollDesired);
*rollRateDesired = pidUpdate(&pidRoll, eulerRollActual, TRUE);
// Update PID for pitch axis
pidSetDesired(&pidPitch, eulerPitchDesired);
*pitchRateDesired = pidUpdate(&pidPitch, eulerPitchActual, TRUE);
// Update PID for yaw axis
float yawError;
yawError = eulerYawDesired - eulerYawActual;
if (yawError > 180.0)
yawError -= 360.0;
else if (yawError < -180.0)
yawError += 360.0;
pidSetError(&pidYaw, yawError);
*yawRateDesired = pidUpdate(&pidYaw, eulerYawActual, FALSE);
} |
|