/*! Call this once with the node ID (0-31), frequency band (0-3), and optional group (0-255 for RF12B, only 212 allowed for RF12). */ void rf12_initialize () { spi_initialize(); pinMode(RFM_IRQ, INPUT); digitalWrite(RFM_IRQ, 1); // pull-up rf12_xfer(0x0000); // intitial SPI transfer added to avoid power-up problem rf12_xfer(RF_SLEEP_MODE); // DC (disable clk pin), enable lbd // wait until RFM12B is out of power-up reset, this takes several *seconds* rf12_xfer(RF_TXREG_WRITE); // in case we're still in OOK mode while (digitalRead(RFM_IRQ) == 0) rf12_xfer(0x0000); rf12_xfer(0x80C7 | (RF12_433MHZ << 4)); // EL (ena TX), EF (ena RX FIFO), 12.0pF rf12_xfer(0xA640); // 868MHz rf12_xfer(0xC606); // approx 49.2 Kbps, i.e. 10000/29/(1+6) Kbps rf12_xfer(0x94A2); // VDI,FAST,134kHz,0dBm,-91dBm rf12_xfer(0xC2AC); // AL,!ml,DIG,DQD4 rf12_xfer(0xCA8B); // FIFO8,1-SYNC,!ff,DR rf12_xfer(0xCE2D); // SYNC=2D; rf12_xfer(0xC483); // @PWR,NO RSTRIC,!st,!fi,OE,EN rf12_xfer(0x9850); // !mp,90kHz,MAX OUT rf12_xfer(0xCC77); // OB1,OB0, LPX,!ddy,DDIT,BW0 rf12_xfer(0xE000); // NOT USE rf12_xfer(0xC800); // NOT USE rf12_xfer(0xC049); // 1.66MHz,3.1V rxstate = TXIDLE; attachInterrupt(0, rf12_interrupt, LOW); }
/** * @brief Initialize the system and interrupts * @return Void */ void initialize_system(void) { timer_interrupt = false; // Clear the timer interrupt flag playing_sound = false; message_count = 0; valid_message = true; PORTA = 0x00; DDRA = 0xFF; PORTA = 0x00; // Set the data direction register values DDRD |= _BV(5) | _BV(6) | _BV(7); TCCR1A = 0x00; TCCR1B = 0x0D; OCR1A = 390; // 1s interval TIMSK1 |= (1<<OCIE1A); // Enable interrupt init_usart_pc(); log_msg("SABT initialising...\n\r"); log_msg("Setting flags...OK\n\r"); log_msg("PC USART...OK\n\r"); log_msg("Keypad USART..."); init_usart_keypad(); log_msg("OK\n\r"); log_msg("SPI..."); spi_initialize(); log_msg("OK\n\r"); log_msg("Interrupt flag..."); sei(); // sets the interrupt flag (enables interrupts) log_msg("OK\n\r"); init_sd_card(true); log_msg("SD card...OK\n\r"); play_mp3_file((unsigned char*)"SYS_WELC.mp3"); ui_check_modes(); log_msg("Parsing modes...OK\n\r"); log_msg("Type info\n\r"); log_msg("char: %d bytes\n\r", sizeof(char)); log_msg("int: %d bytes\n\r", sizeof(int)); log_msg("short: %d bytes\n\r", sizeof(short)); log_msg("long: %d bytes\n\r", sizeof(long)); log_msg("void*: %d bytes\n\r", sizeof(void*)); play_mp3("SYS_","MENU"); }
int do_sd_initialize (sd_context_t *sdc) { /* Initialize the SPI controller */ spi_initialize(); /* Set the maximum SPI clock rate possible */ spi_set_divisor(PERIPH_CLOCKRATE/400000); /* Initialization OK? */ if (sd_initialize(sdc) != 1) return 0; spi_set_divisor(2); //---- 2011 0905 spi full speed return 1; }
void setup_lcd_fs(void){ _disable_interrupts(); WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer BCSCTL1 = CALBC1_16MHZ; /* Set DCO for 16 MHz */ DCOCTL = CALDCO_16MHZ; delay_ms(100); // Initialize SD card: spi_initialize(); dres = disk_initialize(); tft_init_hw(); tft_begin(); tft_fillScreen(ILI9340_WHITE); tft_setCursor(0, 1); tft_setTextColor(ILI9340_BLACK); tft_setTextSize(1); if (dres){ sprintf(buffer, "Disk error = %d\n", (int) dres); tft_writeString(buffer); while (1); } pres = pf_mount(&fs); if (pres){ sprintf(buffer, "PFF error 1 = %d\n", (int) pres); tft_writeString(buffer); while (1); } tft_fillScreen(ILI9340_WHITE); P1DIR |= BIT0 | BIT1; P1OUT &= ~ (BIT0 | BIT1); }
struct spi_slave *spi_setup_slave(unsigned int bus, unsigned int cs, unsigned int max_hz, unsigned int mode) { #ifdef CONFIG_AML_SPICC if (bus == BUS_SPICC) { return spicc_setup_slave(bus, cs, max_hz, mode); } #endif struct aml_spi_slave * amls; amls = ( struct aml_spi_slave *)malloc(sizeof(struct aml_spi_slave)); if (!amls) return NULL; spi_initialize(); amls->slave.bus = bus; amls->slave.cs = cs; amls->adr_base =(void*)0xCC000000; amls->mode=mode; return &amls->slave; }
/*! * @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; }