飞控程序教程解析.docx

红色是一些暂时没去顾及的部分，与我现在关心的地方并无太大关系。 函数对rcDate进行处理（去除死区，根据油门曲线，roll/pitch曲线，油门值，动态PID调整参数，在无头模式对于rcdata进行优化），生成rccommand值用于姿态控制。记录最大循环时间，最小循环时间，解锁时间，最大气压值。用LED表示一些传感器运行的状态。若定义了低压报警则进行电压测量。 rccommand【油门】在0-1000之间 ?rccommand【roll/pitch】-500 +500 ?小于1500为负 大于1500为正。 void annexCode()?// 每个循环都会执行，若执行时间少于650MS则不会影响主循环的执行。 {? ??static uint32_t calibratedAccTime; ??uint16_t tmp,tmp2; ??uint8_t axis,prop1,prop2; ? ??#define BREAKPOINT 1500 ??// PITCH ROLL 进行动态的PID参数调整, ?调整取决于油门大小 ??if (rcData[THROTTLE] ??{ ????prop2 = 100; ??}? ??else ????//油门大于1500 ??{ ????if (rcData[THROTTLE]2000) ??//油门小于2000 ????{ ???????prop2 = 100 - (uint16_t)conf.dynThrPID*(rcData[THROTTLE]-BREAKPOINT)/(2000-BREAKPOINT); //dynThrPID 四轴默认0 可以GUI设置 ??TPA参数 ?????????????PROP 与动态pid有关。dynThrPIDprop 越小 即油门大小对于PID影响越大。 ????}? ????else ??//油门大于2000 （不太可能） ????{ ???????prop2 = 100 - conf.dynThrPID; ????} ??} ??for(axis=0;axis3;axis++)? ??{ ?????tmp = min(abs(rcData[axis]-MIDRC),500); ?????#if defined(DEADBAND) ?????????//死区 ?即偏移小于死区的去掉 大于死区的减去死区范围 ???????if (tmpDEADBAND) { tmp -= DEADBAND; } ???????else { tmp=0; } ?????#endif ?????if(axis!=2)??//ROLL PITCH ?yawaxis=2 ?????{ ????????tmp2 = tmp/100; ??//偏移量除以100 ????????rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp-tmp2*100) * (lookupPitchRollRC[tmp2+1]-lookupPitchRollRC[tmp2]) / ??????100; // ?差值法求用于控制的rc信号，取决于roll pitch 曲线。 ????????prop1 = 100-(uint16_t)conf.rollPitchRate*tmp/500;//tmp=rc数据与中立值差得绝对值 ?rate [0 100]; ????????prop1 = (uint16_t)prop1*prop2/100; ????????????//根据prop2 修改 prop1? ??????}? ??????else?// YAW ????? ??????{ ????? ?????????rcCommand[axis] = tmp; ?????????prop1 = 100-(uint16_t)conf.yawRate*tmp/500; ??//tmp=RC距离中点值 ??yawRate[0 100] 默认为0 ???????} ???????dynP8[axis] = (uint16_t)conf.P8[axis]*prop1/100; ????????//动态YAW PID参数 ???????dynD8[axis] = (uint16_t)conf.D8[axis]*prop1/100; ???????if (rcData[axis] ??} ??tmp = constrain(rcData[THROTTLE],MINCHECK,2000); ????// 令tmp=rcData[THROTTLE]; (前面等于各个轴的输入数值) ??tmp = (uint32_t)(tmp-MINCHECK)*1000/(2000