int main ( void )
{
fun_queue = queueInit(FUN_Q_LEN);
rx_pay_queue = pqInit(12); //replace 12 with a #define const later
test_function tf;
/* Initialization */
SetupClock();
SwitchClocks();
SetupPorts();
SetupInterrupts();
SetupI2C();
SetupADC();
SetupTimer1();
SetupPWM();
SetupTimer2();
gyroSetup();
xlSetup();
dfmemSetup();
WordVal pan_id = {RADIO_PAN_ID};
WordVal src_addr = {RADIO_SRC_ADDR};
WordVal dest_addr = {RADIO_DEST_ADDR};
radioInit(src_addr, pan_id, RADIO_RXPQ_MAX_SIZE, RADIO_TXPQ_MAX_SIZE);
radioSetDestAddr(dest_addr);
radioSetChannel(RADIO_MY_CHAN);
char j;
for(j=0; j<3; j++){
LED_2 = ON;
delay_ms(500);
LED_2 = OFF;
delay_ms(500);
}
LED_2 = ON;
EnableIntT2;
while(1){
while(!queueIsEmpty(fun_queue))
{
rx_payload = pqPop(rx_pay_queue);
tf = (test_function)queuePop(fun_queue);
(*tf)(payGetType(rx_payload), payGetStatus(rx_payload), payGetDataLength(rx_payload), payGetData(rx_payload));
payDelete(rx_payload);
}
}
return 0;
}
int main(void)
{
//
// Enable lazy stacking for interrupt handlers. This allows floating-point
// instructions to be used within interrupt handlers, but at the expense of
// extra stack usage.
//
ROM_FPULazyStackingEnable();
//
// Setup the system clock to run at 50 Mhz from PLL with crystal reference
//
SysCtlClockSet(SYSCTL_SYSDIV_4|SYSCTL_USE_PLL|SYSCTL_XTAL_16MHZ|
SYSCTL_OSC_MAIN);
//
SetupUART();
SetupI2C();
//
UARTprintf("Starting....\n");
if(!L3G4200D_CheckDevice())
{
UARTprintf("L3G4200D not detected\n");
while(1);
}
SetupL3G4200D();
while(1)
{
short x,y,z;
char buff[20];
L3G4200D_ReadRaw(&x,&y,&z);
ftoa(x,buff);
UARTprintf("X: %s,",buff);
ftoa(y,buff);
UARTprintf("Y: %s,",buff);
ftoa(z,buff);
UARTprintf("Z: %s\n",buff);
DELAY_MS(20);
}
}
int mpu6050_init()
{
/*
** Configures I2C to Master mode to generate start
** condition on I2C bus and to transmit data at a
** speed of 100khz
*/
SetupI2C();
rt_hw_interrupt_install(SYS_INT_I2C0INT, mpu6050_isr, NULL, "mpu6050");
rt_hw_interrupt_mask(SYS_INT_I2C0INT);
int result = mpu_init();
if(!result)
{
rt_kprintf("mpu initialization complete......\n "); //mpu_set_sensor
if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
rt_kprintf("mpu_set_sensor complete ......\n");
else
rt_kprintf("mpu_set_sensor come across error ......\n");
if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_configure_fifo
rt_kprintf("mpu_configure_fifo complete ......\n");
else
rt_kprintf("mpu_configure_fifo come across error ......\n");
if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) //mpu_set_sample_rate
rt_kprintf("mpu_set_sample_rate complete ......\n");
else
rt_kprintf("mpu_set_sample_rate error ......\n");
if(!dmp_load_motion_driver_firmware()) //dmp_load_motion_driver_firmvare
rt_kprintf("dmp_load_motion_driver_firmware complete ......\n");
else
rt_kprintf("dmp_load_motion_driver_firmware come across error ......\n");
if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) //dmp_set_orientation
rt_kprintf("dmp_set_orientation complete ......\n");
else
rt_kprintf("dmp_set_orientation come across error ......\n");
if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL)) //dmp_enable_feature
rt_kprintf("dmp_enable_feature complete ......\n");
else
rt_kprintf("dmp_enable_feature come across error ......\n");
if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) //dmp_set_fifo_rate
rt_kprintf("dmp_set_fifo_rate complete ......\n");
else
rt_kprintf("dmp_set_fifo_rate come across error ......\n");
run_self_test();
if(!mpu_set_dmp_state(1))
rt_kprintf("mpu_set_dmp_state complete ......\n");
else
rt_kprintf("mpu_set_dmp_state come across error ......\n");
}
while(1)
{
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
&more);
/* Gyro and accel data are written to the FIFO by the DMP in chip
* frame and hardware units. This behavior is convenient because it
* keeps the gyro and accel outputs of dmp_read_fifo and
* mpu_read_fifo consistent.
*/
/* if (sensors & INV_XYZ_GYRO )
send_packet(PACKET_TYPE_GYRO, gyro);
if (sensors & INV_XYZ_ACCEL)
send_packet(PACKET_TYPE_ACCEL, accel); */
/* Unlike gyro and accel, quaternions are written to the FIFO in
* the body frame, q30. The orientation is set by the scalar passed
* to dmp_set_orientation during initialization.
*/
if (sensors & INV_WXYZ_QUAT )
{
q0=quat[0] / q30;
q1=quat[1] / q30;
q2=quat[2] / q30;
q3=quat[3] / q30;
Pitch = asin(2 * q1 * q3 - 2 * q0* q2)* 57.3; // pitch
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
// printf("Pitch=%.2f ,Roll=%.2f ,Yaw=%.2f \n",Pitch,Roll,Yaw);
UART1_ReportIMU(Yaw*10, Pitch*10, Roll*10,0,0,0,100);
delay_ms(10);
}
}
return 0;
}
