M471_Six-axis_Sensor_MPU6050.zip
(4.29 MB)
ANO_Tech匿名四轴上位机_V2.6.zip
(1.98 MB)
/**************************************************************************//**
* [url=home.php?mod=space&uid=288409]@file[/url] main.c
* [url=home.php?mod=space&uid=247401]@brief[/url] uses M471 GPIO to simulate I2C to initialize
* 6-axis sensor, reads 6-axis sensor data through
* I2C，and sends data to PC through UART.
*
* @note
* Copyright (C) 2021 Nuvoton Technology Corp. All rights reserved.
*****************************************************************************/
#include "SmartM_M4.h"
/*-------------------------------------------------------*/
/* Global variables */
/*-------------------------------------------------------*/
/*-------------------------------------------------------*/
/* Function area */
/*-------------------------------------------------------*/
/**
* @brief Send data by serial port 0
* @param[in] fun is control comand. 0XA0~0XAF
* @param[in] data is databuffer ,most 28 byte
* @param[in] len is Number of valid data
* [url=home.php?mod=space&uid=266161]@return[/url] None
*
* [url=home.php?mod=space&uid=1543424]@Details[/url] Transfer data to four-axis host computer software(V2.6)
*/
void Uart0NimingReport(uint8_t fun, uint8_t *data, uint8_t len)
{
uint8_t send_buf[32];
uint8_t i;
if (len > 28)return; //most of 28 byte
send_buf[len + 3] = 0; //Check number set 0
send_buf[0] = 0X88; //frame head
send_buf[1] = fun; //command
send_buf[2] = len; //data length
for (i = 0; i < len; i++)send_buf[3 + i] = data[i]; //copy data
for (i = 0; i < len + 3; i++)send_buf[len + 3] += send_buf[i]; //Checksum
for (i = 0; i < len + 4; i++)UART_Write(UART0, &send_buf[i], 1);//Send data to serial port 0
}
/**
* @brief Send data from MPU6050 sensor
* @param[in] aacx is Acceleration values in x directions
* @param[in] aacy is Acceleration values in y directions
* @param[in] aacz is Acceleration values in z directions
* @param[in] gyrox is Gyroscope values in x directions
* @param[in] gyroy is Gyroscope values in y directions
* @param[in] gyroz is Gyroscope values in z directions
* @return None
*
* @details Sending Acceleration Sensor Data and Gyroscope Data
*/
void MPU6050SendData(int16_t aacx, int16_t aacy, int16_t aacz, int16_t gyrox, int16_t gyroy, int16_t gyroz)
{
uint8_t tbuf[12];
tbuf[0] = (aacx >> 8) & 0XFF;
tbuf[1] = aacx & 0XFF;
tbuf[2] = (aacy >> 8) & 0XFF;
tbuf[3] = aacy & 0XFF;
tbuf[4] = (aacz >> 8) & 0XFF;
tbuf[5] = aacz & 0XFF;
tbuf[6] = (gyrox >> 8) & 0XFF;
tbuf[7] = gyrox & 0XFF;
tbuf[8] = (gyroy >> 8) & 0XFF;
tbuf[9] = gyroy & 0XFF;
tbuf[10] = (gyroz >> 8) & 0XFF;
tbuf[11] = gyroz & 0XFF;
Uart0NimingReport(0XA1, tbuf, 12); //customized frame ,0XA1
}
/**
* @brief Send processed data
* @param[in] aacx is Acceleration values in x directions.
* @param[in] aacy is Acceleration values in y directions.
* @param[in] aacz is Acceleration values in z directions.
* @param[in] gyrox is Gyroscope values in x directions.
* @param[in] gyroy is Gyroscope values in y directions.
* @param[in] gyroz is Gyroscope values in z directions.
* @param[in] roll is Roll angle.
* Unit 0.01 degrees.-18000 -> 18000 equal -180.00 degrees -> 180.00 degrees.
* @param[in] pitch is Elevation angle.
* Unit 0.01 degrees.-9000 - 9000 equal -90.00 -> 90.00 degrees.
* @param[in] yaw is Heading angle.
* Unit 0.1 degrees 0 -> 3600 equal 0 -> 360.0 degrees.
* @return None
*
* @details Reporting the calculated attitude data to the computer through serial port 0
*/
void Uart0ReportImu(int16_t aacx, int16_t aacy, int16_t aacz, int16_t gyrox, int16_t gyroy, int16_t gyroz, int16_t roll, int16_t pitch, short yaw)
{
uint8_t tbuf[28];
uint8_t i;
for (i = 0; i < 28; i++)tbuf[i] = 0; //set 0
tbuf[0] = (aacx >> 8) & 0XFF;
tbuf[1] = aacx & 0XFF;
tbuf[2] = (aacy >> 8) & 0XFF;
tbuf[3] = aacy & 0XFF;
tbuf[4] = (aacz >> 8) & 0XFF;
tbuf[5] = aacz & 0XFF;
tbuf[6] = (gyrox >> 8) & 0XFF;
tbuf[7] = gyrox & 0XFF;
tbuf[8] = (gyroy >> 8) & 0XFF;
tbuf[9] = gyroy & 0XFF;
tbuf[10] = (gyroz >> 8) & 0XFF;
tbuf[11] = gyroz & 0XFF;
tbuf[18] = (roll >> 8) & 0XFF;
tbuf[19] = roll & 0XFF;
tbuf[20] = (pitch >> 8) & 0XFF;
tbuf[21] = pitch & 0XFF;
tbuf[22] = (yaw >> 8) & 0XFF;
tbuf[23] = yaw & 0XFF;
Uart0NimingReport(0XAF, tbuf, 28); //Flight Control Display Frame ,0XAF
}
INT32 main(VOID)
{
float pitch, roll, yaw; //Euler angle
int16_t aacx, aacy, aacz; //Acceleration sensor raw data
int16_t gyrox, gyroy, gyroz; //Raw data of gyroscope
int16_t temp; //temperature
PROTECT_REG
(
SYS_Init(PLL_CLOCK);
UART0_Init(115200);
)
IIC_Init();
MPU_Init(); //Initialization MPU6050
while (mpu_dmp_init())
{
printf("MPU6050 ERROR \r\n");
Delayms(500);
}
while (1)
{
if (mpu_dmp_get_data(&pitch, &roll, &yaw) == 0)
{
temp = MPU_Get_Temperature(); //Get the temperature value
if (temp > 100)
printf("Temperature = %d\r\n", temp);
MPU_Get_Accelerometer(&aacx, &aacy, &aacz); //Acquisition of Acceleration Sensor Data
MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz); //Acquisition of gyroscope data
MPU6050SendData(aacx, aacy, aacz, gyrox, gyroy, gyroz); //Sending Acceleration and Gyroscope Raw Data with Custom Frame
Uart0ReportImu(aacx, aacy, aacz, gyrox, gyroy, gyroz, (int)(roll * 100), (int)(pitch * 100), (int)(yaw * 10));
}
}
}
M471 使用I2C来传输控制命令至MPU6050 传感器，使用UART0 来上报数据到PC 端 。
首先初始化 MPU6050 并检查传感器 ID 是否正确
uint8_t MPU_Init(void)
{
uint8_t res;
IIC_Init();//Init IIC bus
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //RESET MPU6050
Delayms(100);
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //wake up MPU6050
MPU_Set_Gyro_Fsr(3); //gyroscope ,±2000dps
MPU_Set_Accel_Fsr(0); //Accelerometer,±2g
MPU_Set_Rate(50); //Setting Sampling Rate ,50Hz
MPU_Write_Byte(MPU_INT_EN_REG,0X00); //Close all interrupts
MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C Master Mode close
MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //close FIFO
MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //INT low level Active
res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
printf("RES=%x\r\n",res);
if(res==MPU_ADDR)//Device ID is correct
{
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01);
MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00);
MPU_Set_Rate(50); //Setting Sampling Rate 50Hz
}else return 1;
return 0;
}
MCU 获取 MPU6050 的数据程序如下：
u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
{
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors;
unsigned char more;
long quat[4];
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;
/* 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; //yaw
}else return 2;
return 0;
}
哪位大佬给试试，如何用新唐的BSP标准库的硬件I2C1操作这个陀螺仪。
|
|
楼主
标题置顶
标题高亮
