/*******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();
}
}
