文件名：_BruGi第三部分（loop 主循环 第一部分）
这个部分是整个云台控制的核心流程，通过分析他可以了解整个云台在一个周期内所进行的计算，了解整个控制流程，对于我来说还是太复杂了，欢迎大家一起帮助我完善。
void loop()//主循环函数
{
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时候，表示中断程序执行一次，并执行完毕，也就是说电机的状态发生了变化。我们这里可以提前看一下中断程序中都做了什么：
这里插入一下中断程序代码：
ISR( TIMER1_OVF_vect )//ISR是ardunio的库函数，是专门用来处理中断的程序，参数TIMER1_OVF_vect是中断向量也就是中断触发，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，但是什么时候去触发溢出寄存器置位还是不清楚。。。有待继续学习
{
freqCounter++;//初始化改值为0，每次触发一次溢出中断，该数值+1，
if(freqCounter==(CC_FACTOR*1000/MOTORUPDATE_FREQ))//CC_FACTOR*1000代表PWM的频率，定义MOTORUPDATE_FREQ的是电机状态更新频率是500HZ，这句代码的含义就是每到需要更新电机状态的时候执行下列语句。
{
freqCounter=0;//把计数器清零
PWM_A_MOTOR0 = pwm_a_motor0;//默认pwm_a_motor0=128，把驱动电机的PWM占空比设置为计算出的数据
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;//并将电机状态更新置位，标明状态已经更新。
}
// care for standard timers every 1 ms
if ((freqCounter & 0x01f) == 0) {
TIMER0_isr_emulation();//这里是什么意思还有待研究
}
}
{
motorUpdate = false;//将电机状态更新置位
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//以上这几行是获取最新的状态参数
//****************************
// 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的计算频率与电机更新状态频率要保持一致，因此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);//真正驱动电机来自这一行，这个函数定义在BLcontroller.h中，这里详细分析一下
void MoveMotorPosSpeed(uint8_t motorNumber, int MotorPos, uint16_t maxPWM)//这个函数具有三个参数，1.控制电机的序号，2.电机的位置.3PWM的放大因数，意义是控制具体是向哪个电机以正弦表中那三个值来生成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型数据，16位吧，现在与0xff相与，意味着只保留第八位的数据。也就是值的范围控制在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，也就是把正弦数组的长度（256）除以3分为三份，每每两项之间在数组中相隔85（256/3）个元素,那么我们再看一下正弦数组是怎么计算的呢，根据其定义该数组长度256，也就是说将一个电角度周期划为了256等分，数组中数值大小范围是-128到127，但是这里存在一个问题，因为这里（posStep+170）是不能超过255的，因此posStep值不能超过85？但是并没有看到程序中有对应的限制呢。睡了一晚我突然明白了，他把这里变成了一个无符号数，然后如果相加过255就会溢出，那么这里最后就是溢出的那个值，恰好满足了要求，因此posStep的范围还是0-255。但是posStep的范围是根据PID计算中进行限定的，在PID函数的语句中最后output除以4096又除以8，原来output是32位的有符号整形，除以4096又除以8相当于右移动15位，就还剩下17位，如果是正数的话，其中最高位是符号位，剩下16位为数据位。数据范围为0-65535，在这个函数中又与posStep = MotorPos & 0xff;相当于只保留低八位，数据范围最终变为0-255.之所以这么做，是要保证PID的精度，不要浮点数减少运算量，将所有的数都放大，保证都在整数范围内计算，最后再将数据变小到0-255的范围内。
// apply power factor
pwm_a = maxPWM * pwm_a;//maxPWM是一个0~256的数，乘以一个-127-127的数，范围会变话，但是下面紧接着又向右移动8位，相当于是pwm_a乘以了一个小于等于1的数，范围还是在-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
