void MultiWii_setup() { SerialOpen(SERIAL0_COM_SPEED); #if defined(TRACE) Serial.println("Start MultiWii_setup"); #endif initOutput(); LoadDefaults(); configureReceiver(); initSensors(); previousTime = micros(); calibratingA = 0; calibratingG = 512; #if defined(TRACE) Serial.println("End MultiWii_setup"); #endif }
/*! * @brief Initializes the whole system and runs the desired application * * This is the main function of the project. It calls initialization functions * of the MCU and the sensors. In the infinite loop it repeatedly checks * the USART module read buffer and Streams sensor data periodically (100 ms) via USART. * */ int main(void) { /********************* Initialize global variables **********************/ bmf055_input_state = USART_INPUT_STATE_PRINT_DATA; /************************* Initializations ******************************/ /*Initialize SAMD20 MCU*/ system_init(); /*Initialize clock module of SAMD20 MCU - Internal RC clock*/ //clock_initialize(); // done via conf_clocks.h --> ASF /*SPI master for communicating with sensors*/ spi_initialize(); /*eeprom emulator for configuration storage */ eeprom_emulator_initialize(); /*Initialize timers */ tc_initialize(); /*Initialize UART for communication with PC*/ usart_initialize(); /*Enable the system interrupts*/ system_interrupt_enable_global();/* All interrupts have a priority of level 0 which is the highest. */ /* Initialize the sensors */ bmf055_sensors_initialize(); readEEPROM(); checkFirstTime(0); //readEEPROM(); configureReceiver(); initSensors(); previousTime = micros(); calibratingG = 400; f.SMALL_ANGLES_25=1; // important for gyro only conf if(conf.copterType == 0){//0=Bi,1=Tri,2=QUADP,3=QUADX,4=Y4,5=Y6,6=H6P,7=H6X,8=Vtail4 MULTITYPE = 4; NUMBER_MOTOR = 2; } if(conf.copterType == 1){ MULTITYPE = 1; NUMBER_MOTOR = 3; } if(conf.copterType == 2){ MULTITYPE = 2; NUMBER_MOTOR = 4; } if(conf.copterType == 3){ MULTITYPE = 3; NUMBER_MOTOR = 4; } if(conf.copterType == 4){ MULTITYPE = 9; NUMBER_MOTOR = 4; } if(conf.copterType == 5){ MULTITYPE = 6; NUMBER_MOTOR = 6; } if(conf.copterType == 6){ MULTITYPE = 7; NUMBER_MOTOR = 6; } if(conf.copterType == 7){ MULTITYPE = 10; NUMBER_MOTOR = 6; } if(conf.copterType == 8){ MULTITYPE = 17; NUMBER_MOTOR = 4; } initOutput(); /************************** Infinite Loop *******************************/ while (true) { 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 beepon = 0; uint8_t axis,i; int16_t error,errorAngle; int16_t delta,deltaSum; int16_t PTerm=0,ITerm=0,PTermACC=0,ITermACC=0,PTermGYRO=0,ITermGYRO=0,DTerm=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 uint32_t BeepTime = 0; static uint8_t stickarmed = 0; //static int16_t initialThrottleHold; if(!rcOptions[BOXARM] && stickarmed == 0 && f.ARMED == 0){ if(rcData[YAW]<conf.MINCHECK && rcData[ROLL]>conf.MAXCHECK){ conf.calibState=1; writeParams(1); while(true){ //blinkLED(10,30,1); } } } while(SetupMode == 1){ checkSetup(); } if(conf.RxType == 1 || conf.RxType == 2){ if (rcFrameComplete) computeRC(); } if(!rcOptions[BOXARM] && stickarmed == 0) { f.ARMED = 0; } if (currentTime > rcTime ) { // 50Hz rcTime = currentTime + 20000; if(failsave < 250)failsave++; debug[0] = failsave; if(conf.RxType != 1 && conf.RxType != 2){ computeRC(); } if ((rcData[THROTTLE] < conf.MINCHECK && s3D == 0) || (rcData[THROTTLE] > (1500-conf.MIDDLEDEADBAND) && rcData[THROTTLE] < (1500+conf.MIDDLEDEADBAND) && s3D == 1 && f.ARMED == 0)) { errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; errorGyroI[YAW] = 0; errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; rcDelayCommand++; if (rcData[YAW] < conf.MINCHECK && rcData[PITCH] < conf.MINCHECK && !f.ARMED) { if (rcDelayCommand == 20 && failsave < 20) { calibratingG=400; } }else if (rcData[YAW] > conf.MAXCHECK && rcData[PITCH] > conf.MAXCHECK && !f.ARMED) { if (rcDelayCommand == 20) { previousTime = micros(); } }else if (conf.activate[BOXARM] > 0) { if ( rcOptions[BOXARM] && f.OK_TO_ARM && good_calib) { f.ARMED = 1; stickarmed = 0; } else if (f.ARMED) f.ARMED = 0; rcDelayCommand = 0; } else if ( (rcData[YAW] < conf.MINCHECK ) && f.ARMED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 0) { if (rcDelayCommand == 20) f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged } else if ( (rcData[YAW] > conf.MAXCHECK ) && rcData[PITCH] < conf.MAXCHECK && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 0) { if (rcDelayCommand == 20 && good_calib) { f.ARMED = 1; stickarmed = 1; } } else if ( (rcData[ROLL] < conf.MINCHECK ) && f.ARMED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 1) { if (rcDelayCommand == 20) f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged } else if ( (rcData[ROLL] > conf.MAXCHECK ) && rcData[PITCH] < conf.MAXCHECK && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 1) { if (rcDelayCommand == 20 && good_calib) { f.ARMED = 1; stickarmed = 1; } } else rcDelayCommand = 0; } else if (rcData[THROTTLE] > conf.MAXCHECK && !f.ARMED) { if (rcData[YAW] < conf.MINCHECK && rcData[PITCH] < conf.MINCHECK) { // throttle=max, yaw=left, pitch=min if (rcDelayCommand == 20) calibratingA=400; rcDelayCommand++; } else if (rcData[PITCH] > conf.MAXCHECK) { conf.angleTrim[PITCH]+=2;writeParams(1); } else if (rcData[PITCH] < conf.MINCHECK) { conf.angleTrim[PITCH]-=2;writeParams(1); } else if (rcData[ROLL] > conf.MAXCHECK) { conf.angleTrim[ROLL]+=2;writeParams(1); } else if (rcData[ROLL] < conf.MINCHECK) { conf.angleTrim[ROLL]-=2;writeParams(1); } else { rcDelayCommand = 0; } } uint16_t auxState = 0; 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; if(failsave > 200 && f.ARMED){ rcOptions[BOXACC] = 1; s3D = 0; rcData[THROTTLE] = 1190; rcCommand[THROTTLE] = 1190; } if (rcOptions[BOXACC] && s3D == 0) { // bumpless transfer to Level mode if (!f.ACC_MODE) { errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; f.ACC_MODE = 1; } } else { // failsafe support f.ACC_MODE = 0; } if (rcOptions[BOXBEEP]) { f.FSBEEP = 1; if(currentTime > BeepTime){ BeepTime = currentTime + 50000; if(beepon == 0){ if(conf.RxType == 0){ //digitalWrite(A2,HIGH); }else{ //digitalWrite(8,HIGH); } beepon = 1; }else{ if(conf.RxType == 0){ //digitalWrite(A2,LOW); }else{ //digitalWrite(8,LOW); } beepon = 0; } } } else { f.FSBEEP = 0; if(conf.RxType == 0){ //digitalWrite(A2,LOW); }else{ //digitalWrite(8,LOW); } } if (rcOptions[BOXHORIZON] && s3D == 0) { // bumpless transfer to Horizon mode if (!f.HORIZON_MODE) { errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; f.HORIZON_MODE = 1; } } else { f.HORIZON_MODE = 0; } if (rcOptions[BOX3D] && conf.F3D == 1) { if(f.ARMED == 0 && s3D == 0){ s3D = 1; f.ACC_MODE = 0; f.HORIZON_MODE = 0; } } else if(f.ARMED == 0){ s3D = 0; } if (rcOptions[BOXARM] == 0) f.OK_TO_ARM = 1; } computeIMU(); int16_t prop; if (f.HORIZON_MODE) prop = max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])); // range [0;500] if (f.ACC_MODE){ if(Zadd > 0)Zadd--; if(Zadd < 0)Zadd++; }else{ Zadd = 0; } //**** PITCH & ROLL & YAW PID **** for(axis=0;axis<3;axis++) { if ((f.ACC_MODE || f.HORIZON_MODE) && axis<2 ) { //LEVEL MODE // 50 degrees max inclination errorAngle = constrain(2*rcCommand[axis],-500,+500) - angle[axis] + conf.angleTrim[axis]; //16 bits is ok here #ifdef LEVEL_PDF PTermACC = -(int32_t)angle[axis]*conf.P8[PIDLEVEL]/100 ; #else PTermACC = (int32_t)errorAngle*conf.P8[PIDLEVEL]/100 ; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result #endif PTermACC = constrain(PTermACC,-conf.D8[PIDLEVEL]*5,+conf.D8[PIDLEVEL]*5); errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle,-10000,+10000); // WindUp //16 bits is ok here ITermACC = ((int32_t)errorAngleI[axis]*conf.I8[PIDLEVEL])>>12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result } if ( !f.ACC_MODE || f.HORIZON_MODE || axis == 2 ) { // MODE relying on GYRO or YAW axis if (abs(rcCommand[axis])<350) error = rcCommand[axis]*10*8/conf.P8[axis] ; // 16 bits is needed for calculation: 350*10*8 = 28000 16 bits is ok for result if P8>2 (P>0.2) else error = (int32_t)rcCommand[axis]*10*8/conf.P8[axis] ; // 32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2) error -= gyroData[axis]; PTermGYRO = rcCommand[axis]; errorGyroI[axis] = constrain(errorGyroI[axis]+error,-16000,+16000); // WindUp 16 bits is ok here if (abs(gyroData[axis])>640) errorGyroI[axis] = 0; ITermGYRO = (errorGyroI[axis]/125*conf.I8[axis])>>6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 } if ( f.HORIZON_MODE && axis<2) { PTerm = ((int32_t)PTermACC*(500-prop) + (int32_t)PTermGYRO*prop)/500; ITerm = ((int32_t)ITermACC*(500-prop) + (int32_t)ITermGYRO*prop)/500; } else { if ( f.ACC_MODE && axis<2) { PTerm = PTermACC; ITerm = ITermACC; } else { PTerm = PTermGYRO; ITerm = ITermGYRO; } } if (abs(gyroData[axis])<160) PTerm -= gyroData[axis]*dynP8[axis]/10/8; // 16 bits is needed for calculation 160*200 = 32000 16 bits is ok for result else PTerm -= (int32_t)gyroData[axis]*dynP8[axis]/10/8; // 32 bits is needed for calculation delta = gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 lastGyro[axis] = gyroData[axis]; deltaSum = delta1[axis]+delta2[axis]+delta; delta2[axis] = delta1[axis]; delta1[axis] = delta; if (abs(deltaSum)<640) DTerm = (deltaSum*dynD8[axis])>>5; // 16 bits is needed for calculation 640*50 = 32000 16 bits is ok for result else DTerm = ((int32_t)deltaSum*dynD8[axis])>>5; // 32 bits is needed for calculation axisPID[axis] = PTerm + ITerm - DTerm; }
void setup() { #if !defined(GPS_PROMINI) UARTInit(115200); #endif LEDPIN_PINMODE; SHIELDLED_PINMODE; //POWERPIN_PINMODE; //BUZZERPIN_PINMODE; //STABLEPIN_PINMODE; //POWERPIN_OFF; /* Initialize GPIO (sets up clock) */ GPIOInit(); init_microsec(); enable_microsec(); init_timer16PWM(); enable_PWMtimer(); /******** special version of MultiWii to calibrate all attached ESCs ************/ #if defined(ESC_CALIB_CANNOT_FLY) writeAllMotors(ESC_CALIB_HIGH); delayMs(3000); writeAllMotors(ESC_CALIB_LOW); delayMs(500); while (1) { delayMs(5000); blinkLED(2,20, 2); } // statement never reached #endif writeAllMotors(MINCOMMAND); delayMs(300); readEEPROM(); checkFirstTime(); configureReceiver(); #if defined(OPENLRSv2MULTI) initOpenLRS(); #endif initSensors(); #if defined(I2C_GPS) || defined(GPS_SERIAL) || defined(GPS_FROM_OSD)||defined(I2C_NAV) GPS_set_pids(); #endif previousTime = micros(); #if defined(GIMBAL) calibratingA = 400; #endif calibratingG = 400; 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(ARMEDTIMEWARNING) ArmedTimeWarningMicroSeconds = (ARMEDTIMEWARNING *1000000); #endif /************************************/ #if defined(GPS_SERIAL) SerialOpen(GPS_SERIAL,GPS_BAUD); delay(400); for(uint8_t i=0;i<=5;i++){ GPS_NewData(); LEDPIN_ON delay(20); LEDPIN_OFF delay(80); } if(!GPS_Present){ SerialEnd(GPS_SERIAL); SerialOpen(0,SERIAL_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)|| defined(I2C_NAV) GPS_Enable = 1; #endif #if defined(LCD_ETPP) || defined(LCD_LCD03) || defined(OLED_I2C_128x64) initLCD(); #endif #ifdef LCD_TELEMETRY_DEBUG telemetry_auto = 1; #endif #ifdef LCD_CONF_DEBUG configurationLoop(); #endif #ifdef LANDING_LIGHTS_DDR init_landing_lights(); #endif #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 //initialise median filter structures #ifdef MEDFILTER #ifdef SONAR initMedianFilter(&SonarFilter, 5); #endif #endif initWatchDog(); }