void Init_L3G4200DH(void)
{
unsigned char write_register=0;
//CTRL_REG1
//DR1 DR0 BW1 BW0 PD Zen Xen Yen
//0 0 1 1 1 1 1 1
//ODR=100Hz
write_register=0x3f;
SPI_MasterSSLow(ssPort, PIN1_bm);
SPI_MasterTransceiveByte(&spiMasterC, 0x20);
SPI_MasterTransceiveByte(&spiMasterC, write_register);
// spiSendByte(0x20);
// spiSendByte(write_register);
SPI_MasterSSHigh(ssPort, PIN1_bm);
//CTRL_REG2
//0 0 HPM1 HPM1 HPCF3 HPCF2 HPCF1 HPCF0
//0 0 0 0 0 0 0 0
write_register=0x00;
SPI_MasterSSLow(ssPort, PIN1_bm);
SPI_MasterTransceiveByte(&spiMasterC, 0x21);
SPI_MasterTransceiveByte(&spiMasterC, write_register);
SPI_MasterSSHigh(ssPort, PIN1_bm);
//CTRL_REG3
//I1_Int1 I1_Boot H_Lactive PP_OD I2_DRDY I2_WTM I2_ORun I2_Empty
// 0 0 0 0 0 0 0 0
write_register=0x00;
SPI_MasterSSLow(ssPort, PIN1_bm);
SPI_MasterTransceiveByte(&spiMasterC, 0x22);
SPI_MasterTransceiveByte(&spiMasterC, write_register);
SPI_MasterSSHigh(ssPort, PIN1_bm);
//CTRL_REG4
//BDU BLE FS1 FS0 - ST1 ST0 SIM
// 1 0 0 0 0 0 0 0
write_register=0x80;
SPI_MasterSSLow(ssPort, PIN1_bm);
SPI_MasterTransceiveByte(&spiMasterC, 0x23);
SPI_MasterTransceiveByte(&spiMasterC, write_register);
SPI_MasterSSHigh(ssPort, PIN1_bm);
//CTRL_REG5
//BOOT FIFO_EN - HPen INT1_Sel1 INT1_Sel0 Out_Sel1 Out_Sel0
// 0 0 0 0 0 0 0 0
write_register=0x00;
SPI_MasterSSLow(ssPort, PIN1_bm);
SPI_MasterTransceiveByte(&spiMasterC, 0x24);
SPI_MasterTransceiveByte(&spiMasterC, write_register);
SPI_MasterSSHigh(ssPort, PIN1_bm);
}
void nrf24l01_RX_config_slave(void)
{
ioport_set_pin_low(nrf24l01S_CE);
SPI_MasterSSHigh(&PORTC, PIN4_bm);
delay_us(20);
SPI_MasterSSLow(&PORTC, PIN4_bm);
delay_us(20);
rf_writebuf_slave(WRITE_REG + RX_ADDR_P0, TX_ADDRESS, TX_ADR_WIDTH);
rf_writereg_slave(WRITE_REG + EN_AA, 0x01);//enable autoactive 0x01
rf_writereg_slave(WRITE_REG + EN_RXADDR, 0x01);
rf_writereg_slave(WRITE_REG + RF_CH, 40);
rf_writereg_slave(WRITE_REG + RX_PW_P0, TX_PLOAD_WIDTH);
rf_writereg_slave(WRITE_REG + RF_SETUP, 0x09);
rf_writereg_slave(WRITE_REG + CONFIG, 0x0f);
//SPI_MasterSSLow(ssPort, PIN4_bm);
ioport_set_pin_high(nrf24l01S_CE);
delay_us(150);//at least 130us
PORT_ConfigurePins( &PORTC,
0x01, //set pin PK0 as input 'IRQ';
false,
false,
PORT_OPC_TOTEM_gc,
PORT_ISC_FALLING_gc );//set falling edge as trigger;
PORT_SetPinsAsInput( &PORTC, 0x01 );
/* Configure Interrupt0 to have medium interrupt level, triggered by pin 0. */
PORT_ConfigureInterrupt0( &PORTC, PORT_INT0LVL_MED_gc, 0x01 );
}
void Init_LIS331DLH(void)
{
unsigned char write_register=0;
//CTRL_REG1
//PM2 PM1 PM0 DR1 DR0 Zen Yen Xen
//0 0 1 0 1 1 1 1
//ODR=100Hz
write_register=0x2f;
// write_register=DR
SPI_MasterSSLow(ssPort, PIN0_bm);
SPI_MasterTransceiveByte(&spiMasterC, 0x20);
SPI_MasterTransceiveByte(&spiMasterC, write_register);
SPI_MasterSSHigh(ssPort, PIN0_bm);
//CTRL_REG2
//BOOT HPM1 HPM0 FDS HPen2 HPen1 HPCF1 HPCF0
//0 0 0 0 0 0 00
write_register=0x00;
SPI_MasterSSLow(ssPort, PIN0_bm);
SPI_MasterTransceiveByte(&spiMasterC, 0x21);
SPI_MasterTransceiveByte(&spiMasterC, write_register);
SPI_MasterSSHigh(ssPort, PIN0_bm);
//CTRL_REG3
//IHL PP_OD LIR2 I2_CFG1 I2_CFG0 LIR1 I1_CFG1 I1_CFG0
//0 0 0 0 0 0 1 0
write_register=0x02;
SPI_MasterSSLow(ssPort, PIN0_bm);
SPI_MasterTransceiveByte(&spiMasterC, 0x22);
SPI_MasterTransceiveByte(&spiMasterC, write_register);
SPI_MasterSSHigh(ssPort, PIN0_bm);
//CTRL_REG4
//BDU BLE FS1 FS0 STsign 0 ST SIM
//1 0 0 0 0 0 0 0
write_register=0x80;
SPI_MasterSSLow(ssPort, PIN0_bm);
SPI_MasterTransceiveByte(&spiMasterC, 0x23);
SPI_MasterTransceiveByte(&spiMasterC, write_register);
SPI_MasterSSHigh(ssPort, PIN0_bm);
//CTRL_REG5
//0 0 0 0 0 0 TurnOn1 TurnOn0
//0 0 0 0 0 0 0 0
write_register=0x00;
SPI_MasterSSLow(ssPort, PIN0_bm);
SPI_MasterTransceiveByte(&spiMasterC, 0x24);
SPI_MasterTransceiveByte(&spiMasterC, write_register);
SPI_MasterSSHigh(ssPort, PIN0_bm);
}
uint8_t rf_writereg_slave(uint8_t reg, uint8_t val)
{
uint8_t status;
SPI_MasterSSLow(&PORTC, PIN4_bm);
status = SPI_MasterTransceiveByte(&spiMasterC, reg);
SPI_MasterTransceiveByte(&spiMasterC, val);
SPI_MasterSSHigh(&PORTC, PIN4_bm);
return status;
}
/*! \brief SPI transceive data packet
*
* This function transceives a number of bytes contained in a data packet
* struct. The SS line is kept low until all bytes are transceived. The
* received bytes are stored in the data packet struct.
*
* \param spi The SPI_Master_t struct instance.
* \param dataPacket The SPI_dataPacket_t struct instance.
*
* \return Wether the function was successfully completed
* \retval true Success
* \retval false Failure
*/
bool SPI_MasterTransceivePacket(SPI_Master_t *spi,
SPI_DataPacket_t *dataPacket)
{
/* Check if data packet has been created. */
if(dataPacket == NULL) {
return false;
}
/* Assign datapacket to SPI module. */
spi->dataPacket = dataPacket;
uint8_t ssPinMask = spi->dataPacket->ssPinMask;
/* If SS signal to slave(s). */
if (spi->dataPacket->ssPort != NULL) {
/* SS to slave(s) low. */
SPI_MasterSSLow(spi->dataPacket->ssPort, ssPinMask);
}
/* Transceive bytes. */
uint8_t bytesTransceived = 0;
uint8_t bytesToTransceive = dataPacket->bytesToTransceive;
while (bytesTransceived < bytesToTransceive) {
/* Send pattern. */
uint8_t data = spi->dataPacket->transmitData[bytesTransceived];
spi->module->DATA = data;
/* Wait for transmission complete. */
while(!(spi->module->STATUS & SPI_IF_bm)) {
}
/* Read received data. */
data = spi->module->DATA;
spi->dataPacket->receiveData[bytesTransceived] = data;
bytesTransceived++;
}
/* If SS signal to slave(s). */
if (spi->dataPacket->ssPort != NULL) {
/* Release SS to slave(s). */
SPI_MasterSSHigh(spi->dataPacket->ssPort, ssPinMask);
}
/* Set variables to indicate that transmission is complete. */
spi->dataPacket->bytesTransceived = bytesTransceived;
spi->dataPacket->complete = true;
/* Report success. */
return true;
}
uint8_t rf_readbuf_master(uint8_t reg, uint8_t readbuf[TX_PLOAD_WIDTH], uint8_t bytes)
{
uint8_t status;
uint16_t i;
SPI_MasterSSLow(&PORTF, PIN4_bm);
status = SPI_MasterTransceiveByte(&spiMasterF, reg);
for (i=0; i<bytes; i++)
{
readbuf[i] = SPI_MasterTransceiveByte(&spiMasterF, 0xff);
}
SPI_MasterSSHigh(&PORTF, PIN4_bm);
return(status);
}
uint8_t rf_writebuf_slave(uint8_t reg, uint8_t * writebuf, uint8_t bytes)
{
uint8_t status;
uint16_t i;
SPI_MasterSSLow(&PORTC, PIN4_bm);
status = SPI_MasterTransceiveByte(&spiMasterC, reg);
for (i=0; i<bytes; i++)
{
SPI_MasterTransceiveByte(&spiMasterC, writebuf[i]);
}
SPI_MasterSSHigh(&PORTC, PIN4_bm);
return(status);
}
uint8_t rf_readreg_slave(uint8_t reg)
{
uint8_t val;
SPI_MasterSSHigh(&PORTC, PIN4_bm);
delay_us(20);
SPI_MasterSSLow(&PORTC, PIN4_bm);
delay_us(20);
SPI_MasterTransceiveByte(&spiMasterC, reg);
val = SPI_MasterTransceiveByte(&spiMasterC, 0xff);
SPI_MasterSSHigh(&PORTC, PIN4_bm);
return val;
}
/*! \brief Start transmission.
*
* This function starts a SPI transmission. A data packet must be prepared
* for transmission first.
*
* \param spi The SPI_Master_t struct instance.
* \param dataPacket The SPI_dataPacket_t struct instance.
*
* \return Status code
* \retval SPI_OK The transmission was completed successfully.
* \retval SPI_BUSY The SPI module is busy.
* \retval SPI_INTERRUPTED The transmission was interrupted by another master.
*/
uint8_t SPI_MasterInterruptTransceivePacket(SPI_Master_t *spi,
SPI_DataPacket_t *dataPacket)
{
uint8_t data;
bool interrupted = spi->interrupted;
/* If no packets sent so far. */
if (spi->dataPacket == NULL) {
spi->dataPacket = dataPacket;
}
/* If ongoing transmission. */
else if (spi->dataPacket->complete == false) {
return (SPI_BUSY);
}
/* If interrupted by other master. */
else if (interrupted) {
/* If SS released. */
if (spi->port->OUT & SPI_SS_bm) {
/* No longer interrupted. */
interrupted = false;
}
else {
return (SPI_INTERRUPTED);
}
}
/* NOT interrupted by other master.
* Start transmission. */
spi->dataPacket = dataPacket;
spi->dataPacket->complete = false;
spi->interrupted = false;
/* SS to slave(s) low.*/
uint8_t ssPinMask = spi->dataPacket->ssPinMask;
SPI_MasterSSLow(spi->dataPacket->ssPort, ssPinMask);
spi->dataPacket->bytesTransceived = 0;
/* Start sending data. */
data = spi->dataPacket->transmitData[0];
spi->module->DATA = data;
/* Successs */
return (SPI_OK);
}
void nrf24l01_TX_config_slave(uint8_t * writebuf)
{
ioport_set_pin_low(nrf24l01S_CE);
SPI_MasterSSHigh(&PORTC, PIN4_bm);
delay_us(20);
SPI_MasterSSLow(&PORTC, PIN4_bm);
delay_us(20);
rf_writebuf_slave(WRITE_REG + TX_ADDR, TX_ADDRESS, TX_ADR_WIDTH);
rf_writebuf_slave(WRITE_REG + RX_ADDR_P0, TX_ADDRESS, TX_ADR_WIDTH);
rf_writebuf_slave(WR_TX_PLOAD, writebuf, TX_PLOAD_WIDTH);
rf_writereg_slave(WRITE_REG + EN_AA, 0x01);//enable autoactive 0x01
rf_writereg_slave(WRITE_REG + EN_RXADDR, 0x01);
rf_writereg_slave(WRITE_REG + SETUP_RETR, 0x0a);
rf_writereg_slave(WRITE_REG + RF_CH, 40);
rf_writereg_slave(WRITE_REG + RF_SETUP, 0x09);
rf_writereg_slave(WRITE_REG + CONFIG, 0x0e);
ioport_set_pin_high(nrf24l01S_CE);
delay_us(12);//at least 10us
}
void nrf24l01_RX_config_master(void)
{
ioport_set_pin_low(nrf24l01M_CE);
SPI_MasterSSHigh(&PORTF, PIN4_bm);
delay_us(20);
SPI_MasterSSLow(&PORTF, PIN4_bm);
delay_us(20);
rf_writebuf_master(WRITE_REG + RX_ADDR_P0, TX_ADDRESS, TX_ADR_WIDTH);
rf_writereg_master(WRITE_REG + EN_AA, 0x01);//enable autoactive 0x01
rf_writereg_master(WRITE_REG + EN_RXADDR, 0x01);
rf_writereg_master(WRITE_REG + RF_CH, 40);
rf_writereg_master(WRITE_REG + RX_PW_P0, TX_PLOAD_WIDTH);
//rf_writereg(WRITE_REG + RF_SETUP, 0x07);//1Mbps, 0dBm
rf_writereg_master(WRITE_REG + RF_SETUP, 0x09);
rf_writereg_master(WRITE_REG + CONFIG, 0x0f);
//SPI_MasterSSLow(ssPort, PIN4_bm);
ioport_set_pin_high(nrf24l01M_CE);
delay_us(150);//at least 130us
}
/*
void Init_L3G4200DH(void)
{
unsigned char write_register=0;
//CTRL_REG1
//DR1 DR0 BW1 BW0 PD Zen Xen Yen
//0 0 1 1 1 1 1 1
//ODR=100Hz
write_register=0x3f;
SPI_MasterSSLow(ssPort, PIN1_bm);
SPI_MasterTransceiveByte(&spiMasterC, 0x20);
SPI_MasterTransceiveByte(&spiMasterC, 0x3f);
// spiSendByte(0x20);
// spiSendByte(write_register);
SPI_MasterSSHigh(ssPort, PIN1_bm);
//CTRL_REG2
//0 0 HPM1 HPM1 HPCF3 HPCF2 HPCF1 HPCF0
//0 0 0 0 0 0 0 0
write_register=0x00;
SPI_MasterSSLow(ssPort, PIN1_bm);
SPI_MasterTransceiveByte(&spiMasterC, 0x21);
SPI_MasterTransceiveByte(&spiMasterC, 0x00);
SPI_MasterSSHigh(ssPort, PIN1_bm);
//CTRL_REG3
//I1_Int1 I1_Boot H_Lactive PP_OD I2_DRDY I2_WTM I2_ORun I2_Empty
// 0 0 0 0 0 0 0 0
write_register=0x00;
SPI_MasterSSLow(ssPort, PIN1_bm);
SPI_MasterTransceiveByte(&spiMasterC, 0x22);
SPI_MasterTransceiveByte(&spiMasterC, 0x00);
SPI_MasterSSHigh(ssPort, PIN1_bm);
//CTRL_REG4
//BDU BLE FS1 FS0 - ST1 ST0 SIM
// 1 0 0 0 0 0 0 0
write_register=0x80;
SPI_MasterSSLow(ssPort, PIN1_bm);
SPI_MasterTransceiveByte(&spiMasterC, 0x23);
SPI_MasterTransceiveByte(&spiMasterC, 0x80);
SPI_MasterSSHigh(ssPort, PIN1_bm);
//CTRL_REG5
//BOOT FIFO_EN - HPen INT1_Sel1 INT1_Sel0 Out_Sel1 Out_Sel0
// 0 0 0 0 0 0 0 0
write_register=0x00;
SPI_MasterSSLow(ssPort, PIN1_bm);
SPI_MasterTransceiveByte(&spiMasterC, 0x24);
SPI_MasterTransceiveByte(&spiMasterC, 0x00);
SPI_MasterSSHigh(ssPort, PIN1_bm);
}
*/
void Init_LIS3DH(void)
{
unsigned char write_register=0;
//CTRL_REG1
//ODR3 ODR2 ODR1 ODR0 LPen Zen Yen Xen
// 0 1 0 1 0 1 1 1
//ODR=100Hz
write_register=0x57;
// write_register=DR
SPI_MasterSSLow(ssPort, PIN0_bm);
_delay_us(10);
SPI_MasterTransceiveByte(&spiMasterD, 0x20);
SPI_MasterTransceiveByte(&spiMasterD, write_register);
_delay_us(10);
SPI_MasterSSHigh(ssPort, PIN0_bm);
_delay_us(10);
//CTRL_REG2
//HPM1 HPM0 HPCF2 HPCF1 FDS HPCLICK HPIS2 HPIS1
// 0 0 0 0 0 0 0 0
write_register=0x00;
SPI_MasterSSLow(ssPort, PIN0_bm);
_delay_us(10);
SPI_MasterTransceiveByte(&spiMasterD, 0x21);
SPI_MasterTransceiveByte(&spiMasterD, write_register);
_delay_us(10);
SPI_MasterSSHigh(ssPort, PIN0_bm);
_delay_us(10);
//CTRL_REG3
//I1_CLICK I1_AOI1 I1_AOI2 I1_DRDY1 I1_DRDY2 I1_WTM I1_OVERRUN --
// 0 0 0 0 0 0 1 0
write_register=0x00;
SPI_MasterSSLow(ssPort, PIN0_bm);
_delay_us(10);
SPI_MasterTransceiveByte(&spiMasterD, 0x22);
SPI_MasterTransceiveByte(&spiMasterD, write_register);
_delay_us(10);
SPI_MasterSSHigh(ssPort, PIN0_bm);
_delay_us(10);
//CTRL_REG4
//BDU BLE FS1 FS0 HR ST1 ST0 SIM
//1 0 0 0 1 0 0 0
write_register=0x88;
SPI_MasterSSLow(ssPort, PIN0_bm);
_delay_us(10);
SPI_MasterTransceiveByte(&spiMasterD, 0x23);
SPI_MasterTransceiveByte(&spiMasterD, write_register);
_delay_us(10);
SPI_MasterSSHigh(ssPort, PIN0_bm);
_delay_us(10);
//CTRL_REG5
//BOOT FIFO_EN -- -- LIR_INT1 D4D_INT1 0 0
//0 0 0 0 0 0 0 0
write_register=0x00;
SPI_MasterSSLow(ssPort, PIN0_bm);
_delay_us(10);
SPI_MasterTransceiveByte(&spiMasterD, 0x24);
SPI_MasterTransceiveByte(&spiMasterD, write_register);
_delay_us(10);
SPI_MasterSSHigh(ssPort, PIN0_bm);
//CTRL_REG6
//I2_CLICKen I2_INT1 0 BOOT_I1 0 - - H_LACTIVE -
// 0 0 0 0 0 0 0 0
write_register=0x00;
SPI_MasterSSLow(ssPort, PIN0_bm);
_delay_us(10);
SPI_MasterTransceiveByte(&spiMasterD, 0x25);
SPI_MasterTransceiveByte(&spiMasterD, write_register);
_delay_us(10);
SPI_MasterSSHigh(ssPort, PIN0_bm);
write_register=0xC0;
SPI_MasterSSLow(ssPort, PIN0_bm);
_delay_us(10);
SPI_MasterTransceiveByte(&spiMasterD, 0x1F);
SPI_MasterTransceiveByte(&spiMasterD, write_register);
_delay_us(10);
SPI_MasterSSHigh(ssPort, PIN0_bm);
}
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();
}
}
/*! \brief Test function.
*
* This function tests the SPI master and slave drivers in polled operation,
* with a master (on port C) communicating with a slave (on port D).
*
* Hardware setup:
*
* - Connect PC4 to PD4 (SS)
* - Connect PC5 to PD5 (MOSI)
* - Connect PC6 to PD6 (MISO)
* - Connect PC7 to PD7 (SCK)
*
* The drivers are tested in two phases:
*
* 1: Data is transmitted on byte at a time from the master to the slave.
* The slave increments the received data and sends it back. The master reads
* the data from the slave and verifies that it equals the data sent + 1.
*
* 2: Data is transmitted 4 bytes at a time to the slave. As the master sends
* a byte to the slave, the preceding byte is sent back to the master.
* When all bytes have been sent, it is verified that the last 3 bytes
* received at the master, equal the first 3 bytes sent.
*
* The variable, 'success', will be non-zero when the function reaches the
* infinite for-loop if the test was successful.
*/
int main(void)
{
/* Init SS pin as output with wired AND and pull-up. */
PORTC.DIRSET = PIN4_bm;
PORTC.PIN4CTRL = PORT_OPC_WIREDANDPULL_gc;
/* Set SS output to high. (No slave addressed). */
PORTC.OUTSET = PIN4_bm;
/* Instantiate pointer to ssPort. */
PORT_t *ssPort = &PORTC;
/* Initialize SPI master on port C. */
SPI_MasterInit(&spiMasterC,
&SPIC,
&PORTC,
false,
SPI_MODE_0_gc,
SPI_INTLVL_LO_gc,
false,
SPI_PRESCALER_DIV4_gc);
/* Initialize SPI slave on port D. */
SPI_SlaveInit(&spiSlaveD,
&SPID,
&PORTD,
false,
SPI_MODE_0_gc,
SPI_INTLVL_LO_gc);
/* PHASE 1: Transceive individual bytes. */
/* MASTER: Pull SS line low. This has to be done since
* SPI_MasterTransceiveByte() does not control the SS line(s). */
SPI_MasterSSLow(ssPort, PIN4_bm);
for(uint8_t i = 0; i < NUM_BYTES; i++) {
/* MASTER: Transmit data from master to slave. */
SPI_MasterTransceiveByte(&spiMasterC, masterSendData[i]);
/* SLAVE: Wait for data to be available. */
while (SPI_SlaveDataAvailable(&spiSlaveD) == false) {
}
/* SLAVE: Get the byte received. */
uint8_t slaveByte = SPI_SlaveReadByte(&spiSlaveD);
/* SLAVE: Increment received byte and send back. */
slaveByte++;
SPI_SlaveWriteByte(&spiSlaveD, slaveByte);
/* MASTER: Transmit dummy data to shift data from slave to master. */
uint8_t masterReceivedByte = SPI_MasterTransceiveByte(&spiMasterC, 0x00);
/* MASTER: Check if the correct value was received. */
if (masterReceivedByte != (masterSendData[i] + 1) ) {
success = false;
}
}
/* MASTER: Release SS to slave. */
SPI_MasterSSHigh(ssPort, PIN4_bm);
/* PHASE 2: Transceive data packet. */
/* Create data packet (SS to slave by PC4). */
SPI_MasterCreateDataPacket(&dataPacket,
masterSendData,
masterReceivedData,
NUM_BYTES,
&PORTC,
PIN4_bm);
/* Transceive packet. */
SPI_MasterTransceivePacket(&spiMasterC, &dataPacket);
/* Check that correct data was received. Assume success at first. */
for (uint8_t i = 0; i < NUM_BYTES - 1; i++) {
if (masterReceivedData[i + 1] != masterSendData[i]) {
success = false;
}
}
while(true) {
nop();
}
}
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();
}
}
