MWC v2.2 代码解读annexCode()
红色是一些暂时没去顾及的部分，与我现在关心的地方并无太大关系。
函数对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;axis<3;axis++)
{
tmp = min(abs(rcData[axis]-MIDRC),500);
#if defined(DEADBAND) //死区 即偏移小于死区的去掉 大于死区的减去死区范围
if (tmp>DEADBAND) { 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-MINCHECK); // 把油门控制转化为【0 1000】
tmp2 = tmp/100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp-tmp2*100) * (lookupThrottleRC[tmp2+1]- lookupThrottleRC[tmp2]) / 100; //通过油门曲线对油门控制量进行换算
if(f.HEADFREE_MODE) //to optimize 开启HEADFREE模式时
{
float radDiff = (heading - headFreeModeHold) * 0.0174533f; // 转化为弧度 PI/180 ~= 0.0174533
float cosDiff = cos(radDiff); //求余弦
float sinDiff = sin(radDiff); //求正弦
int16_t rcCommand_PITCH = rcCommand[PITCH]*cosDiff + rcCommand[ROLL]*sinDiff;
rcCommand[ROLL] = rcCommand[ROLL]*cosDiff - rcCommand[PITCH]*sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
#if defined(POWERMETER_HARD)
uint16_t pMeterRaw; // used for current reading
static uint16_t psensorTimer = 0;
if (! (++psensorTimer % PSENSORFREQ)) {
pMeterRaw = analogRead(PSENSORPIN);
//lcdprint_int16(pMeterRaw); LCDcrlf();
powerValue = ( conf.psensornull > pMeterRaw ? conf.psensornull - pMeterRaw : pMeterRaw - conf.psensornull); // do not use abs(), it would induce implicit cast to uint and overrun
if ( powerValue < 333) { // only accept reasonable values. 333 is empirical
#ifdef LCD_TELEMETRY
if (powerValue > powerMax) powerMax = powerValue;
#endif
} else {
powerValue = 333;
}
pMeter[PMOTOR_SUM] += (uint32_t) powerValue;
}
#endif
#if defined(BUZZER) //关于低电压报警
#if defined(VBAT)
static uint8_t vbatTimer = 0;
static uint8_t ind = 0;
uint16_t vbatRaw = 0;
static uint16_t vbatRawArray[8];
if (! (++vbatTimer % VBATFREQ)) {
vbatRawArray[(ind++)%8] = analogRead(V_BATPIN);
for (uint8_t i=0;i<8;i++) vbatRaw += vbatRawArray[i];
vbat = (vbatRaw*2) / conf.vbatscale; // result is Vbatt in 0.1V steps
}
#endif
alarmHandler(); // external buzzer routine that handles buzzer events globally now
#endif
#if defined(RX_RSSI) //接收信号强度指示
static uint8_t sig = 0;
uint16_t rssiRaw = 0;
static uint16_t rssiRawArray[8];
rssiRawArray[(sig++)%8] = analogRead(RX_RSSI_PIN);
for (uint8_t i=0;i<8;i++) rssiRaw += rssiRawArray[i];
rssi = rssiRaw / 8;
#endif
if ( (calibratingA>0 && ACC ) || (calibratingG>0) ) // Calibration phasis
{
LEDPIN_TOGGLE; //初始化LED引脚 digital pin13
}
else
{
if (f.ACC_CALIBRATED) {LEDPIN_OFF;} //ACC校准结束 灯灭
if (f.ARMED) {LEDPIN_ON;} //解锁灯亮
}
#if defined(LED_RING)
static uint32_t LEDTime;
if ( currentTime > LEDTime )
{
LEDTime = currentTime + 50000; //50s闪烁一次led
i2CLedRingState();
}
#endif
#if defined(LED_FLASHER)
auto_switch_led_flasher();
#endif
if ( currentTime > calibratedAccTime ) 每100S检测一次
{
if (! f.SMALL_ANGLES_25) //倾斜太大 或者未校准ACC
{
f.ACC_CALIBRATED = 0; //校准标志位清0
LEDPIN_TOGGLE;
calibratedAccTime = currentTime + 100000;
}
else
{
f.ACC_CALIBRATED = 1;
}
}
#if !(defined(SPEKTRUM) && defined(PROMINI)) //Only one serial port on ProMini. Skip serial com if Spektrum Sat in use. Note: Spek code will auto-call serialCom if GUI data detected on serial0.
#if defined(GPS_PROMINI)
if(GPS_Enable == 0) {serialCom();}
#else
serialCom();
#endif
#endif
#if defined(POWERMETER) //功率计有关
intPowerMeterSum = (pMeter[PMOTOR_SUM]/conf.pleveldiv);
intPowerTrigger1 = conf.powerTrigger1 * PLEVELSCALE;
#endif
#ifdef LCD_TELEMETRY_AUTO
static char telemetryAutoSequence [] = LCD_TELEMETRY_AUTO;
static uint8_t telemetryAutoIndex = 0;
static uint16_t telemetryAutoTimer = 0;
if ( (telemetry_auto) && (! (++telemetryAutoTimer % LCD_TELEMETRY_AUTO_FREQ) ) ){
telemetry = telemetryAutoSequence[++telemetryAutoIndex % strlen(telemetryAutoSequence)];
LCDclear(); // make sure to clear away remnants
}
#endif
#ifdef LCD_TELEMETRY
static uint16_t telemetryTimer = 0;
if (! (++telemetryTimer % LCD_TELEMETRY_FREQ)) {
#if (LCD_TELEMETRY_DEBUG+0 > 0)
telemetry = LCD_TELEMETRY_DEBUG;
#endif
if (telemetry) lcd_telemetry();
}
#endif
#if GPS & defined(GPS_LED_INDICATOR) // 用LED来表示一些GPS的信息
static uint32_t GPSLEDTime; // - No GPS FIX -> LED blink at speed of incoming GPS frames
static uint8_t blcnt; // - Fix and sat no. bellow 5 -> LED off
if(currentTime > GPSLEDTime) { // - Fix and sat no. >= 5 -> LED blinks, one blink for 5 sat, two blinks for 6 sat, three for 7 ...
if(f.GPS_FIX && GPS_numSat >= 5) {
if(++blcnt > 2*GPS_numSat) blcnt = 0;
GPSLEDTime = currentTime + 150000;
if(blcnt >= 10 && ((blcnt%2) == 0)) {STABLEPIN_ON;} else {STABLEPIN_OFF;}
}else{
if((GPS_update == 1) && !f.GPS_FIX) {STABLEPIN_ON;} else {STABLEPIN_OFF;}
blcnt = 0;
}
}
#endif
#if defined(LOG_VALUES) && (LOG_VALUES >= 2) // 记录值
if (cycleTime > cycleTimeMax) cycleTimeMax = cycleTime; // 记录最大循环时间
if (cycleTime < cycleTimeMin) cycleTimeMin = cycleTime; // 记录最小循环时间
#endif
if (f.ARMED)
{
#if defined(LCD_TELEMETRY) || defined(ARMEDTIMEWARNING) || defined(LOG_PERMANENT)
armedTime += (uint32_t)cycleTime; //记录解锁时间
#endif
#if defined(VBAT)
if ( (vbat > conf.no_vbat) && (vbat < vbatMin) ) vbatMin = vbat; //记录最低电压值
#endif
#ifdef LCD_TELEMETRY
#if BARO
if ( (BaroAlt > BAROaltMax) ) BAROaltMax = BaroAlt; //记录最大气压值
#endif
#endif
}
}MWC v2.2 代码解读LOOP()
函数很长不用文字了 贴个流程图，说明一切：
void loop () {
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
static uint8_t rcSticks; // this hold sticks position for command combos
uint8_t axis,i;
int16_t error,errorAngle;
int16_t delta,deltaSum;
int16_t PTerm,ITerm,DTerm;
int16_t PTermACC = 0 , ITermACC = 0 , PTermGYRO = 0 , ITermGYRO = 0;
static int16_t lastGyro[3] = {0,0,0};
static int16_t delta1[3],delta2[3];
static int16_t errorGyroI[3] = {0,0,0};
static int16_t errorAngleI[2] = {0,0};
static uint32_t rcTime = 0;
static int16_t initialThrottleHold;
static uint32_t timestamp_fixated = 0;
#if defined(SPEKTRUM)
if (spekFrameFlags == 0x01) readSpektrum(); //支持的一种特殊遥控器 读取数据
#endif
#if defined(OPENLRSv2MULTI)
Read_OpenLRS_RC(); //支持的一种特殊的遥控器 读取数据
#endif
if (currentTime > rcTime ) // 50Hz 时间过了20ms
{
rcTime = currentTime + 20000;
computeRC(); //对已经接收的遥控接收的信号进行循环滤波，取4组数据，80MS，算平均值，大于平均值的减小2，小于平均值的增大2.
// Failsafe routine - added by MIS
#if defined(FAILSAFE)
if ( failsafeCnt > (5*FAILSAFE_DELAY) && f.ARMED) // 使之稳定, 并设置油门到指定的值
{
for(i=0; i<3; i++) rcData[i] = MIDRC; // 丢失信号(in 0.1sec)后，把所有通道数据设置为 MIDRC=1500
rcData[THROTTLE] = conf.failsafe_throttle; // 把油门设置为conf.failsafe_throttle
if (failsafeCnt > 5*(FAILSAFE_DELAY+FAILSAFE_OFF_DELAY)) // 在特定时间之后关闭电机 (in 0.1sec)
{
go_disarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // 进入锁定状态，之后起飞需要解锁
}
failsafeEvents++; //掉落保护事件标志位至1
}
if ( failsafeCnt > (5*FAILSAFE_DELAY) && !f.ARMED)
{ //Turn of "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm
go_disarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; //进入锁定状态，之后起飞需要解锁
}
failsafeCnt++; //掉落保护计数+1 每1 代表20ms 大于5倍FAILSAFE_DELAY 则进入保护
#endif
// end of failsafe routine - next change is made with RcOptions setting
// ------------------ STICKS COMMAND HANDLER --------------------
// 检测控制杆位置
uint8_t stTmp = 0;
for(i=0;i<4;i++)
{
stTmp >>= 2; //stTmp除以4
if(rcData[i] > MINCHECK) stTmp |= 0x80; // MINCHECK=1100 1000 0000B
if(rcData[i] < MAXCHECK) stTmp |= 0x40; // MAXCHECK=1900 0100 0000B 通过stTmp判断是否控制杆是否在最大最小之外
}
if(stTmp == rcSticks)
{
if(rcDelayCommand<250) rcDelayCommand++; //若控制杆在最大最小位置外的状态未改变（20ms内），则rcDelayCommand+1
}
else rcDelayCommand = 0; //否则清0
rcSticks = stTmp; //保存stTmp
// 采取行动
if (rcData[THROTTLE] <= MINCHECK) //油门在最低值
{
errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; errorGyroI[YAW] = 0; //把roll pitch yaw 误差置0
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
if (conf.activate[BOXARM] > 0) // Arming/Disarming via ARM BOX
{
if ( rcOptions[BOXARM] && f.OK_TO_ARM ) go_arm(); //解锁
else if (f.ARMED) go_disarm(); //上锁
}
}
if(rcDelayCommand == 20) //若控制杆在最大最小位置外的状态未改变(20*20ms)
{
if(f.ARMED) // 当处在解锁时
{
#ifdef ALLOW_ARM_DISARM_VIA_TX_YAW //上锁方式1
if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) go_disarm(); // Disarm via YAW
#endif
#ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL //上锁方式2
if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO) go_disarm(); // Disarm via ROLL
#endif
}
else // 当处在未解锁时
{
i=0;
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) // GYRO（陀螺仪） 校准
{
calibratingG=512; //校准G 512*20Ms
#if GPS
GPS_reset_home_position(); //GPS 设置HOME
#endif
#if BARO
calibratingB=10; // 气压计设置基准气压(10 * 25 ms = ~250 ms non blocking)
#endif
}
#if defined(INFLIGHT_ACC_CALIBRATION) //使得可以飞行中ACC校准 （怎么手在抖。。）
else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI) // Inflight ACC calibration START/STOP
{
if (AccInflightCalibrationMeasurementDone) // trigger saving into eeprom after landing
{
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
}
else
{
AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
#if defined(BUZZER)
if (AccInflightCalibrationArmed) alarmArray[0]=2; else alarmArray[0]=3;
#endif
}
}
#endif
#ifdef MULTIPLE_CONFIGURATION_PROFILES //多配置文件读取
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) i=1; // ROLL left -> Profile 1 //配置1
else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) i=2; // PITCH up -> Profile 2 //配置2
else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) i=3; // ROLL right -> Profile 3 //配置3
if(i)
{
global_conf.currentSet = i-1;
writeGlobalSet(0);
readEEPROM();
blinkLED(2,40,i);
alarmArray[0] = i;
}
#endif
if (rcSticks == THR_LO + YAW_HI + PIT_HI + ROL_CE) // 进入LCD配置
{
#if defined(LCD_CONF)
configurationLoop(); // beginning LCD configuration
#endif
previousTime = micros(); //设置时间
}
#ifdef ALLOW_ARM_DISARM_VIA_TX_YAW //允许使用YAW进行解锁
else if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) go_arm(); // Arm via YAW
#endif
#ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL
else if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI) go_arm(); // Arm via ROLL
#endif
#ifdef LCD_TELEMETRY_AUTO //与LCD有关 telemetry 遥测
else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) // Auto telemetry ON/OFF
{
if (telemetry_auto)
{
telemetry_auto = 0;
telemetry = 0;
}
else
telemetry_auto = 1;
}
#endif
#ifdef LCD_TELEMETRY_STEP
else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) // Telemetry next step
{
telemetry = telemetryStepSequence[++telemetryStepIndex % strlen(telemetryStepSequence)];
#ifdef OLED_I2C_128x64
if (telemetry != 0) i2c_OLED_init();
#endif
LCDclear();
}
#endif
else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) calibratingA=512; //加速度校准
else if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) f.CALIBRATE_MAG = 1; //电子罗盘校准
i=0;
if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {conf.angleTrim[PITCH]+=2; i=1;} //角度矫正 在计算PID时有用
else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {conf.angleTrim[PITCH]-=2; i=1;}
else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {conf.angleTrim[ROLL] +=2; i=1;}
else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {conf.angleTrim[ROLL] -=2; i=1;}
if (i)
{
writeParams(1);
rcDelayCommand = 0; // allow autorepetition
#if defined(LED_RING) //调整之后灯闪
blinkLedRing(); //灯闪烁 使用IIC接口
#endif
}
}
}
#if defined(LED_FLASHER)
led_flasher_autoselect_sequence(); //仍在20MS循环中，LED闪烁
#endif
#if defined(INFLIGHT_ACC_CALIBRATION) //空中校准ACC
if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > MINCHECK && !rcOptions[BOXARM] ){ // Copter is airborne and you are turning it off via boxarm : start measurement
InflightcalibratingA = 50;
AccInflightCalibrationArmed = 0;
}
if (rcOptions[BOXCALIB]) { // 使用Calib来标定 : Calib = TRUE 测试开始, 降落 且 Calib = 0 测量储存
if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone){ //若空中校准ACC未运行
InflightcalibratingA = 50; //设定校准时间 50
}
}else if(AccInflightCalibrationMeasurementDone && !f.ARMED){ //若结束 就保存
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
}
#endif
uint16_t auxState = 0; //测量辅助通道位置 小于1300 1300到1700 大于1700
for(i=0;i<4;i++)
auxState |= (rcData[AUX1+i]<1300)<<(3*i) | (1300<rcData[AUX1+i] && rcData[AUX1+i]<1700)<<(3*i+1) | (rcData[AUX1+i]>1700)<<(3*i+2);
for(i=0;i<CHECKBOXITEMS;i++)
rcOptions[i] = (auxState & conf.activate[i])>0; //将辅助通道位置与选择的开启方式比较，保存开启的模式
// note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAFE_DELAY is always false
#if ACC
if ( rcOptions[BOXANGLE] || (failsafeCnt > 5*FAILSAFE_DELAY) ) // 开启自稳模式anglemode
{
if (!f.ANGLE_MODE)
{
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
f.ANGLE_MODE = 1;
}
}
else // failsafe模式时的动作 {
f.ANGLE_MODE = 0;
}
if ( rcOptions[BOXHORIZON] ) //开启 HORIZON模式 rc选择
{
f.ANGLE_MODE = 0; //关闭angle模式
if (!f.HORIZON_MODE) //若horizon模式未开启
{
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
f.HORIZON_MODE = 1; //开启horizon模式
}
}
else //否则
{
f.HORIZON_MODE = 0; //关闭horizon模式
}
#endif
if (rcOptions[BOXARM] == 0) f.OK_TO_ARM = 1;
#if !defined(GPS_LED_INDICATOR)
if (f.ANGLE_MODE || f.HORIZON_MODE) {STABLEPIN_ON;} else {STABLEPIN_OFF;}
#endif
#if BARO
#if (!defined(SUPPRESS_BARO_ALTHOLD)) //若未宏定义SUPPRESS_BARO_ALTHOLD
if (rcOptions[BOXBARO]) //rc 若选择baro
{
if (!f.BARO_MODE) //若baro模式未开启
{
f.BARO_MODE = 1; //开启baro模式 气压计定高
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE]; //储存此时 rc 油门输出值
errorAltitudeI = 0; //重置PID输出 和高度误差
BaroPID=0;
}
} else //若RC未选择BARO模式
{
f.BARO_MODE = 0; //关闭baro模式
}
#endif
#ifdef VARIOMETER //若定义了VARIOMETER
if (rcOptions[BOXVARIO]) //rc 若选择vario模式
{
if (!f.VARIO_MODE)
{
f.VARIO_MODE = 1; //开启vario模式
}
} else //rc未选择VARIO模式
{
f.VARIO_MODE = 0; //关闭vario模式
}
#endif
#endif
#if MAG //若配置了磁场传感器
if (rcOptions[BOXMAG]) //开启磁场传感器 与上面开启各种模式一样
{
if (!f.MAG_MODE)
{
f.MAG_MODE = 1;
magHold = heading;
}
} else
{
f.MAG_MODE = 0;
}
if (rcOptions[BOXHEADFREE]) //开启无头模式与上面开启各种模式一样
{
if (!f.HEADFREE_MODE)
{
f.HEADFREE_MODE = 1;
}
}
else
{
f.HEADFREE_MODE = 0;
}
if (rcOptions[BOXHEADADJ])
{
headFreeModeHold = heading; // acquire new heading
}
#endif
#if GPS
static uint8_t GPSNavReset = 1;
if (f.GPS_FIX && GPS_numSat >= 5 )
{
if (rcOptions[BOXGPSHOME]) //若GPS_HOME 和 GPS_HOLD 都被选择了额 GPS_HOME 具有优先权
{
if (!f.GPS_HOME_MODE)
{
f.GPS_HOME_MODE = 1;
f.GPS_HOLD_MODE = 0;
GPSNavReset = 0;
#if defined(I2C_GPS)
GPS_I2C_command(I2C_GPS_COMMAND_START_NAV,0); //waypoint zero
#else // 串口
GPS_set_next_wp(&GPS_home[LAT],&GPS_home[LON]);
nav_mode = NAV_MODE_WP;
#endif
}
} else {
f.GPS_HOME_MODE = 0;
if (rcOptions[BOXGPSHOLD] && abs(rcCommand[ROLL])< AP_MODE && abs(rcCommand[PITCH]) < AP_MODE) {
if (!f.GPS_HOLD_MODE) {
f.GPS_HOLD_MODE = 1;
GPSNavReset = 0;
#if defined(I2C_GPS)
GPS_I2C_command(I2C_GPS_COMMAND_POSHOLD,0);
#else
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD;
#endif
}
} else {
f.GPS_HOLD_MODE = 0;
// both boxes are unselected here, nav is reset if not already done
if (GPSNavReset == 0 ) {
GPSNavReset = 1;
GPS_reset_nav();
}
}
}
} else {
f.GPS_HOME_MODE = 0;
f.GPS_HOLD_MODE = 0;
#if !defined(I2C_GPS)
nav_mode = NAV_MODE_NONE;
#endif
}
#endif
#if defined(FIXEDWING) || defined(HELICOPTER) //另外的机型的模式 与四轴无关
if (rcOptions[BOXPASSTHRU]) {f.PASSTHRU_MODE = 1;}
else {f.PASSTHRU_MODE = 0;}
#endif
} //RC循环到此为止
else //若未进入RC则依次进行以下5个任务
{
static uint8_t taskOrder=0; // 不把所有的任务放在一个循环中，避免高延迟使得RC循环无法进入。
if(taskOrder>4) taskOrder-=5;
switch (taskOrder) {
case 0:
taskOrder++;
#if MAG //获取MAG数据
if (Mag_getADC()) break; // max 350 µs (HMC5883) // only break when we actually did something
#endif
case 1:
taskOrder++;
#if BARO //获取BARO数据
if (Baro_update() != 0 ) break;
#endif
case 2:
taskOrder++;
#if BARO //获取BARO数据
if (getEstimatedAltitude() !=0) break;
#endif
case 3:
taskOrder++;
#if GPS //获取GPS数据
if(GPS_Enable) GPS_NewData();
break;
#endif
case 4:
taskOrder++;
#if SONAR //获取SONAR （声呐）数据
Sonar_update();debug[2] = sonarAlt;
#endif
#ifdef LANDING_LIGHTS_DDR
auto_switch_landing_lights();
#endif
#ifdef VARIOMETER
if (f.VARIO_MODE) vario_signaling();
#endif
break;
}
}
computeIMU(); //计算IMU（惯性测量单元）
// Measure loop rate just afer reading the sensors
currentTime = micros();
cycleTime = currentTime - previousTime;
previousTime = currentTime;
#ifdef CYCLETIME_FIXATED
if (conf.cycletime_fixated) {
if ((micros()-timestamp_fixated)>conf.cycletime_fixated) {
} else {
while((micros()-timestamp_fixated)<conf.cycletime_fixated) ; // waste away
}
timestamp_fixated=micros();
}
#endif
//***********************************
//**** Experimental FlightModes ***** 实验的飞行模式
//***********************************
#if defined(ACROTRAINER_MODE) //固定翼训练者模式
if(f.ANGLE_MODE)
{
if (abs(rcCommand[ROLL]) + abs(rcCommand[PITCH]) >= ACROTRAINER_MODE ) //ACROTRAINER_MODE=200
{
f.ANGLE_MODE=0; //取消自稳 定向 定高 GPS回家 GPS定点
f.HORIZON_MODE=0;
f.MAG_MODE=0;
f.BARO_MODE=0;
f.GPS_HOME_MODE=0;
f.GPS_HOLD_MODE=0;
}
}
#endif
//***********************************
#if MAG //磁场定向的算法 保持机头方向不变
if (abs(rcCommand[YAW]) <70 && f.MAG_MODE) //开启定高，且yaw控制值在1430-1570之间
{
int16_t dif = heading - magHold;
if (dif <= - 180) dif += 360; //转过头了从另一方向转回去
if (dif >= + 180) dif -= 360; //转过头了从另一方向转回去
if ( f.SMALL_ANGLES_25 ) rcCommand[YAW] -= dif*conf.P8[PIDMAG]>>5; //dif*pidmag/32
}
else magHold = heading;
#endif
#if BARO && (!defined(SUPPRESS_BARO_ALTHOLD)) //气压计定高算法
if (f.BARO_MODE) //若开启了气压定高
{
static uint8_t isAltHoldChanged = 0;
#if defined(ALTHOLD_FAST_THROTTLE_CHANGE) //默认开启的
if (abs(rcCommand[THROTTLE]-initialThrottleHold) > ALT_HOLD_THROTTLE_NEUTRAL_ZONE) //initialThrottleHold=开启气压定高时的油门值 ALT_HOLD_THROTTLE_NEUTRAL_ZONE=40； 控制量超过死区 则开始执行
{
errorAltitudeI = 0;
isAltHoldChanged = 1; //气压定点改变标志位
rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -ALT_HOLD_THROTTLE_NEUTRAL_ZONE : ALT_HOLD_THROTTLE_NEUTRAL_ZONE; //减去 或者加上 死区
}
else
{
if (isAltHoldChanged)
{
AltHold = EstAlt; //改变定高高度为现在估计高度 cm
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;//油门控制量=initialThrottleHold+PID控制量 非增量式PID控制
}
#else
static int16_t AltHoldCorr = 0;
if (abs(rcCommand[THROTTLE]-initialThrottleHold)>ALT_HOLD_THROTTLE_NEUTRAL_ZONE)
{
// 缓慢增加或减少气压定高的高度，其值与操作杆位移有关（+100的油门（与开启定高时相比）使其1s升高50cm（程序循环时间3-4ms））
AltHoldCorr+= rcCommand[THROTTLE] - initialThrottleHold;//每个循环累加
if(abs(AltHoldCorr) > 500) //累加大于500
{
AltHold += AltHoldCorr/500; //改变气压定高高度。单位cm
AltHoldCorr %= 500;
}
errorAltitudeI = 0;
isAltHoldChanged = 1; //高度改变标志位记1
}
else if (isAltHoldChanged)
{
AltHold = EstAlt;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;//油门控制量=initialThrottleHold+PID控制量 非增量式PID控制
#endif
}
#endif
#if GPS //与GPS有关
if ( (f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME ) {
float sin_yaw_y = sin(heading*0.0174532925f);
float cos_yaw_x = cos(heading*0.0174532925f);
#if defined(NAV_SLEW_RATE)
nav_rated[LON] += constrain(wrap_18000(nav[LON]-nav_rated[LON]),-NAV_SLEW_RATE,NAV_SLEW_RATE);
nav_rated[LAT] += constrain(wrap_18000(nav[LAT]-nav_rated[LAT]),-NAV_SLEW_RATE,NAV_SLEW_RATE);
GPS_angle[ROLL] = (nav_rated[LON]*cos_yaw_x - nav_rated[LAT]*sin_yaw_y) /10;
GPS_angle[PITCH] = (nav_rated[LON]*sin_yaw_y + nav_rated[LAT]*cos_yaw_x) /10;
#else
GPS_angle[ROLL] = (nav[LON]*cos_yaw_x - nav[LAT]*sin_yaw_y) /10;
GPS_angle[PITCH] = (nav[LON]*sin_yaw_y + nav[LAT]*cos_yaw_x) /10;
#endif
} else {
GPS_angle[ROLL] = 0;
GPS_angle[PITCH] = 0;
}
#endif
//**** PITCH & ROLL & YAW PID ****
int16_t prop;
prop = min(max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])),500); // range [0;500]prop pitch和roll控制量大的。
for(axis=0;axis<3;axis++)
{
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis<2 ) // 开启自稳或者HORIZON模式 axis=pitch|roll
//此项为闭环控制，有角度作为反馈，控制信号消失，四轴回中。
{
// 最大倾斜角为50度
errorAngle = constrain((rcCommand[axis]<<1) + GPS_angle[axis],-500,+500) - angle[axis] + conf.angleTrim[axis]; //控制5为1度
//误差角度（要到的角度与现角度之差+角度修正），角度修正 即微调ACC
自稳ACC比例部分值。角度误差*P level除以128
//计算需要32位 errorAngle*P8[PIDLEVEL] 会超过 32768 结果只需要16位数据。
PTermACC = constrain(PTermACC,-conf.D8[PIDLEVEL]*5,+conf.D8[PIDLEVEL]*5); //限定PtermAcc的范围
errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle,-10000,+10000); // 累加ACC角度积分项误差
自稳ACC积分部分值。/4096
}
if ( !f.ANGLE_MODE || f.HORIZON_MODE || axis == 2 ) //若普通模式或开启了HORIZON或者axis=yaw
//此项为开环控制，即有控制量则一直进行动作，并且由于积分项的存在，动作速度会有加快。当控制信号消失，四轴保持该状态。
{
if (abs(rcCommand[axis])<500) error = (rcCommand[axis]<<6)/conf.P8[axis] ; //误差=rcCommand[axis]*64/P值 ？
else error = ((int32_t)rcCommand[axis]<<6)/conf.P8[axis] ; // 32 bits is needed for calculation
error -= gyroData[axis];
GYRO 比例部分值。感觉应该等于与ACC类似？
errorGyroI[axis] = constrain(errorGyroI[axis]+error,-16000,+16000); //累加GYRO角度积分项误差
if (abs(gyroData[axis])>640) errorGyroI[axis] = 0;
GYRO积分部分值。errorGyroI[axis]除以128乘以I8除以64 总共右移13位 大于ACC的12位 I8<250;
}
if ( f.HORIZON_MODE && axis<2)
{
PTerm = ((int32_t)PTermACC*(512-prop) + (int32_t)PTermGYRO*prop)>>9; // the real factor should be 500, but 512 is ok
ITerm = ((int32_t)ITermACC*(512-prop) + (int32_t)ITermGYRO*prop)>>9; //综合 Pacc与Pgyro并用prop进行修正 控制量大调小自稳系数，控制量小，以自稳为主。 除以512
}
else
{
if ( f.ANGLE_MODE && axis<2) //开启自稳，则使用 PTermACC ITermACC
{
PTerm = PTermACC;
ITerm = ITermACC;
}
else //未开自稳，则使用PTermGYRO ITermGYRO
{
PTerm = PTermGYRO;
ITerm = ITermGYRO;
}
}
PTerm -= ((int32_t)gyroData[axis]*dynP8[axis])>>6;
// dynP8=P8*prop1/100； 与当时油门有关 与 rate大 则dyn小 用角加速度值对角度P值进行修正。 /64
delta = gyroData[axis] - lastGyro[axis]; // 2次连续的gyro 最大不会超过800 角速度变化（角加速度）
lastGyro[axis] = gyroData[axis]; //保存当前角速度值
deltaSum = delta1[axis]+delta2[axis]+delta; //对角速度差（角加速度）值进行累加
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = ((int32_t)deltaSum*dynD8[axis])>>5; // 与当时油门有关 与 rate大 则dyn小 用角加速度值对角度P值进行修正。 /32
axisPID[axis] = PTerm + ITerm - DTerm;
}
mixTable(); //设置各个电机的输出
writeServos(); //舵机输出
writeMotors(); //电机输出
}MWC v2.2 代码解读ACC_Common()
ACC校准 以及改变传感器的朝向问题以解决某些时候无法正常安装传感器，得到最终结果是ACCADC【3】
void ACC_Common()
{
static int32_t a[3];
if (calibratingA>0) //校准ACC
{
for (uint8_t axis = 0; axis < 3; axis++)
{
if (calibratingA == 512) a[axis]=0; // 在校准的最开始，清零a[axis]
a[axis] +=accADC[axis]; // 对512次读取求和
accADC[axis]=0;
global_conf.accZero[axis]=0; // 清零全局变量
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (calibratingA == 1)
{
global_conf.accZero[ROLL] = a[ROLL]>>9; //除以512
global_conf.accZero[PITCH] = a[PITCH]>>9; //除以512
global_conf.accZero[YAW] = (a[YAW]>>9)-acc_1G; // ADXL345 acc_1G=265
conf.angleTrim[ROLL] = 0;
conf.angleTrim[PITCH] = 0;
writeGlobalSet(1); //将零点信息写入 EEPROM
}
calibratingA--; //每个主函数减1,512次主循环校准结束，花费大概512*3ms=1.5S
}
#if defined(INFLIGHT_ACC_CALIBRATION)
static int32_t b[3];
static int16_t accZero_saved[3] = {0,0,0};
static int16_t angleTrim_saved[2] = {0, 0};
//在校准之前，存储原来的零点
if (InflightcalibratingA==50) //空中校准ACC 50个循环之内 可改为64便于之后除法使用移位的方法
{
accZero_saved[ROLL] = global_conf.accZero[ROLL] ;
accZero_saved[PITCH] = global_conf.accZero[PITCH];
accZero_saved[YAW] = global_conf.accZero[YAW] ;
angleTrim_saved[ROLL] = conf.angleTrim[ROLL] ;
angleTrim_saved[PITCH] = conf.angleTrim[PITCH] ;
}
if (InflightcalibratingA>0)
{
for (uint8_t axis = 0; axis < 3; axis++)
{
if (InflightcalibratingA == 50) b[axis]=0;
b[axis] +=accADC[axis]; //求和50次测量值
accADC[axis]=0;
global_conf.accZero[axis]=0; // 清零全局变量
}
if (InflightcalibratingA == 1) //校准结束
{
AccInflightCalibrationActive = 0;
AccInflightCalibrationMeasurementDone = 1;
#if defined(BUZZER)
alarmArray[7] = 1; //buzzer for indicatiing the end of calibration
#endif
// recover saved values to maintain current flight behavior until new values are transferred
global_conf.accZero[ROLL] = accZero_saved[ROLL] ;
global_conf.accZero[PITCH] = accZero_saved[PITCH];
global_conf.accZero[YAW] = accZero_saved[YAW] ;
conf.angleTrim[ROLL] = angleTrim_saved[ROLL] ;
conf.angleTrim[PITCH] = angleTrim_saved[PITCH] ;
}
InflightcalibratingA--;
}
// 校准结束后，计算平均值, 更改 Z 通过1G 存储值在 EEPROM
if (AccInflightCalibrationSavetoEEProm == 1)//飞行器降落, 锁定之后有效
{
AccInflightCalibrationSavetoEEProm = 0;
global_conf.accZero[ROLL] = b[ROLL]/50;
global_conf.accZero[PITCH] = b[PITCH]/50;
global_conf.accZero[YAW] = b[YAW]/50-acc_1G; // for nunchuk 200=1G
conf.angleTrim[ROLL] = 0;
conf.angleTrim[PITCH] = 0;
writeGlobalSet(1);
}
#endif
accADC[ROLL] -= global_conf.accZero[ROLL] ;
accADC[PITCH] -= global_conf.accZero[PITCH];
accADC[YAW] -= global_conf.accZero[YAW] ;
#if defined(SENSORS_TILT_45DEG_LEFT) //旋转45度 有精度损失。
int16_t temp = ((accADC[PITCH] - accADC[ROLL] )*7)/10;
accADC[ROLL] = ((accADC[ROLL] + accADC[PITCH])*7)/10;
accADC[PITCH] = temp;
#endif
#if defined(SENSORS_TILT_45DEG_RIGHT)
int16_t temp = ((accADC[PITCH] + accADC[ROLL] )*7)/10;
accADC[ROLL] = ((accADC[ROLL] - accADC[PITCH])*7)/10;
accADC[PITCH] = temp;
#endif
}
MWC v2.2 代码解读go_arm() go_disarm()
解锁，上锁函数。解锁时进行无头模式头标定和气压计基准标定。
void go_arm()
{
if(calibratingG == 0 && f.ACC_CALIBRATED
#if defined(FAILSAFE)
&& failsafeCnt < 2
#endif
)
{
if(!f.ARMED) //若未解锁
{
f.ARMED = 1; //解锁标志位置1
headFreeModeHold = heading; //定义无头模式的头 即刺客四轴头的朝向
#if defined(VBAT)
if (vbat > conf.no_vbat) vbatMin = vbat; //跟电池电压有关，无关紧要。
#endif
#ifdef LCD_TELEMETRY // 解锁时重置一些参数
#if BARO
BAROaltMax = BaroAlt; //气压最大值等于解锁时的气压值
#endif
#endif
#ifdef LOG_PERMANENT
plog.arm++; // 解锁事件
plog.running = 1; // 飞行器运行事件
// write now.
writePLog(); //写入eerom
#endif
}
}
else if(!f.ARMED)
{
blinkLED(2,255,1); //灯闪烁
alarmArray[8] = 1;
}
}
void go_disarm()
{
if (f.ARMED)
{
f.ARMED = 0; //解锁标志位清0
#ifdef LOG_PERMANENT
plog.disarm++; // 锁定事件
plog.armed_time = armedTime ; //解锁时间保存入日志
if (failsafeEvents) plog.failsafe++; // 掉落保护事件进日志
if (i2c_errors_count > 10) plog.i2c++; // i2c错误事件进日志
plog.running = 0; //飞行停止事件
// write now.
writePLog(); //日志写入eerom
#endif
}
}
MWC v2.2 代码解读computeRC()
对遥控接收的信号进行循环滤波，取4组数据，即80MS，算出平均值，把大于平均值的减小2，小于平均值的增大2.
void computeRC()
{
static uint16_t rcData4Values[RC_CHANS][4], rcDataMean[RC_CHANS]; // chan 通道
static uint8_t rc4ValuesIndex = 0;
uint8_t chan,a;
#if !(defined(RCSERIAL) || defined(OPENLRSv2MULTI)) // dont know if this is right here
#if defined(SBUS)
readSBus();
#endif
rc4ValuesIndex++;
if (rc4ValuesIndex == 4) rc4ValuesIndex = 0; // 到达4 就重置为0
for (chan = 0; chan < RC_CHANS; chan++) //rc_CHAN 遥控通道数
{
#if defined(FAILSAFE)
uint16_t rcval = readRawRC(chan); //读取 rcValue[rcChannel[chan]]
if(rcval>FAILSAFE_DETECT_TRESHOLD || chan > 3)
// 当脉冲大于AILSAFE_DETECT_TRESHOLD时上传通道值 剔除太短的坏值。
{
rcData4Values[chan][rc4ValuesIndex] = rcval; //4组通道值
}
#else
rcData4Values[chan][rc4ValuesIndex] = readRawRC(chan); //直接赋值4组通道值
#endif
rcDataMean[chan] = 0;
for (a=0;a<4;a++) rcDataMean[chan] += rcData4Values[chan][a];
rcDataMean[chan]= (rcDataMean[chan]+2)>>2; //求4通道平均值
if ( rcDataMean[chan] < (uint16_t)rcData[chan] -3) rcData[chan] = rcDataMean[chan]+2; //对各个通道进行循环滤波
if ( rcDataMean[chan] > (uint16_t)rcData[chan] +3) rcData[chan] = rcDataMean[chan]-2;
}
#endif
}
MWC V2.2 代码解读setup()
函数主要功能是开启一些传输外设，设置一些引脚状态，读取之前四轴一些常规参数。很出乎意料没有对于传感器的校准。本人接触Arduino时间不长，难免有错误，欢迎指正。
void setup() {
#if !defined(GPS_PROMINI) 设置GPS串口通信
SerialOpen(0,SERIAL0_COM_SPEED);
#if defined(PROMICRO)
SerialOpen(1,SERIAL1_COM_SPEED);
#endif
#if defined(MEGA)
SerialOpen(1,SERIAL1_COM_SPEED);
SerialOpen(2,SERIAL2_COM_SPEED);
SerialOpen(3,SERIAL3_COM_SPEED);
#endif
#endif
LEDPIN_PINMODE; 设置LED引脚状态
POWERPIN_PINMODE; 设置POWER引脚状态
BUZZERPIN_PINMODE; 设置BUZZER引脚状态
STABLEPIN_PINMODE; 设置 STABLE引脚状态
POWERPIN_OFF; POWER引脚输出0
initOutput(); 使能所有PWM引脚
#ifdef MULTIPLE_CONFIGURATION_PROFILES 多种配置支持 存储在EEPROM中
for(global_conf.currentSet=0; global_conf.currentSet<3; global_conf.currentSet++) { // check all settings integrity
readEEPROM();
}
#else
global_conf.currentSet=0;
readEEPROM();
#endif
readGlobalSet(); //读取acc零点 mag零点 currentset
readEEPROM(); // load current setting data
blinkLED(2,40,global_conf.currentSet+1);
configureReceiver(); //设置RC接收引脚状态，如aux2
#if defined (PILOTLAMP) //信号灯有关
PL_INIT;
#endif
#if defined(OPENLRSv2MULTI) //sensors 板
initOpenLRS(); //初始化一些引脚
#endif
initSensors(); //初始化IIC接口，以及10DOF传感器芯片，并把标志位f.I2C_INIT_DONE置1
#if defined(I2C_GPS) || defined(GPS_SERIAL) || defined(GPS_FROM_OSD)
GPS_set_pids();
#endif
previousTime = micros(); //运行微秒数
#if defined(GIMBAL) //平衡
calibratingA = 512; //矫正时间=512=12s
#endif
calibratingG = 512; //矫正时间G=512*25M=12s
calibratingB = 200; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
#if defined(POWERMETER) //功率计
for(uint8_t i=0;i<=PMOTOR_SUM;i++)
pMeter[i]=0;
#endif
#if defined(GPS_SERIAL) //串行GPS
GPS_SerialInit();
for(uint8_t i=0;i<=5;i++){
GPS_NewData(); //更新GPS数据
LEDPIN_ON
delay(20);
LEDPIN_OFF
delay(80);
}
if(!GPS_Present){ //校验和正确标志位
SerialEnd(GPS_SERIAL); //开普通串口 关闭GPS串口
SerialOpen(0,SERIAL0_COM_SPEED);
}
#if !defined(GPS_PROMINI)
GPS_Present = 1;
#endif
GPS_Enable = GPS_Present;
#endif
#if defined(I2C_GPS) || defined(TINY_GPS) || defined(GPS_FROM_OSD)
GPS_Enable = 1; //启用GPS
#endif
#if defined(LCD_ETPP) || defined(LCD_LCD03) || defined(OLED_I2C_128x64) || defined(LCD_TELEMETRY_STEP)
initLCD(); //初始化LCD
#endif
#ifdef LCD_TELEMETRY_DEBUG
telemetry_auto = 1;
#endif
#ifdef LCD_CONF_DEBUG
configurationLoop();
#endif
#ifdef LANDING_LIGHTS_DDR
init_landing_lights();
#endif
ADCSRA |= _BV(ADPS2) ; ADCSRA &= ~_BV(ADPS1); ADCSRA &= ~_BV(ADPS0); // 这要的模拟数据读取速度使得分辨率损失小
#if defined(LED_FLASHER)
init_led_flasher();
led_flasher_set_sequence(LED_FLASHER_SEQUENCE);
#endif
f.SMALL_ANGLES_25=1; // important for gyro only conf 倾斜角小于25度
#ifdef LOG_PERMANENT
// read last stored set
readPLog(); //读取一些设置 是否锁定，解锁时间，IIC错误数等
plog.lifetime += plog.armed_time / 1000000;
plog.start++; // #powercycle/reset/initialize events
// dump plog data to terminal
#ifdef LOG_PERMANENT_SHOW_AT_STARTUP
dumpPLog(0);
#endif
plog.armed_time = 0; // lifetime in seconds
//plog.running = 0; // toggle on arm & disarm to monitor for clean shutdown vs. powercut
#endif
debugmsg_append_str("initialization completed\n");
}
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
