飛控MWC-v2.3 運(yùn)行標(biāo)準(zhǔn)流程

1、void setup() #if !defined(GPS_PROMINI) /設(shè)立串口 SerialOpen(0,SERIAL0_COM_SPEED); #if defined(PROMICRO) /設(shè)立串口 SerialOpen(1,SERIAL1_COM_SPEED); #endif #if defined(MEGA) /設(shè)立串口 SerialOpen(1,SERIAL1_COM_SPEED); SerialOpen(2,SERIAL2_COM_SPEED); SerialOpen(3,SERIAL3_COM_SPEED); #endif #endif LEDPIN_PINMODE; P

2、OWERPIN_PINMODE; BUZZERPIN_PINMODE; STABLEPIN_PINMODE; /設(shè)立部分IO口 POWERPIN_OFF; initOutput(); /輸出初始化 /*/* 初始化PWM定期器和寄存器 */*/void initOutput() /* 設(shè)立所有旳PWM口為輸出 */ for(uint8_t i=0;i 0) / init 16bit timer 3 TCCR3A |= (1WGM31); / phase correct mode TCCR3A &= (1WGM30); TCCR3B |= (1WGM33); TCCR3B &= (1 1) TC

3、CR3A |= _BV(COM3A1); / connect pin 5 to timer 3 channel A #endif #if (NUMBER_MOTOR 2) / init 16bit timer 4 TCCR4A |= (1WGM41); / phase correct mode TCCR4A &= (1WGM40); TCCR4B |= (1WGM43); TCCR4B &= (1 3) TCCR3A |= _BV(COM3B1); / connect pin 2 to timer 3 channel B #endif #if (NUMBER_MOTOR 4) TCCR4A |

4、= _BV(COM4B1); / connect pin 7 to timer 4 channel B TCCR4A |= _BV(COM4C1); / connect pin 8 to timer 4 channel C #endif #if (NUMBER_MOTOR 6) / timer 2 is a 8bit timer so we cant change its range TCCR2A |= _BV(COM2B1); / connect pin 9 to timer 2 channel B TCCR2A |= _BV(COM2A1); / connect pin 10 to tim

5、er 2 channel A #endif #endif /*特殊旳 PWM 定期器 & 暫存器為了atmega32u4 (Promicro) */ #if defined(PROMICRO) #if (NUMBER_MOTOR 0) & ( !defined(A32U4_4_HW_PWM_SERVOS) ) TCCR1A |= (1WGM11); / phase correct mode & no prescaler TCCR1A &= (1WGM10); TCCR1B &= (1WGM12) & (1CS11) & (1CS12); TCCR1B |= (1WGM13) | (1 1) T

6、CCR1A |= _BV(COM1B1); / connect pin 10 to timer 1 channel B #endif #if (NUMBER_MOTOR 2) #if !defined(HWPWM6) / timer 4A TCCR4E |= (1ENHC4); / enhanced pwm mode TCCR4B &= (1CS41); TCCR4B |= (1CS42)|(1CS40); / prescaler to 16 TCCR4D |= (1WGM40); TC4H = 0 x3; OCR4C = 0 xFF; / phase and frequency correc

7、t mode & top to 1023 but with enhanced pwm mode we have 2047 TCCR4A |= (1COM4A0)|(1PWM4A); / connect pin 5 to timer 4 channel A #else / timer 3A TCCR3A |= (1WGM31); / phase correct mode & no prescaler TCCR3A &= (1WGM30); TCCR3B &= (1WGM32) & (1CS31) & (1CS32); TCCR3B |= (1WGM33) | (1 3) | ( (NUMBER_

8、MOTOR 0) & defined(A32U4_4_HW_PWM_SERVOS) ) #if defined(HWPWM6) TCCR4E |= (1ENHC4); / enhanced pwm mode TCCR4B &= (1CS41); TCCR4B |= (1CS42)|(1CS40); / prescaler to 16 TCCR4D |= (1WGM40); TC4H = 0 x3; OCR4C = 0 xFF; / phase and frequency correct mode & top to 1023 but with enhanced pwm mode we have

