static rt_err_t rt_serial_close(rt_device_t dev)
{
struct rt_ppc405_serial* device;
device = (struct rt_ppc405_serial*) dev;
RT_ASSERT(device != RT_NULL);
if (dev->flag & RT_DEVICE_FLAG_INT_RX)
{
/* mask UART rx interrupt */
rt_hw_interrupt_mask(device->irqno);
}
return RT_EOK;
}
static rt_err_t am33xx_control(struct rt_serial_device *serial, int cmd, void *arg)
{
struct am33xx_uart* uart;
RT_ASSERT(serial != RT_NULL);
uart = (struct am33xx_uart *)serial->parent.user_data;
switch (cmd)
{
case RT_DEVICE_CTRL_CLR_INT:
/* disable rx irq */
rt_hw_interrupt_mask(uart->irq);
break;
case RT_DEVICE_CTRL_SET_INT:
/* enable rx irq */
rt_hw_interrupt_umask(uart->irq);
break;
}
return RT_EOK;
}
int rt_hw_serial_init(void)
{
struct serial_configure config;
poweron_per_domain();
start_uart_clk();
config_pinmux();
#ifdef RT_USING_UART0
config.baud_rate = BAUD_RATE_115200;
config.bit_order = BIT_ORDER_LSB;
config.data_bits = DATA_BITS_8;
config.parity = PARITY_NONE;
config.stop_bits = STOP_BITS_1;
config.invert = NRZ_NORMAL;
config.bufsz = RT_SERIAL_RB_BUFSZ;
serial0.ops = &am33xx_uart_ops;
serial0.config = config;
/* enable RX interrupt */
UART_IER_REG(uart0.base) = 0x01;
/* install ISR */
rt_hw_interrupt_install(uart0.irq, am33xx_uart_isr, &serial0, "uart0");
rt_hw_interrupt_control(uart0.irq, 0, 0);
rt_hw_interrupt_mask(uart0.irq);
/* register UART0 device */
rt_hw_serial_register(&serial0, "uart0",
RT_DEVICE_FLAG_RDWR | RT_DEVICE_FLAG_INT_RX,
&uart0);
#endif
#ifdef RT_USING_UART1
config.baud_rate = BAUD_RATE_115200;
config.bit_order = BIT_ORDER_LSB;
config.data_bits = DATA_BITS_8;
config.parity = PARITY_NONE;
config.stop_bits = STOP_BITS_1;
config.invert = NRZ_NORMAL;
config.bufsz = RT_SERIAL_RB_BUFSZ;
serial1.ops = &am33xx_uart_ops;
serial1.config = config;
/* enable RX interrupt */
UART_IER_REG(uart1.base) = 0x01;
/* install ISR */
rt_hw_interrupt_install(uart1.irq, am33xx_uart_isr, &serial1, "uart1");
rt_hw_interrupt_control(uart1.irq, 0, 0);
rt_hw_interrupt_mask(uart1.irq);
/* register UART0 device */
rt_hw_serial_register(&serial1, "uart1",
RT_DEVICE_FLAG_RDWR | RT_DEVICE_FLAG_INT_RX,
&uart1);
#endif
#ifdef RT_USING_UART2
config.baud_rate = BAUD_RATE_115200;
config.bit_order = BIT_ORDER_LSB;
config.data_bits = DATA_BITS_8;
config.parity = PARITY_NONE;
config.stop_bits = STOP_BITS_1;
config.invert = NRZ_NORMAL;
config.bufsz = RT_SERIAL_RB_BUFSZ;
serial2.ops = &am33xx_uart_ops;
serial2.config = config;
/* enable RX interrupt */
UART_IER_REG(uart2.base) = 0x01;
/* install ISR */
rt_hw_interrupt_install(uart2.irq, am33xx_uart_isr, &serial2, "uart2");
rt_hw_interrupt_control(uart2.irq, 0, 0);
rt_hw_interrupt_mask(uart2.irq);
/* register UART2 device */
rt_hw_serial_register(&serial2, "uart2",
RT_DEVICE_FLAG_RDWR | RT_DEVICE_FLAG_INT_RX,
&uart2);
#endif
#ifdef RT_USING_UART3
config.baud_rate = BAUD_RATE_115200;
config.bit_order = BIT_ORDER_LSB;
config.data_bits = DATA_BITS_8;
config.parity = PARITY_NONE;
config.stop_bits = STOP_BITS_1;
config.invert = NRZ_NORMAL;
config.bufsz = RT_SERIAL_RB_BUFSZ;
serial3.ops = &am33xx_uart_ops;
serial3.config = config;
/* enable RX interrupt */
UART_IER_REG(uart3.base) = 0x01;
/* install ISR */
rt_hw_interrupt_install(uart3.irq, am33xx_uart_isr, &serial3, "uart3");
rt_hw_interrupt_control(uart3.irq, 0, 0);
rt_hw_interrupt_mask(uart3.irq);
/* register UART3 device */
rt_hw_serial_register(&serial3, "uart3",
RT_DEVICE_FLAG_RDWR | RT_DEVICE_FLAG_INT_RX,
&uart3);
#endif
#ifdef RT_USING_UART4
config.baud_rate = BAUD_RATE_115200;
config.bit_order = BIT_ORDER_LSB;
config.data_bits = DATA_BITS_8;
config.parity = PARITY_NONE;
config.stop_bits = STOP_BITS_1;
config.invert = NRZ_NORMAL;
config.bufsz = RT_SERIAL_RB_BUFSZ;
serial4.ops = &am33xx_gdb_ops;
serial4.config = config;
/* enable RX interrupt */
UART_IER_REG(uart4.base) = 0x00;
/* install ISR */
rt_hw_interrupt_install(uart4.irq, am33xx_uart_isr, &serial4, "uart4");
rt_hw_interrupt_control(uart4.irq, 0, 0);
rt_hw_interrupt_mask(uart4.irq);
/* register UART4 device */
rt_hw_serial_register(&serial4, "uart4",
RT_DEVICE_FLAG_RDWR | RT_DEVICE_FLAG_STREAM,
&uart4);
#endif
#ifdef RT_USING_UART5
config.baud_rate = BAUD_RATE_115200;
config.bit_order = BIT_ORDER_LSB;
config.data_bits = DATA_BITS_8;
config.parity = PARITY_NONE;
config.stop_bits = STOP_BITS_1;
config.invert = NRZ_NORMAL;
config.bufsz = RT_SERIAL_RB_BUFSZ;
serial5.ops = &am33xx_uart_ops;
serial5.config = config;
/* enable RX interrupt */
UART_IER_REG(uart5.base) = 0x01;
/* install ISR */
rt_hw_interrupt_install(uart5.irq, am33xx_uart_isr, &serial5, "uart5");
rt_hw_interrupt_control(uart5.irq, 0, 0);
rt_hw_interrupt_mask(uart5.irq);
/* register UART4 device */
rt_hw_serial_register(&serial5, "uart5",
RT_DEVICE_FLAG_RDWR | RT_DEVICE_FLAG_INT_RX,
&uart5);
#endif
return 0;
}
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;
}
