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模式時的動作
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
PTermACC = ((int32_t)errorAngle*conf.P8[PIDLEVEL])>>7; //
自穩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角度積分項誤差
ITermACC = ((int32_t)errorAngleI[axis]*conf.I8[PIDLEVEL])>>12; //
自穩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];
PTermGYRO = rcCommand[axis]; //
GYRO
比例部分值。感覺應該等於與ACC類似？
errorGyroI[axis] = constrain(errorGyroI[axis]+error,-16000,+16000); //累加GYRO角度積分項誤差
if (abs(gyroData[axis])>640) errorGyroI[axis] = 0;
ITermGYRO = ((errorGyroI[axis]>>7)*conf.I8[axis])>>6; //
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");
}