9、2047 #endif TCCR4C |= (1COM4D1)|(1 4) #if defined(HWPWM6) TCCR1A |= _BV(COM1C1); / connect pin 11 to timer 1 channel C TCCR4A |= (1COM4A1)|(1 6) #if defined(HWPWM6) initializeSoftPWM(); #endif #endif #endif /* 特殊旳 PWM 定期器 & 暫存器 for the atmega328P (Promini) */ #if defined(PROMINI) #if (NUMBER_MOTOR 0

10、) TCCR1A |= _BV(COM1A1); / 連接 針 9 到 定期器 1 channel A #endif #if (NUMBER_MOTOR 1) TCCR1A |= _BV(COM1B1); / connect pin 10 to timer 1 channel B #endif #if (NUMBER_MOTOR 2) TCCR2A |= _BV(COM2A1); / connect pin 11 to timer 2 channel A #endif #if (NUMBER_MOTOR 3) TCCR2A |= _BV(COM2B1); / connect pin 3 to

11、timer 2 channel B #endif #if (NUMBER_MOTOR 4) / PIN 5 & 6 or A0 & A1 initializeSoftPWM(); /初始化定期器和PWM通道 #if defined(A0_A1_PIN_HEX) | (NUMBER_MOTOR 6) pinMode(5,INPUT);pinMode(6,INPUT); / we reactivate the INPUT affectation for these two PINs pinMode(A0,OUTPUT);pinMode(A1,OUTPUT); #endif #endif #endi

12、f /* special version of MultiWii to calibrate all attached ESCs */* 設(shè)立特殊電子調(diào)速器*/ #if defined(ESC_CALIB_CANNOT_FLY) writeAllMotors(ESC_CALIB_HIGH); blinkLED(2,20, 2); delay(4000); writeAllMotors(ESC_CALIB_LOW); blinkLED(3,20, 2); while (1) delay(5000); blinkLED(4,20, 2); #if defined(BUZZER) alarmArray

13、7 = 2; #endif exit; / statement never reached #endif writeAllMotors(MINCOMMAND); delay(300); #if defined(SERVO) initializeServo(); #endif readGlobalSet(); /讀取存儲(chǔ)旳全局變量 #ifndef NO_FLASH_CHECK #if defined(MEGA) uint16_t i = 65000; / only first 64K for mega board due to pgm_read_byte limitation #else uin

14、t16_t i = 3; #endif uint16_t flashsum = 0; uint8_t pbyt; while(i-) pbyt = pgm_read_byte(i); / calculate flash checksum flashsum += pbyt; flashsum = (pbyt8); #endif #ifdef MULTIPLE_CONFIGURATION_PROFILES global_conf.currentSet=2; #else global_conf.currentSet=0; #endif while(1) / check settings integr

15、ity #ifndef NO_FLASH_CHECK if(readEEPROM() / check current setting integrity if(flashsum != global_conf.flashsum) update_constants(); / update constants if firmware is changed and integrity is OK #else readEEPROM(); / check current setting integrity #endif if(global_conf.currentSet = 0) break; / all

16、 checks is done global_conf.currentSet-; / next setting for check readGlobalSet(); /重新加載全局設(shè)立得到最后檔案號(hào)碼 #ifndef NO_FLASH_CHECK if(flashsum != global_conf.flashsum) global_conf.flashsum = flashsum; / new flash sum writeGlobalSet(1); / update flash sum in global config #endif readEEPROM(); / load setting

17、 data from last used profile加載設(shè)立數(shù)據(jù)從去年使用概要文獻(xiàn) blinkLED(2,40,global_conf.currentSet+1); configureReceiver(); #if defined (PILOTLAMP) PL_INIT; #endif #if defined(OPENLRSv2MULTI) / OpenLRS v2 Multi Rc Receiver board including ITG3205 and ADXL345 initOpenLRS(); #endif initSensors(); /傳感器初始化 #if defined(I2

18、C_GPS) | defined(GPS_SERIAL) | defined(GPS_FROM_OSD) GPS_set_pids(); / /獲取有關(guān)P I D值和設(shè)立旳PID控制器 #endif previousTime = micros(); #if defined(GIMBAL) /如果 自穩(wěn)云臺(tái)（多旋翼飛行器種類） calibratingA = 512; #endif calibratingG = 512; calibratingB = 200; / 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pres

