這個部分是整個云臺控制的核心流程，通過分析他可以了解整個云臺在一個周期內(nèi)所進(jìn)行的計(jì)算，了解整個控制流程，對于我來說還是太復(fù)雜了，歡迎大家一起幫助我完善。
void loop()//主循環(huán)函數(shù)
{
int32_t pitchPIDVal;//用來存儲俯仰軸的PID計(jì)算結(jié)果
int32_t rollPIDVal;//用來存儲橫滾軸的PID計(jì)算結(jié)果
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就是電機(jī)中斷程序的標(biāo)志位，當(dāng)它為ture時候，表示中斷程序執(zhí)行一次，并執(zhí)行完畢，也就是說電機(jī)的狀態(tài)發(fā)生了變化。我們這里可以提前看一下中斷程序中都做了什么：
這里插入一下中斷程序代碼：
ISR( TIMER1_OVF_vect )//ISR是ardunio的庫函數(shù)，是專門用來處理中斷的程序，參數(shù)TIMER1_OVF_vect是中斷向量也就是中斷觸發(fā)，TIMER1_OVF_vect就是當(dāng)TIMER1溢出的時候?yàn)門RUE，具體這個溢出周期時間是多少呢，需要看TIMER1的具體設(shè)置了。在BLcontroller.h中有這么一行：TIMSK1 |= _BV(TOIE1);這句代碼就是開啟timer1的溢出中斷。而具體timer1如何配置是跟選擇PWM的輸出頻率有關(guān)，8KHZ，32KHZ，4KHZ三種選擇。這里已32KHZ設(shè)置舉例，
TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);//timer1為phase
?corrected,8bit模式 PWM模式，最大值為）0xFF（255），A通道和B通道都為升序清零，降序置位
TCCR1B = _BV(CS10);//表示不分頻，那就是32KHz，但是什么時候去觸發(fā)溢出寄存器置位還是不清楚。。。有待繼續(xù)學(xué)習(xí)
{
freqCounter++;//初始化改值為0，每次觸發(fā)一次溢出中斷，該數(shù)值+1，
if(freqCounter==(CC_FACTOR*1000/MOTORUPDATE_FREQ))//CC_FACTOR*1000代表PWM的頻率，定義MOTORUPDATE_FREQ的是電機(jī)狀態(tài)更新頻率是500HZ，這句代碼的含義就是每到需要更新電機(jī)狀態(tài)的時候執(zhí)行下列語句。
{
freqCounter=0;//把計(jì)數(shù)器清零
PWM_A_MOTOR0 = pwm_a_motor0;//默認(rèn)pwm_a_motor0=128，把驅(qū)動電機(jī)的PWM占空比設(shè)置為計(jì)算出的數(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;//并將電機(jī)狀態(tài)更新置位，標(biāo)明狀態(tài)已經(jīng)更新。
}
// care for standard timers every 1 ms
if ((freqCounter & 0x01f) == 0) {
TIMER0_isr_emulation();//這里是什么意思還有待研究
}
}
{
motorUpdate = false;//將電機(jī)狀態(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) {//當(dāng)不在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的計(jì)算頻率與電機(jī)更新狀態(tài)頻率要保持一致，因此PID的DT_INT為2ms，頻率為500HZ，通過計(jì)算得出新的俯仰軸輸出值。
// motor control
pitchMotorDrive = pitchPIDVal * config.dirMotorPitch;//默認(rèn)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);//同理計(jì)算出橫滾的PID輸出值
// motor control
rollMotorDrive = rollPIDVal * config.dirMotorRoll;
}
// motor update t=6us (*)
if (enableMotorUpdates)//
{
// set pitch motor pwm
MoveMotorPosSpeed(config.motorNumberPitch, pitchMotorDrive, maxPWMmotorPitchScaled);//真正驅(qū)動電機(jī)來自這一行，這個函數(shù)定義在BLcontroller.h中，這里詳細(xì)分析一下
void MoveMotorPosSpeed(uint8_t motorNumber, int MotorPos, uint16_t maxPWM)//這個函數(shù)具有三個參數(shù)，1.控制電機(jī)的序號，2.電機(jī)的位置.3PWM的放大因數(shù)，意義是控制具體是向哪個電機(jī)以正弦表中那三個值來生成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計(jì)算出的位置與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)];//驅(qū)動電機(jī)的三項(xiàng)PWM電角度應(yīng)該相差120°，360°/120°=3，也就是把正弦數(shù)組的長度（256）除以3分為三份，每每兩項(xiàng)之間在數(shù)組中相隔85（256/3）個元素,那么我們再看一下正弦數(shù)組是怎么計(jì)算的呢，根據(jù)其定義該數(shù)組長度256，也就是說將一個電角度周期劃為了256等分，數(shù)組中數(shù)值大小范圍是-128到127，但是這里存在一個問題，因?yàn)檫@里（posStep+170）是不能超過255的，因此posStep值不能超過85？但是并沒有看到程序中有對應(yīng)的限制呢。睡了一晚我突然明白了，他把這里變成了一個無符號數(shù)，然后如果相加過255就會溢出，那么這里最后就是溢出的那個值，恰好滿足了要求，因此posStep的范圍還是0-255。但是posStep的范圍是根據(jù)PID計(jì)算中進(jìn)行限定的，在PID函數(shù)的語句中最后output除以4096又除以8，原來output是32位的有符號整形，除以4096又除以8相當(dāng)于右移動15位，就還剩下17位，如果是正數(shù)的話，其中最高位是符號位，剩下16位為數(shù)據(jù)位。數(shù)據(jù)范圍為0-65535，在這個函數(shù)中又與posStep = MotorPos & 0xff;相當(dāng)于只保留低八位，數(shù)據(jù)范圍最終變?yōu)?-255.之所以這么做，是要保證PID的精度，不要浮點(diǎn)數(shù)減少運(yùn)算量，將所有的數(shù)都放大，保證都在整數(shù)范圍內(nèi)計(jì)算，最后再將數(shù)據(jù)變小到0-255的范圍內(nèi)。
// apply power factor
pwm_a = maxPWM * pwm_a;//maxPWM是一個0~256的數(shù)，乘以一個-127-127的數(shù)，范圍會變話，但是下面緊接著又向右移動8位，相當(dāng)于是pwm_a乘以了一個小于等于1的數(shù)，范圍還是在-127到127變化，但是應(yīng)該是變小了。
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占空比值送到控制對應(yīng)電機(jī)的通道中
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);
}//控制橫滾軸的電機(jī)運(yùn)動
// Evaluate RC-Signals, td = 120 us//這一段跟遙控信號有關(guān)，暫時先不分析
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
