/*******Test_Module_TYPE == Test_LED END*******/ ///*******Test_Module_TYPE == Test_IMU*******/ void Module_setup(void) { u32 total,free; Modules_Power_Control(ALL_Module,1); USART_INIT(115200,3,ENABLE); //serial port 2 initialize, baud rate 9600 ,enable receive interupt (bluebooth) USART_INIT(9600,2,ENABLE); //Initializes the IMU all modules IMU_INIT(IMU_ALL_IC); GPS_Config(); LED_Init(); if(SD_Init()!= SD_OK){ printf("SD Init failed!\r\n"); } else printf("SD Init OK!\r\n"); exfuns_init(); f_mount(fs[0],"0:",1); if(exf_getfree((u8 *)"0",&total,&free)) { printf("SD Card Fatfs Error!\r\n"); } else{ printf("SD Total Size: %d MB\r\n",total>>10); printf("SD Free Size: %d MB\r\n",free>>10); } }
int main(void) { uint8_t *val; USART_INIT(51); USART_SENDSTRING("PROGRAM STARTED"); initSPI(); receive_init(); DDRD |= (1<<LEDPin); //Set LEDPin as Output while(1) { USART_TRANSMIT(GetReg(CONFIG)); USART_TRANSMIT(GetReg(STATUS)); receive_data(); if(((GetReg(STATUS) & (1<<6)) != 0 )) { PORTD |= (1<<LEDPin); _delay_ms(100); PORTD &= ~(1<<LEDPin); val = WriteToNrf(R,R_RX_PAYLOAD,val,5); for(int i=0;i<5;i++) { USART_TRANSMIT(val[i]); } } reset(); } }
bool _SYS_DEBUG_INIT(int debug_port) { bool initRes = USART_INIT(debug_port, SYS_DEBUG_BAUDRATE); if(initRes) { debug_handle = _SYS_DEBUG_OPEN(debug_port); return debug_handle != 0; } return false; }
//TEST IMU void Module_setup_imu(void) { //Open IMU power Modules_Power_Control(IMU_Blue_Power,1); Modules_Power_Control(GPS_Power,1); //Initializes serial port 2 USART_INIT(9600,2,ENABLE); //Initializes the IMU all modules IMU_INIT(IMU_ALL_IC); GPS_Config(); }
bool _SYS_CONSOLE_INIT(int console_port) { bool initRes = USART_INIT(console_port, SYS_CONSOLE_BAUDRATE); if(initRes) { console_handle = _SYS_CONSOLE_OPEN(console_port); return console_handle != 0; } return false; }
/*******Test_Module_TYPE == Test_GPS END *******/ ///*********Test_Module_TYPE == Test_SD**************/ void Module_setup_sd(void) { u32 total,free; //open bule power for testing // Modules_Power_Control(Zigbee_Power,1); USART_INIT(115200,3,ENABLE); // //open SD power Modules_Power_Control(SD_Power,1); Modules_Power_Control(IMU_Blue_Power,1); Modules_Power_Control(GPS_Power,1); //Initializes serial port 2 USART_INIT(9600,2,ENABLE); //Initializes the IMU all modules IMU_INIT(IMU_ALL_IC); GPS_Config(); printf("Before SDCARD init\r\n"); // //³õʼ»¯SD¿¨ SD initialize if(SD_Init()!= SD_OK){ printf("SD Init failed!\r\n"); } else printf("SD Init OK!\r\n"); exfuns_init(); f_mount(fs[0],"0:",1); if(exf_getfree((u8 *)"0",&total,&free)) { printf("SD Card Fatfs Error!\r\n"); } else{ printf("SD Total Size: %d MB\r\n",total>>10); printf("SD Free Size: %d MB\r\n",free>>10); } }
/*void Module(void) { //Open Bluetooth power Modules_Power_Control(IMU_Blue_Power,1); //// Modules_Power_Control(Zigbee_Power,1); //// USART_INIT(9600,3,ENABLE); //Initializes serial port 2 USART_INIT(9600,2,ENABLE); } void loop(void) { uint8_t AddSDstr[256],length; // //Receive data from bluetooth length = USART_RECV_DATA_LEN(AddSDstr,2); //send a string to bluetooth USART_Write_LEN("recv data",strlen("recv data"),2); // //send data to bluetooth USART_Write_LEN(AddSDstr,length,2); }*/ ///*******Test_Module_TYPE == Test_IMU END *******/ ///******Test_Module_TYPE == Test_GPS******/ void Module_setup_gps(void) { // // The value of "Print_Port" is 3 in the retarget.h (serial 3 printing) //// //Initialize serial port 3 testing //// Modules_Power_Control(Zigbee_Power,1); //// USART_INIT(9600,3,ENABLE); // // // The value of "Print_Port" is 2 in the retarget.h (Bluetooth printing) // //Initialize serial port 2 testing(blue) // Modules_Power_Control(IMU_Blue_Power,1); USART_INIT(9600,2,ENABLE); // //GPS power on Modules_Power_Control(GPS_Power,1); // //GPS config GPS_Config(); }
int at91usart_cn_attach(bus_space_tag_t iot, bus_addr_t iobase, bus_space_handle_t ioh, uint32_t mstclk, int ospeed, tcflag_t cflag) { cn_tab = &at91usart_cons; cn_init_magic(&at91usart_cnm_state); cn_set_magic("\047\001"); usart_cn_sc.sc_iot = iot; usart_cn_sc.sc_ioh = ioh; usart_cn_sc.sc_hwbase = iobase; usart_cn_sc.sc_ospeed = ospeed; usart_cn_sc.sc_cflag = cflag; USART_INIT(mstclk, ospeed); return (0); }
int main(void) { unsigned int XXL,YXL,ZXL; //unsigned int ADC1,ADC2,ADC3; // int16_t ZGY; unsigned int XGY,YGY,ZGY; // unsigned char XYZ_buffer[100]; unsigned int timer=0; unsigned int timer_old=0; unsigned int diff_counter=0; unsigned int m_IMU_count=0; double m_XXL_sum=0; double m_YXL_sum=0; double m_ZXL_sum=0; double m_XGY_sum=0; double m_YGY_sum=0; double m_ZGY_sum=0; clk_init(); port_init(); USART_INIT(); timer_Init(); int_init(); // sei(); // spi_init(); /* Init SS pin as output with wired AND and pull-up. */ //120813 portC를 D로 바꿈 SPI인거 같아서 PORTD.DIRSET = PIN0_bm; PORTD.PIN0CTRL = PORT_OPC_WIREDANDPULL_gc; PORTD.DIRSET = PIN1_bm; PORTD.PIN1CTRL = PORT_OPC_WIREDANDPULL_gc; /* Set SS output to high. (No slave addressed). */ PORTD.OUTSET = PIN0_bm; PORTD.OUTSET = PIN1_bm; /* Instantiate pointer to ssPort. */ PORT_t *ssPort = &PORTD; /* Initialize SPI master on port D. */ SPI_MasterInit(&spiMasterD, &SPID, &PORTD, false, SPI_MODE_3_gc, SPI_INTLVL_OFF_gc, false, SPI_PRESCALER_DIV16_gc); /* Use USARTD0 and initialize buffers. */ USART_InterruptDriver_Initialize(&USARTD0_data, &USARTD0, USART_DREINTLVL_HI_gc); /* Enable RXC interrupt. */ USART_RxdInterruptLevel_Set(USARTD0_data.usart, USART_RXCINTLVL_HI_gc); /* Enable PMIC interrupt level low. */ PMIC.CTRL |= PMIC_HILVLEX_bm; /* Enable global interrupts.*/ sei(); /* Initialize ACC & Gyro */ // Init_L3G4200DH(); Init_LIS3DH(); SPI_MasterCreateDataPacket(&dataPacket, masterSendData_gyro_init, GYRO_DATA, NUM_BYTES, &PORTD, PIN1_bm); SPI_MasterSSLow(ssPort, PIN1_bm); /* Transceive packet. */ SPI_MasterTransceivePacket(&spiMasterD, &dataPacket); /* MASTER: Release SS to slave. */ SPI_MasterSSHigh(ssPort, PIN1_bm); // USART_Rx_Enable(&USARTD0); // USART_Tx_Enable(&USARTD0); double z_deg=0; unsigned int oldT=0; unsigned int newT=0; unsigned int dT=0; int i; double x1m=0; double x1p=0; double P1p=1.0; double x2m[4]={0,0,0,0}; double x2p[4]={0,0,0,0}; double P2p[4][4]={{0.1,0,0,0}, {0,0.1,0,0}, {0,0,0.1,0}, {0,0,0,0.1}}; double enc_time_cnt=0; for(i=0;i<DELAY_COMP_G;i++) { m_XGY_buf[i]=0.0; m_YGY_buf[i]=0.0; m_ZGY_buf[i]=0.0; m_XXL_buf[i]=0.0; m_YXL_buf[i]=0.0; m_ZXL_buf[i]=0.0; m_enc_buf[i]=0.0; m_enc_timebuf[i]=0.0; m_T_buf[i]=0.0; } //char XYZ_buffer_debug[20]; //sprintf((char*)XYZ_buffer_debug,"Hello W"); //uartSendTX((unsigned char*)XYZ_buffer_debug); while(1) { //if(1) if(samplingFlag) { // adc_start_conversion(&ADCA, ADC_CH0); samplingFlag=0; timer=TCC0.CNT; diff_counter=timer-timer_old; /* Create data packet (SS to slave by PC0). */ SPI_MasterCreateDataPacket(&dataPacket, masterSendData, ACC_DATA, NUM_BYTES, &PORTD, PIN0_bm); /* MASTER: Pull SS line low. This has to be done since * SPI_MasterTransceiveByte() does not control the SS line(s). */ SPI_MasterSSLow(ssPort, PIN0_bm); _delay_us(5); /* Transceive packet. */ SPI_MasterTransceivePacket(&spiMasterD, &dataPacket); /* MASTER: Release SS to slave. */ _delay_us(5); SPI_MasterSSHigh(ssPort, PIN0_bm); /* Create data packet (SS to slave by PC1). */ SPI_MasterCreateDataPacket(&dataPacket, masterSendData_gyro, GYRO_DATA, NUM_BYTES, &PORTD, PIN1_bm); /* MASTER: Pull SS line low. This has to be done since * SPI_MasterTransceiveByte() does not control the SS line(s). */ SPI_MasterSSLow(ssPort, PIN1_bm); /* Transceive packet. */ _delay_us(5); SPI_MasterTransceivePacket(&spiMasterD, &dataPacket); /* MASTER: Release SS to slave. */ _delay_us(5); SPI_MasterSSHigh(ssPort, PIN1_bm); timer_old=timer; T=(double)diff_counter/2000000.0*32.0; ACC_DATA[2]=ACC_DATA[2]+0x80; ACC_DATA[4]=ACC_DATA[4]+0x80; ACC_DATA[6]=ACC_DATA[6]+0x80; YXL= (unsigned int)ACC_DATA[2]*256+ACC_DATA[1]; XXL= (unsigned int)ACC_DATA[4]*256+ACC_DATA[3]; ZXL= (unsigned int)ACC_DATA[6]*256+ACC_DATA[5]; GYRO_DATA[2]=GYRO_DATA[2]+0x80; GYRO_DATA[4]=GYRO_DATA[4]+0x80; GYRO_DATA[6]=GYRO_DATA[6]+0x80; XGY= (unsigned int)GYRO_DATA[4]*256+GYRO_DATA[3]; YGY= (unsigned int)GYRO_DATA[2]*256+GYRO_DATA[1]; ZGY= (unsigned int)GYRO_DATA[6]*256+GYRO_DATA[5]; if(m_IMU_count<Tsample) { m_IMU_count++; } else if(m_IMU_count<Tsample+Bsample) { m_XXL_sum+=XXL; m_YXL_sum+=YXL; m_ZXL_sum+=ZXL; //m_XGY_sum+=XGY; //m_YGY_sum+=YGY; m_ZGY_sum+=ZGY; m_IMU_count++; } else if(m_IMU_count==Tsample+Bsample) { //SetTimer(25,1,NULL); m_biasXXL=(double)m_XXL_sum/(double)Bsample; m_biasYXL=(double)m_YXL_sum/(double)Bsample; m_biasZXL=(double)m_ZXL_sum/(double)Bsample-GRAVITY_COUNT; //m_biasXGY=(double)m_XGY_sum/(double)Bsample; //m_biasYGY=(double)m_YGY_sum/(double)Bsample; m_biasZGY=(double)m_ZGY_sum/(double)Bsample; gravityVect[0]=0; gravityVect[1]=0; gravityVect[2]=SCALE_ZXL*GRAVITY_COUNT; m_IMU_count++; } else { //encoder_interface(0); //encoder_interface(1); //host_interface(); //unsigned int a=TCC0.CNT; //position_estimator(XXL,YXL,ZXL,XGY,YGY,ZGY); //unsigned int b=TCC0.CNT; //TJX=(double)(b-a)/2000000.0*32.0; //FF_controller(); newT=TCC0.CNT; dT=newT-oldT; //////////////////////////////////////////////// /////////////////////////////////////////////////////// /////////////////////////////////////////////////////// /////////////////////////////////////////////////////// /////////////////////////////////////////////////////// /////////////////////////////////////////////////////// /////////////////////////////////////////////////////// int i,j,k; m_XXL=-SCALE_XXL*((double)XXL-(double)m_biasXXL); m_YXL=-SCALE_YXL*((double)YXL-(double)m_biasYXL); m_ZXL=SCALE_ZXL*((double)ZXL-(double)m_biasZXL); m_XGY=-SCALE_XGY*((double)XGY-(double)m_biasXGY);//-0.001212142/0.00015711 m_YGY=SCALE_YGY*((double)YGY-(double)m_biasYGY);//+ //if(ZGY<3000) m_ZGY=SCALE_ZGY*((double)ZGY-(double)m_biasZGY+65536.0); //else m_ZGY=SCALE_ZGY*((double)ZGY-(double)m_biasZGY);//+ m_ZGY=SCALE_ZGY*((double)ZGY-(double)m_biasZGY); sprintf(XYZ_buffer,"%u %u %u %u %u %u \n",XXL, YXL, ZXL, XGY, YGY, ZGY); uartC0SendTX((unsigned char*)XYZ_buffer); } } } while(true) { nop(); } }
void at91usart_attach_subr(struct at91usart_softc *sc, struct at91bus_attach_args *sa) { struct tty *tp; int err; printf("\n"); if (bus_space_map(sa->sa_iot, sa->sa_addr, sa->sa_size, 0, &sc->sc_ioh)) panic("%s: Cannot map registers", device_xname(sc->sc_dev)); sc->sc_iot = sa->sa_iot; sc->sc_hwbase = sa->sa_addr; sc->sc_dmat = sa->sa_dmat; sc->sc_pid = sa->sa_pid; /* allocate fifos */ err = at91pdc_alloc_fifo(sc->sc_dmat, &sc->sc_rx_fifo, AT91USART_RING_SIZE, BUS_DMA_READ | BUS_DMA_STREAMING); if (err) panic("%s: cannot allocate rx fifo", device_xname(sc->sc_dev)); err = at91pdc_alloc_fifo(sc->sc_dmat, &sc->sc_tx_fifo, AT91USART_RING_SIZE, BUS_DMA_WRITE | BUS_DMA_STREAMING); if (err) panic("%s: cannot allocate tx fifo", device_xname(sc->sc_dev)); /* initialize uart */ at91_peripheral_clock(sc->sc_pid, 1); at91usart_writereg(sc, US_IDR, -1); at91usart_writereg(sc, US_RTOR, 12); // 12-bit timeout at91usart_writereg(sc, US_PDC + PDC_PTCR, PDC_PTCR_TXTDIS | PDC_PTCR_RXTDIS); at91_intr_establish(sa->sa_pid, IPL_TTY, INTR_HIGH_LEVEL, at91usart_intr, sc); USART_INIT(sc, 115200U); #ifdef NOTYET if (sc->sc_iot == usart_cn_sc.sc_iot && sc->sc_hwbase == usart_cn_sc.sc_hwbase) { usart_cn_sc.sc_attached = 1; /* Make sure the console is always "hardwired". */ delay(10000); /* wait for output to finish */ SET(sc->sc_hwflags, COM_HW_CONSOLE); SET(sc->sc_swflags, TIOCFLAG_SOFTCAR); SET(sc->sc_ier, USART_INT_RXRDY); USARTREG(USART_IER) = USART_INT_RXRDY; // @@@@@ } #endif // NOTYET tp = tty_alloc(); tp->t_oproc = at91usart_start; tp->t_param = at91usart_param; tp->t_hwiflow = at91usart_hwiflow; sc->sc_tty = tp; tty_attach(tp); #if NOTYET if (ISSET(sc->sc_hwflags, COM_HW_CONSOLE)) { int maj; /* locate the major number */ maj = cdevsw_lookup_major(&at91usart_cdevsw); cn_tab->cn_dev = makedev(maj, device_unit(sc->sc_dev)); aprint_normal("%s: console (maj %u min %u cn_dev %u)\n", device_xname(sc->sc_dev), maj, device_unit(sc->sc_dev), cn_tab->cn_dev); } #endif /* NOTYET */ sc->sc_si = softint_establish(SOFTINT_SERIAL, at91usart_soft, sc); #ifdef RND_COM rnd_attach_source(&sc->rnd_source, device_xname(sc->sc_dev), RND_TYPE_TTY, RND_FLAG_DEFAULT); #endif /* if there are no enable/disable functions, assume the device is always enabled */ if (!sc->enable) sc->enabled = 1; /* XXX configure register */ /* xxx_config(sc) */ SET(sc->sc_hwflags, COM_HW_DEV_OK); }
int main(void) { unsigned int XXL,YXL,ZXL,XGL,YGL,ZGL; unsigned char XYZ_buffer[100]; clk_init(); port_init(); USART_INIT(); // spi_init(); /* Init SS pin as output with wired AND and pull-up. */ PORTC.DIRSET = PIN0_bm; PORTC.PIN0CTRL = PORT_OPC_WIREDANDPULL_gc; PORTC.DIRSET = PIN1_bm; PORTC.PIN1CTRL = PORT_OPC_WIREDANDPULL_gc; /* Set SS output to high. (No slave addressed). */ PORTC.OUTSET = PIN0_bm; PORTC.OUTSET = PIN1_bm; /* Instantiate pointer to ssPort. */ PORT_t *ssPort = &PORTC; /* Initialize SPI master on port C. */ SPI_MasterInit(&spiMasterC, &SPIC, &PORTC, false, SPI_MODE_3_gc, SPI_INTLVL_OFF_gc, false, SPI_PRESCALER_DIV16_gc); /* Initialize ACC & Gyro */ Init_L3G4200DH(); Init_LIS331DLH(); /* Read Sensor data */ while(true) { /* Create data packet (SS to slave by PC0). */ SPI_MasterCreateDataPacket(&dataPacket, masterSendData, ACC_DATA, NUM_BYTES, &PORTC, PIN0_bm); /* MASTER: Pull SS line low. This has to be done since * SPI_MasterTransceiveByte() does not control the SS line(s). */ SPI_MasterSSLow(ssPort, PIN0_bm); /* Transceive packet. */ SPI_MasterTransceivePacket(&spiMasterC, &dataPacket); /* MASTER: Release SS to slave. */ SPI_MasterSSHigh(ssPort, PIN0_bm); /* Create data packet (SS to slave by PC1). */ SPI_MasterCreateDataPacket(&dataPacket, masterSendData, GYRO_DATA, NUM_BYTES, &PORTC, PIN1_bm); /* MASTER: Pull SS line low. This has to be done since * SPI_MasterTransceiveByte() does not control the SS line(s). */ SPI_MasterSSLow(ssPort, PIN1_bm); /* Transceive packet. */ SPI_MasterTransceivePacket(&spiMasterC, &dataPacket); /* MASTER: Release SS to slave. */ SPI_MasterSSHigh(ssPort, PIN1_bm); ACC_DATA[2]=ACC_DATA[2]+0x80; ACC_DATA[4]=ACC_DATA[4]+0x80; ACC_DATA[6]=ACC_DATA[6]+0x80; XXL= (unsigned int)ACC_DATA[2]*256+ACC_DATA[1]; YXL= (unsigned int)ACC_DATA[4]*256+ACC_DATA[3]; ZXL= (unsigned int)ACC_DATA[6]*256+ACC_DATA[5]; GYRO_DATA[2]=GYRO_DATA[2]+0x80; GYRO_DATA[4]=GYRO_DATA[4]+0x80; GYRO_DATA[6]=GYRO_DATA[6]+0x80; XGL= (unsigned int)GYRO_DATA[2]*256+GYRO_DATA[1]; YGL= (unsigned int)GYRO_DATA[4]*256+GYRO_DATA[3]; ZGL= (unsigned int)GYRO_DATA[6]*256+GYRO_DATA[5]; sprintf((char*)XYZ_buffer,"[XXL%6u][YXL%6u][ZXL%6u][XGY%6u][YGY%6u][ZGY%6u]\r\n",XXL,YXL,ZXL,XGL,YGL,ZGL); //uartSendTXbit('3'); //sprintf((char*)XYZ_buffer,"abcdefgefgh12341\n\r"); uartSendTX(XYZ_buffer); } while(true) { nop(); } }