19、sure settles #if defined(POWERMETER) for(uint8_t j=0; j=PMOTOR_SUM; j+) pMeterj=0; #endif /*/ #if defined(GPS_SERIAL) / flyduino v2應(yīng)設(shè)為2。此為arduino MEGA上旳串標(biāo)語(yǔ) GPS_SerialInit(); /GPS串口初始化 for(uint8_t j=0;j rcTime ) / 50Hz rcTime = currentTime + 0; computeRC(); / Failsafe routine - added by MIS #if defin

20、ed(FAILSAFE) if ( failsafeCnt (5*FAILSAFE_DELAY) & f.ARMED) / Stabilize, and set Throttle to specified level for(i=0; i 5*(FAILSAFE_DELAY+FAILSAFE_OFF_DELAY) / Turn OFF motors after specified Time (in 0.1sec) go_disarm(); / This will prevent the copter to automatically rearm if failsafe shuts it dow

21、n and prevents f.OK_TO_ARM = 0; / to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm failsafeEvents+; 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

22、to arm go_disarm(); / This will prevent the copter to automatically rearm if failsafe shuts it down and prevents f.OK_TO_ARM = 0; / to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm failsafeCnt+; #endif / end of failsafe routine - next change is made wit

23、h RcOptions setting / - STICKS COMMAND HANDLER - / checking sticks positions uint8_t stTmp = 0; for(i=0;i= 2; if(rcDatai MINCHECK) stTmp |= 0 x80; / check for MIN if(rcDatai MAXCHECK) stTmp |= 0 x40; / check for MAX if(stTmp = rcSticks) if(rcDelayCommand250) rcDelayCommand+; else rcDelayCommand = 0;

24、 rcSticks = stTmp; / perform actions if (rcDataTHROTTLE 0) / Arming/Disarming via ARM BOX if ( rcOptionsBOXARM & f.OK_TO_ARM ) go_arm(); else if (f.ARMED) go_disarm(); if(rcDelayCommand = 20) if(f.ARMED) / actions during armed #ifdef ALLOW_ARM_DISARM_VIA_TX_YAW if (conf.activateBOXARM = 0 & rcSticks

25、 = THR_LO + YAW_LO + PIT_CE + ROL_CE) go_disarm(); / Disarm via YAW #endif #ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL if (conf.activateBOXARM = 0 & rcSticks = THR_LO + YAW_CE + PIT_CE + ROL_LO) go_disarm(); / Disarm via ROLL #endif else / actions during not armed i=0; if (rcSticks = THR_LO + YAW_LO + PIT_L

26、O + ROL_CE) / GYRO calibration calibratingG=512; #if GPS GPS_reset_home_position(); #endif #if BARO calibratingB=10; / calibrate baro to new ground level (10 * 25 ms = 250 ms non blocking) #endif #if defined(INFLIGHT_ACC_CALIBRATION) else if (rcSticks = THR_LO + YAW_LO + PIT_HI + ROL_HI) / Inflight

27、ACC calibration START/STOP if (AccInflightCalibrationMeasurementDone) / trigger saving into eeprom after landing AccInflightCalibrationMeasurementDone = 0; AccInflightCalibrationSavetoEEProm = 1; else AccInflightCalibrationArmed = !AccInflightCalibrationArmed; #if defined(BUZZER) if (AccInflightCali

28、brationArmed) alarmArray0=2; else alarmArray0=3; #endif #endif #ifdef MULTIPLE_CONFIGURATION_PROFILES if (rcSticks = THR_LO + YAW_LO + PIT_CE + ROL_LO) i=1; / ROLL left - Profile 1 else if (rcSticks = THR_LO + YAW_LO + PIT_HI + ROL_CE) i=2; / PITCH up - Profile 2 else if (rcSticks = THR_LO + YAW_LO

29、+ PIT_CE + ROL_HI) i=3; / ROLL right - Profile 3 if(i) global_conf.currentSet = i-1; writeGlobalSet(0); readEEPROM(); blinkLED(2,40,i); alarmArray0 = i; #endif if (rcSticks = THR_LO + YAW_HI + PIT_HI + ROL_CE) / Enter LCD config #if defined(LCD_CONF) configurationLoop(); / beginning LCD configuratio

