這個部分是整個云臺控制的核心流程，通過分析他可以了解整個云臺在一個周期內所進行的計算，了解整個控制流程，對于我來說還是太復雜了，歡迎大家一起幫助我完善。

void loop()//主循環(huán)函數(shù)

{

int32_t pitchPIDVal;//用來存儲俯仰軸的PID計算結果

int32_t rollPIDVal;//用來存儲橫滾軸的PID計算結果

static char pOutCnt = 0;

static char tOutCnt = 0;

static char tOutCntSub = 0;

static int stateCount = 0;

static uint8_t ledBlinkCnt = 0;

static uint8_t ledBlinkOnTime = 10;

static uint8_t ledBlinkPeriod = 20;

if (motorUpdate) // loop runs with motor ISR update rate (500 Hz)，motorUpdate就是電機中斷程序的標志位，當它為ture時候，表示中斷程序執(zhí)行一次，并執(zhí)行完畢，也就是說電機的狀態(tài)發(fā)生了變化。我們這里可以提前看一下中斷程序中都做了什么：

這里插入一下中斷程序代碼：

ISR( TIMER1_OVF_vect )//ISR是ardunio的庫函數(shù)，是專門用來處理中斷的程序，參數(shù)TIMER1_OVF_vect是中斷向量也就是中斷觸發(fā)，TIMER1_OVF_vect就是當TIMER1溢出的時候為TRUE，具體這個溢出周期時間是多少呢，需要看TIMER1的具體設置了。在BLcontroller.h中有這么一行：TIMSK1 |= _BV(TOIE1);這句代碼就是開啟timer1的溢出中斷。而具體timer1如何配置是跟選擇PWM的輸出頻率有關，8KHZ，32KHZ，4KHZ三種選擇。這里已32KHZ設置舉例，

TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);//timer1為phase

?corrected,8bit模式 PWM模式，最大值為）0xFF（255），A通道和B通道都為升序清零，降序置位

TCCR1B = _BV(CS10);//表示不分頻，那就是32KHz，但是什么時候去觸發(fā)溢出寄存器置位還是不清楚。。。有待繼續(xù)學習

{

freqCounter++;//初始化改值為0，每次觸發(fā)一次溢出中斷，該數(shù)值+1，

if(freqCounter==(CC_FACTOR*1000/MOTORUPDATE_FREQ))//CC_FACTOR*1000代表PWM的頻率，定義MOTORUPDATE_FREQ的是電機狀態(tài)更新頻率是500HZ，這句代碼的含義就是每到需要更新電機狀態(tài)的時候執(zhí)行下列語句。

{

freqCounter=0;//把計數(shù)器清零

PWM_A_MOTOR0 = pwm_a_motor0;//默認pwm_a_motor0=128，把驅動電機的PWM占空比設置為計算出的數(shù)據(jù)

PWM_B_MOTOR0 = pwm_b_motor0;

PWM_C_MOTOR0 = pwm_c_motor0;

PWM_A_MOTOR1 = pwm_a_motor1;

PWM_B_MOTOR1 = pwm_b_motor1;

PWM_C_MOTOR1 = pwm_c_motor1;

// update event

motorUpdate = true;//并將電機狀態(tài)更新置位，標明狀態(tài)已經(jīng)更新。

}

// care for standard timers every 1 ms

if ((freqCounter & 0x01f) == 0) {

TIMER0_isr_emulation();//這里是什么意思還有待研究

}

}

