|
KK的
while(1)
{
TimerRst();
CaclAttitude(); //Caculate plane attitude 计算飞行器姿态
TimerTo(1000);
//Do something between MotorControlBegin() and MotorControlEnd
//See functions declaration, can not exceed 1000us
//在MotorControlBegin()和MotorControlEnd()之间干点事儿,注意不要超过1000us
MotorControlBegin(); //Output head of ppm signal 输出PPM信号的头部分
PpmReadSignal(); //Read rx 读取接收机信号 140us
if(RxThr<RxThrLow) //If thr shutdown 如果油门关闭
{
GyroGainRead(); //Read gain 读取感度电位器 670us
ArmingRoutine(); //Arm/disarm 加锁解锁测试 5us
}
else
{
AxisMixer(); //Cacl motor signal 计算电机信号 305us
}
//If locked(arm) or no gyro base, shutdown all motor
//如果处于锁定态或者陀螺仪基准未建立,关闭所有马达
if(InLock || GyroBaseCnt || RxThr<5)
{
Motor1=Motor2=Motor3=Motor4=0;
}
//MOTOR6_L(); //I use it for test execute period 我用来观察执行时间的
MotorControlEnd(); //Output whole ppm signal 输出完整的PPM信号
}
这是KK的mian的循环
只看
AxisMixer(); //Cacl motor signal 计算电机信号 305us
void AxisMixer(void)
{
int thr,ail,ele,rud;
//Stick exp
//摇杆指数
if(BITTST(SoftSet,SOFT_EXP))
{
//Rudder do not need exp
thr=StickExp(RxThr);
ail=StickExp(RxAil)/2;
ele=StickExp(RxEle)/2;
}
else
{
thr=RxThr;
ail=RxAil/4;
ele=RxEle/4;
}
//Add gyro to adjustment
//将陀螺仪信号累加到调节量上
ail+=GainAdj(GyroRol,GainRol)+GainAdj(GyroRolI,GainPit);
ele+=GainAdj(GyroPit,GainRol)+GainAdj(GyroPitI,GainPit);
rud=RxRud/4+GainAdj(GyroYaw,GainYaw);//+GainAdj(GyroYawI,GainPit);
if(AxisMode==AXIS_CROSS)
{
// + Mode 十字模式
// 1
// 3 + 2
// 4
Motor1=MotorLimitValue(thr - ele + rud);
Motor2=MotorLimitValue(thr - ail - rud);
Motor3=MotorLimitValue(thr + ail - rud);
Motor4=MotorLimitValue(thr + ele + rud);
}
else
{
// X Mode X模式
// 1 2
// X
// 3 4
Motor1=MotorLimitValue(thr + ail - ele + rud);
Motor2=MotorLimitValue(thr - ail - ele - rud);
Motor3=MotorLimitValue(thr + ail + ele - rud);
Motor4=MotorLimitValue(thr - ail + ele + rud);
}
}
再具体就是平衡方面的计算了~还没看懂,就不贴了。 |
|