30、n #endif previousTime = micros(); #ifdef ALLOW_ARM_DISARM_VIA_TX_YAW else if (conf.activateBOXARM = 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.activateBOXARM = 0 & rcSticks = THR_LO + YAW_CE + PIT_CE + ROL_HI) go

31、_arm(); / Arm via ROLL #endif #ifdef LCD_TELEMETRY_AUTO 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_H

32、I) / Telemetry next step telemetry = telemetryStepSequence+telemetryStepIndex % strlen(telemetryStepSequence); #if defined( OLED_I2C_128x64) if (telemetry != 0) i2c_OLED_init(); #elif defined(OLED_DIGOLE) if (telemetry != 0) i2c_OLED_DIGOLE_init(); #endif LCDclear(); #endif #if ACC else if (rcSticks

33、 = THR_HI + YAW_LO + PIT_LO + ROL_CE) calibratingA=512; / throttle=max, yaw=left, pitch=min #endif #if MAG else if (rcSticks = THR_HI + YAW_HI + PIT_LO + ROL_CE) f.CALIBRATE_MAG = 1; / throttle=max, yaw=right, pitch=min #endif i=0; if (rcSticks = THR_HI + YAW_CE + PIT_HI + ROL_CE) conf.angleTrimPITC

34、H+=2; i=1; else if (rcSticks = THR_HI + YAW_CE + PIT_LO + ROL_CE) conf.angleTrimPITCH-=2; i=1; else if (rcSticks = THR_HI + YAW_CE + PIT_CE + ROL_HI) conf.angleTrimROLL +=2; i=1; else if (rcSticks = THR_HI + YAW_CE + PIT_CE + ROL_LO) conf.angleTrimROLL -=2; i=1; if (i) writeParams(1); rcDelayCommand

35、 = 0; / allow autorepetition #if defined(LED_RING) blinkLedRing(); #endif #if defined(LED_FLASHER) led_flasher_autoselect_sequence(); #endif #if defined(INFLIGHT_ACC_CALIBRATION) if (AccInflightCalibrationArmed & f.ARMED & rcDataTHROTTLE MINCHECK & !rcOptionsBOXARM ) / Copter is airborne and you are

36、 turning it off via boxarm : start measurement InflightcalibratingA = 50; AccInflightCalibrationArmed = 0; if (rcOptionsBOXCALIB) / Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored if (!AccInflightCalibrationActive & !AccInflightCalibrationMe

37、asurementDone) InflightcalibratingA = 50; else if(AccInflightCalibrationMeasurementDone & !f.ARMED) AccInflightCalibrationMeasurementDone = 0; AccInflightCalibrationSavetoEEProm = 1; #endif uint16_t auxState = 0; for(i=0;i4;i+) auxState |= (rcDataAUX1+i1300)(3*i) | (1300rcDataAUX1+i & rcDataAUX1+i17

38、00)1700)(3*i+2); for(i=0;i0; / note: if FAILSAFE is disable, failsafeCnt 5*FAILSAFE_DELAY is always false #if ACC if ( rcOptionsBOXANGLE | (failsafeCnt 5*FAILSAFE_DELAY) ) / bumpless transfer to Level mode if (!f.ANGLE_MODE) errorAngleIROLL = 0; errorAngleIPITCH = 0; f.ANGLE_MODE = 1; else / failsaf

39、e support f.ANGLE_MODE = 0; if ( rcOptionsBOXHORIZON ) f.ANGLE_MODE = 0; if (!f.HORIZON_MODE) errorAngleIROLL = 0; errorAngleIPITCH = 0; f.HORIZON_MODE = 1; else f.HORIZON_MODE = 0; #endif if (rcOptionsBOXARM = 0) f.OK_TO_ARM = 1; #if !defined(GPS_LED_INDICATOR) if (f.ANGLE_MODE | f.HORIZON_MODE) ST

40、ABLEPIN_ON; else STABLEPIN_OFF; #endif #if BARO #if (!defined(SUPPRESS_BARO_ALTHOLD) if (rcOptionsBOXBARO) if (!f.BARO_MODE) f.BARO_MODE = 1; AltHold = alt.EstAlt; #if defined(ALT_HOLD_THROTTLE_MIDPOINT) initialThrottleHold = ALT_HOLD_THROTTLE_MIDPOINT; #else initialThrottleHold = rcCommandTHROTTLE;

41、 #endif errorAltitudeI = 0; BaroPID=0; else f.BARO_MODE = 0; #endif #ifdef VARIOMETER if (rcOptionsBOXVARIO) if (!f.VARIO_MODE) f.VARIO_MODE = 1; else f.VARIO_MODE = 0; #endif #endif #if MAG if (rcOptionsBOXMAG) if (!f.MAG_MODE) f.MAG_MODE = 1; magHold = att.heading; else f.MAG_MODE = 0; if (rcOptio

42、nsBOXHEADFREE) if (!f.HEADFREE_MODE) f.HEADFREE_MODE = 1; #if defined(ADVANCED_HEADFREE)if (f.GPS_FIX & GPS_numSat = 5) & (GPS_distanceToHome ADV_HEADFREE_RANGE) ) if (GPS_directionToHome = 5 ) if (rcOptionsBOXGPSHOME) / if both GPS_HOME & GPS_HOLD are checked = GPS_HOME is the priority if (!f.GPS_H

