stm32f103c8t6螺旋仪GY-521调姿代码oled显示
时间: 2023-07-02 20:06:47 浏览: 72
这里提供一个基于STM32F103C8T6和GY-521螺旋仪的调姿代码,同时使用OLED显示姿态数据。
首先,需要使用I2C通信协议读取GY-521螺旋仪的原始数据。可以使用HAL库提供的函数进行读取。
```c
#include "stm32f1xx_hal.h"
#define MPU_ADDR 0x68
I2C_HandleTypeDef hi2c1;
void MPU6050_Init(void)
{
uint8_t check;
uint8_t Data;
HAL_I2C_Mem_Read(&hi2c1, MPU_ADDR, 0x75, 1, &check, 1, 1000);
if (check == 0x68)
{
Data = 0x00;
HAL_I2C_Mem_Write(&hi2c1, MPU_ADDR, 0x6B, 1, &Data, 1, 1000);
Data = 0x07;
HAL_I2C_Mem_Write(&hi2c1, MPU_ADDR, 0x1B, 1, &Data, 1, 1000);
Data = 0x08;
HAL_I2C_Mem_Write(&hi2c1, MPU_ADDR, 0x1C, 1, &Data, 1, 1000);
}
}
void MPU6050_Read_Accel(int16_t *accData)
{
uint8_t buf[6];
HAL_I2C_Mem_Read(&hi2c1, MPU_ADDR, 0x3B, 1, buf, 6, 1000);
accData[0] = (buf[0] << 8) | buf[1];
accData[1] = (buf[2] << 8) | buf[3];
accData[2] = (buf[4] << 8) | buf[5];
}
void MPU6050_Read_Gyro(int16_t *gyroData)
{
uint8_t buf[6];
HAL_I2C_Mem_Read(&hi2c1, MPU_ADDR, 0x43, 1, buf, 6, 1000);
gyroData[0] = (buf[0] << 8) | buf[1];
gyroData[1] = (buf[2] << 8) | buf[3];
gyroData[2] = (buf[4] << 8) | buf[5];
}
```
接下来,需要对原始数据进行处理,得到姿态数据。这里使用互补滤波算法来获得稳定的姿态数据。同时,需要注意将原始数据转换为度数。
```c
#define PI 3.14159265358979323846f
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float Roll = 0, Pitch = 0, Yaw = 0;
float AccAngleX = 0, AccAngleY = 0;
float GyroAngleX = 0, GyroAngleY = 0, GyroAngleZ = 0;
float CFangleX = 0, CFangleY = 0;
float Complementary_Filter(float AccAngle, float GyroRate, float DT)
{
return (0.98 * (AccAngle + GyroRate * DT) + 0.02 * GyroRate);
}
void Get_Attitude(void)
{
MPU6050_Read_Accel(AccRaw);
MPU6050_Read_Gyro(GyroRaw);
AccX = (float)AccRaw[0] / 16384.0f;
AccY = (float)AccRaw[1] / 16384.0f;
AccZ = (float)AccRaw[2] / 16384.0f;
GyroX = (float)GyroRaw[0] / 131.0f;
GyroY = (float)GyroRaw[1] / 131.0f;
GyroZ = (float)GyroRaw[2] / 131.0f;
AccAngleX = atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI;
AccAngleY = atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI;
GyroAngleX += GyroX * DT;
GyroAngleY += GyroY * DT;
GyroAngleZ += GyroZ * DT;
CFangleX = Complementary_Filter(AccAngleX, GyroX, DT);
CFangleY = Complementary_Filter(AccAngleY, GyroY, DT);
Roll = CFangleX;
Pitch = CFangleY;
Yaw = GyroAngleZ;
}
```
最后,使用OLED显示姿态数据。这里使用了STM32的标准库函数进行OLED的显示操作。
```c
#include "stm32f1xx_hal.h"
#include "ssd1306.h"
#include "fonts.h"
#define OLED_I2C_ADDR 0x78
void OLED_Init(void)
{
ssd1306_Init();
ssd1306_Fill(Black);
ssd1306_SetCursor(0, 0);
ssd1306_WriteString("Roll: ", Font_11x18, White);
ssd1306_SetCursor(0, 25);
ssd1306_WriteString("Pitch: ", Font_11x18, White);
ssd1306_UpdateScreen();
}
void OLED_Display(float Roll, float Pitch)
{
char strRoll[20], strPitch[20];
sprintf(strRoll, "%.2f", Roll);
sprintf(strPitch, "%.2f", Pitch);
ssd1306_SetCursor(60, 0);
ssd1306_WriteString(strRoll, Font_11x18, White);
ssd1306_SetCursor(70, 25);
ssd1306_WriteString(strPitch, Font_11x18, White);
ssd1306_UpdateScreen();
}
```
完整代码如下:
相关推荐
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)