{

motorUpdate = false;//將電機狀態(tài)更新置位

CH2_ON

// loop period

//? ? 2.053/2.035 ms max/min, error = +5/-13 us (w/o rc)

//? ? 2.098/2.003 ms max/min, error = +50/-45 us (1 x PPM16 1 x PWM)

// update IMU data

readGyros();? // td = 330us

if (config.enableGyro) updateGyroAttitude(); // td = 176 us

if (config.enableACC) updateACCAttitude(); // td = 21 us

getAttiduteAngles(); // td = 372 us//以上這幾行是獲取最新的狀態(tài)參數(shù)

//****************************

// pitch PID

//****************************

if (fpvModeFreezePitch==false) {//當不在FPV模式下鎖定俯仰軸

// td = 92 us

pitchPIDVal = ComputePID(DT_INT_MS, DT_INT_INV, angle[PITCH], pitchAngleSet*1000, &pitchErrorSum, &pitchErrorOld, pitchPIDpar.Kp, pitchPIDpar.Ki, pitchPIDpar.Kd);//PID的計算頻率與電機更新狀態(tài)頻率要保持一致，因此PID的DT_INT為2ms，頻率為500HZ，通過計算得出新的俯仰軸輸出值。

// motor control

pitchMotorDrive = pitchPIDVal * config.dirMotorPitch;//默認config.dirMotorPitch為1

}

//****************************

// roll PID

//****************************

if (fpvModeFreezeRoll==false) {

// td = 92 us

rollPIDVal = ComputePID(DT_INT_MS, DT_INT_INV, angle[ROLL], rollAngleSet*1000, &rollErrorSum, &rollErrorOld, rollPIDpar.Kp, rollPIDpar.Ki, rollPIDpar.Kd);//同理計算出橫滾的PID輸出值

// motor control

rollMotorDrive = rollPIDVal * config.dirMotorRoll;

}

// motor update t=6us (*)

if (enableMotorUpdates)//

{

// set pitch motor pwm

MoveMotorPosSpeed(config.motorNumberPitch, pitchMotorDrive, maxPWMmotorPitchScaled);//真正驅動電機來自這一行，這個函數(shù)定義在BLcontroller.h中，這里詳細分析一下

void MoveMotorPosSpeed(uint8_t motorNumber, int MotorPos, uint16_t maxPWM)//這個函數(shù)具有三個參數(shù)，1.控制電機的序號，2.電機的位置.3PWM的放大因數(shù)，意義是控制具體是向哪個電機以正弦表中那三個值來生成PWM波，并且增益會是多少。

{

uint16_t posStep;

uint16_t pwm_a;

uint16_t pwm_b;

uint16_t pwm_c;

// fetch pwm from sinus table?

posStep = MotorPos & 0xff;//要將pid計算出的位置與0xff相與，MotorPos是一個int型數(shù)據(jù)，16位吧，現(xiàn)在與0xff相與，意味著只保留第八位的數(shù)據(jù)。也就是值的范圍控制在0-256

pwm_a = pwmSinMotor[(uint8_t)posStep];

pwm_b = pwmSinMotor[(uint8_t)(posStep + 85)];

pwm_c = pwmSinMotor[(uint8_t)(posStep + 170)];//驅動電機的三項PWM電角度應該相差120°，360°/120°=3，也就是把正弦數(shù)組的長度（256）除以3分為三份，每每兩項之間在數(shù)組中相隔85（256/3）個元素,那么我們再看一下正弦數(shù)組是怎么計算的呢，根據(jù)其定義該數(shù)組長度256，也就是說將一個電角度周期劃為了256等分，數(shù)組中數(shù)值大小范圍是-128到127，但是這里存在一個問題，因為這里（posStep+170）是不能超過255的，因此posStep值不能超過85？但是并沒有看到程序中有對應的限制呢。睡了一晚我突然明白了，他把這里變成了一個無符號數(shù)，然后如果相加過255就會溢出，那么這里最后就是溢出的那個值，恰好滿足了要求，因此posStep的范圍還是0-255。但是posStep的范圍是根據(jù)PID計算中進行限定的，在PID函數(shù)的語句中最后output除以4096又除以8，原來output是32位的有符號整形，除以4096又除以8相當于右移動15位，就還剩下17位，如果是正數(shù)的話，其中最高位是符號位，剩下16位為數(shù)據(jù)位。數(shù)據(jù)范圍為0-65535，在這個函數(shù)中又與posStep = MotorPos & 0xff;相當于只保留低八位，數(shù)據(jù)范圍最終變?yōu)?-255.之所以這么做，是要保證PID的精度，不要浮點數(shù)減少運算量，將所有的數(shù)都放大，保證都在整數(shù)范圍內計算，最后再將數(shù)據(jù)變小到0-255的范圍內。

// apply power factor

pwm_a = maxPWM * pwm_a;//maxPWM是一個0~256的數(shù)，乘以一個-127-127的數(shù)，范圍會變話，但是下面緊接著又向右移動8位，相當于是pwm_a乘以了一個小于等于1的數(shù)，范圍還是在-127到127變化，但是應該是變小了。

pwm_a = pwm_a >> 8;//

pwm_a += 128;//這里最終又加上128，將范圍變化到了1-255。

pwm_b = maxPWM * pwm_b;

pwm_b = pwm_b >> 8;

pwm_b += 128;

pwm_c = maxPWM * pwm_c;

pwm_c = pwm_c >> 8;

pwm_c += 128;

// set motor pwm variables

if (motorNumber == 0)

{

pwm_a_motor0 = (uint8_t)pwm_a;

pwm_b_motor0 = (uint8_t)pwm_b;

pwm_c_motor0 = (uint8_t)pwm_c;

}//將PWM占空比值送到控制對應電機的通道中

if (motorNumber == 1)

{

pwm_a_motor1 = (uint8_t)pwm_a;

pwm_b_motor1 = (uint8_t)pwm_b;

pwm_c_motor1 = (uint8_t)pwm_c;

}

}

// set roll motor pwm

MoveMotorPosSpeed(config.motorNumberRoll, rollMotorDrive, maxPWMmotorRollScaled);

}//控制橫滾軸的電機運動

// Evaluate RC-Signals, td = 120 us//這一段跟遙控信號有關，暫時先不分析

if (fpvModePitch==true) {

pitchAngleSet = utilLP3_float(qLPPitch, PitchPhiSet, rcLPFPitchFpv_tc);

} else if(config.rcAbsolutePitch==1) {

pitchAngleSet = utilLP3_float(qLPPitch, PitchPhiSet, rcLPFPitch_tc); // 63us

} else {

pitchAngleSet = utilLP3_float(qLPPitch, PitchPhiSet, LOWPASS_K_FLOAT(0.03));

}

if (fpvModeRoll==true) {

rollAngleSet = utilLP3_float(qLPRoll, RollPhiSet, rcLPFRollFpv_tc);

} else if(config.rcAbsoluteRoll==1) {

rollAngleSet = utilLP3_float(qLPRoll, RollPhiSet, rcLPFRoll_tc);

} else {

rollAngleSet = utilLP3_float(qLPRoll, RollPhiSet, LOWPASS_K_FLOAT(0.03));

}

// tElapsed = 1.250 ms