43、OME_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 / SERIAL GPS_set_next_wp(&GPS_homeLAT,&GPS_homeLON); nav_mode = NAV_MODE_WP; #endif else f.GPS_HOME_MODE = 0; if (rcOptionsBOXGPSHOLD & abs(rcC

44、ommandROLL) AP_MODE & abs(rcCommandPITCH) 4) taskOrder-=5; switch (taskOrder) case 0: taskOrder+; #if MAG if (Mag_getADC() break; / max 350 s (HMC5883) / only break when we actually did something #endif case 1: taskOrder+; #if BARO if (Baro_update() != 0 ) break; #endif case 2: taskOrder+; #if BARO

45、if (getEstimatedAltitude() !=0) break; #endif case 3: taskOrder+; #if GPS if(GPS_Enable) GPS_NewData(); break; #endif case 4: taskOrder+; #if SONAR Sonar_update(); /debug2 = sonarAlt; #endif #ifdef LANDING_LIGHTS_DDR auto_switch_landing_lights(); #endif #ifdef VARIOMETER if (f.VARIO_MODE) vario_sign

46、aling(); #endif break; computeIMU(); / Measure loop rate just afer reading the sensors currentTime = micros(); cycleTime = currentTime - previousTime; previousTime = currentTime; /* /* Experimental FlightModes * /* #if defined(ACROTRAINER_MODE) if(f.ANGLE_MODE) if (abs(rcCommandROLL) + abs(rcCommand

47、PITCH) = ACROTRAINER_MODE ) f.ANGLE_MODE=0; 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(rcCommandYAW) 70 & f.MAG_MODE) int16_t dif = att.heading - magHold; if (dif = + 180) dif -= 360; if ( f.SMALL_ANGLES_25 ) rcCommandYAW -= dif*con

48、f.pidPIDMAG.P85; else magHold = att.heading; #endif #if BARO & (!defined(SUPPRESS_BARO_ALTHOLD) /* Smooth alt change routine , for slow auto and aerophoto modes (in general solution from alexmos). Its slowly increase/decrease * altitude proportional to stick movement (+/-100 throttle gives about +/-

49、50 cm in 1 second with cycle time about 3-4ms) */ if (f.BARO_MODE) static uint8_t isAltHoldChanged = 0; static int16_t AltHoldCorr = 0; if (abs(rcCommandTHROTTLE-initialThrottleHold)ALT_HOLD_THROTTLE_NEUTRAL_ZONE) / Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle give

50、s +50 cm in 1 second with cycle time about 3-4ms) AltHoldCorr+= rcCommandTHROTTLE - initialThrottleHold; if(abs(AltHoldCorr) 512) AltHold += AltHoldCorr/512; AltHoldCorr %= 512; isAltHoldChanged = 1; else if (isAltHoldChanged) AltHold = alt.EstAlt; isAltHoldChanged = 0; rcCommandTHROTTLE = initialTh

51、rottleHold + BaroPID; #endif #if defined(THROTTLE_ANGLE_CORRECTION) if(f.ANGLE_MODE | f.HORIZON_MODE) rcCommandTHROTTLE+= throttleAngleCorrection; #endif #if GPS if ( (f.GPS_HOME_MODE | f.GPS_HOLD_MODE) & f.GPS_FIX_HOME ) float sin_yaw_y = sin(att.heading*0.f); float cos_yaw_x = cos(att.heading*0.f)

