<div>setPWM(pwm, phi, power)<br> -> pwm[3]</div>
PID pid.c
TIM 8,1,(5,4)
CCR1
CCR2
CCR3
<span style="font-size: 12px;">TIM8_channel_1 </span>占空比
pointingCmd - sensors.margAttitude500Hz<br>
<span style="font-size: 13px;">setPWMData( int *target, int *pwm)</span><br><span style="font-size: 13px;"> pwm [3] -> phase [3]</span>
angle=testPhase = -1.0f * D2R
MAX_CNT (PWM_PERIOD * 8 / 10) =800
<div>sensors.margAttitude500Hz[ROLL ] = atan2f(2.0f * (q0q1 + q2q3), <br> q0q0 - q1q1 - q2q2 + q3q3);</div><div>sensors.margAttitude500Hz[PITCH] = -asinf(2.0f * (q1q3 - q0q2));</div><div>sensors.margAttitude500Hz[YAW ] = atan2f(2.0f * (q1q2 + q0q3), <br> q0q0 + q1q1 - q2q2 - q3q3);</div>
<div>TIM8->CCR1 = rollPhase[0];</div>
maxCnt[ROLL] = 0;<br>minCnt[ROLL] = PWM_PERIOD + 1;<br>
cnt
TIM1_UP_IRQHandler
<span style="font-size: 13px;">TIM8->CNT = timer4timer5deadTimeDelay(80) + 5 + PWM_PERIOD * 2 / 3 (666) =751</span><br>
TIM1->CNT = timer4timer5deadTimeDelay + 3 + PWM_PERIOD / 3; = 416
TIM4->CNT = timer4timer5deadTimeDelay =80
TIM5->CNT <br>
TIM_BDTRInitStructure.TIM_DeadTime = timer1timer8deadTimeRegister =200<br>(this is not just a delay value, check CPU reference manual for TIMx_BDTR DTG bit 0-7)<br>
cnt 作用<br>make sure there is enough time to make all changes<br>
setPIDintegralError<br>
zeroPIDintegralError<br>
setPIDstates<br>
zeroPIDstates<br>
iTerm += error * deltaT<br>
<div> ANGULAR?</div><div> return(P * error +</div><div> I * PIDparameters->iTerm + </div><div> D * dAverage); </div><div>else</div><div> P * PIDparameters->B * command +</div><div> I * PIDparameters->iTerm +</div><div> D * dAverage -</div><div> P * state);//计算增量</div>
dTerm = (error - lastDcalcValue) / deltaT<br>
dTermFiltered-> dAverage<br>
<span style="font-size: 17px;">eepromConfig</span>
version
accelTCBiasSlope[3]
accelTCBiasIntercept[3];
gyroTCBiasSlope[3];
gyroTCBiasIntercept[3]
magBias[3]
accelCutoff
KpAcc,KiAcc,KpMag,KiMag
uint8_t dlpfSetting
midCommand
roll,pitch,yaw Power
roll,pitch,yaw Enabled
<div> uint8_t </div><div> float </div>
rollAutoPanEnabled;
pitchAutoPanEnabled;
yawAutoPanEnabled;
imuOrientation
rollMotorPoles
pitchMotorPoles
yawMotorPoles
rateLimit
rollRateCmdInput
pitchRateCmdInput
yawRateCmdInput
gimbalRoll.Pitch,YawRate;
gimbalRoll,pitch,yaw; Left Right Limit;
accel X,Y,Z 500HzLowPassTau;
roll,pitch,yaw RatePointingCmd50HzLowPassTau;
roll,pitch,yaw AttPointingCmd50HzLowPassTau;
PIDdata_t PID[NUMBER_OF_PIDS]
B, P, I, D
iTerm
windupGuard
lastDcalcValue
lastDterm
lastLastDterm
dErrorCalc
type
initPID
autoPan
step = MOTORPOS2SETPNT (0.35)* motorPos
stepSmooth = (stepSmooth * (AUTOPANSMOOTH(40) - 1.0f) + step) <br> / AUTOPANSMOOTH
return (setpoint -= stepSmooth)(滞后滤波=平滑)
computeMotorCommands
drv_pwmMotors
updateCounter
setupPWMIrq
TIM8_UP_IRQHandler
TIM1_UP_IRQHandler
TIM5_IRQHandler
timerChannelConfig
timerPWMadvancedConfig
timerPWMgeneralConfig
setPWM
setPWMFastTable
setPWMData
limitYawPWM
activateIRQ
set Roll,pitch,yaw Motor
forceMotorUpdate
pwmMotorDriverInit
autoPan
step = MOTORPOS2SETPNT * motorPos
stepSmooth = (stepSmooth * (AUTOPANSMOOTH - 1.0f) + step) <br> / AUTOPANSMOOTH
return (setpoint -= stepSmooth)