52、; #if defined(NAV_SLEW_RATE) nav_ratedLON += constrain(wrap_18000(navLON-nav_ratedLON),-NAV_SLEW_RATE,NAV_SLEW_RATE); nav_ratedLAT += constrain(wrap_18000(navLAT-nav_ratedLAT),-NAV_SLEW_RATE,NAV_SLEW_RATE); GPS_angleROLL = (nav_ratedLON*cos_yaw_x - nav_ratedLAT*sin_yaw_y) /10; GPS_anglePITCH = (nav_

53、ratedLON*sin_yaw_y + nav_ratedLAT*cos_yaw_x) /10; #else GPS_angleROLL = (navLON*cos_yaw_x - navLAT*sin_yaw_y) /10; GPS_anglePITCH = (navLON*sin_yaw_y + navLAT*cos_yaw_x) /10; #endif else GPS_angleROLL = 0; GPS_anglePITCH = 0; #endif /* PITCH & ROLL & YAW PID *#if PID_CONTROLLER = 1 / evolved oldscho

54、ol if ( f.HORIZON_MODE ) prop = min(max(abs(rcCommandPITCH),abs(rcCommandROLL),512); / PITCH & ROLL for(axis=0;axis2;axis+) rc = rcCommandaxis640) errorGyroIaxis = 0; ITerm = (errorGyroIaxis7)*conf.pidaxis.I86; / 16 bits is ok here 16000/125 = 128 ; 128*250 = 3 PTerm = (int32_t)rc*conf.pidaxis.P86;

55、if (f.ANGLE_MODE | f.HORIZON_MODE) / axis relying on ACC / 50 degrees max inclination errorAngle = constrain(rc + GPS_angleaxis,-500,+500) - att.angleaxis + conf.angleTrimaxis; /16 bits is ok here errorAngleIaxis = constrain(errorAngleIaxis+errorAngle,-10000,+10000); / WindUp /16 bits is ok here PTe

56、rmACC = (int32_t)errorAngle*conf.pidPIDLEVEL.P8)7; / 32 bits is needed for calculation: errorAngle*P8 could exceed 32768 16 bits is ok for result int16_t limit = conf.pidPIDLEVEL.D8*5; PTermACC = constrain(PTermACC,-limit,+limit); ITermACC = (int32_t)errorAngleIaxis*conf.pidPIDLEVEL.I8)12; / 32 bits

57、 is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result ITerm = ITermACC + (ITerm-ITermACC)*prop9); PTerm = PTermACC + (PTerm-PTermACC)*prop9); PTerm -= (int32_t)imu.gyroDataaxis*dynP8axis)6; / 32 bits is needed for calculation delta = imu.gyroDataaxis - lastGyroaxis; / 16 bi

58、ts is ok here, the dif between 2 consecutive gyro reads is limited to 800 lastGyroaxis = imu.gyroDataaxis; DTerm = delta1axis+delta2axis+delta; delta2axis = delta1axis; delta1axis = delta; DTerm = (int32_t)DTerm*dynD8axis)5; / 32 bits is needed for calculation axisPIDaxis = PTerm + ITerm - DTerm; /Y

59、AW #define GYRO_P_MAX 300 #define GYRO_I_MAX 250 rc = (int32_t)rcCommandYAW * (2*conf.yawRate + 30) 5; error = rc - imu.gyroDataYAW; errorGyroI_YAW += (int32_t)error*conf.pidYAW.I8; errorGyroI_YAW = constrain(errorGyroI_YAW, 2-(int32_t)128), -2+(int32_t)1 50) errorGyroI_YAW = 0; PTerm = (int32_t)err

60、or*conf.pidYAW.P86; #ifndef COPTER_WITH_SERVO int16_t limit = GYRO_P_MAX-conf.pidYAW.D8; PTerm = constrain(PTerm,-limit,+limit); #endif ITerm = constrain(int16_t)(errorGyroI_YAW13),-GYRO_I_MAX,+GYRO_I_MAX); axisPIDYAW = PTerm + ITerm; #elif PID_CONTROLLER = 2 / alexK #define GYRO_I_MAX 256 #define A