【开源】基于STM32的无线姿态检测仪设计

项目说明

使用STM32为主要控制器，通过读取姿态传感器的原始数据，解算出姿态数据，并通过无线通信方式传给其他设备。

功能如下：

1、使用 mpu6050 获得传感器原始数据。

2、可通过卡尔曼滤波、一阶滤波、Mahony解算3种算法解算出姿态数据。

3、使用 OLED 屏幕显示解算数据。

4、使用 ESP8266 连接 WIFI 通过 MQTT 实时上报检测数据。

5、可使用 MQTT 软件检测上报数据。

项目开源链接

本项目资料完全开源。资料包获取方式：

github ： https://github.com/snqx-lqh/ProjectReleasePage

gitee(国内镜像) ：https://gitee.com/snqx-lqh/ProjectOpenSourceReleasePage

项目属于 32 的编号 B006 ，在发布页中，找到对应项目获取方式。

硬件设计

硬件设计如图所示。

实际接线如下：

MPU6050:

SCL->PB6
SDA->PB7
INT->PB5
VCC->3V3
GND->GND

OLED:

SCK->PB8
SDA->PB9
VCC->3V3
GND->GND

ESP8266:

3V3->3V3
GND->GND
TX->PB11
RX->PB10

USB转TTL：

TX->PA10
RX->PA9
GND->GND

软件设计

软件设计包含驱动设计和具体功能设计。

驱动设计

MPU6050驱动

关于 MPU6050 ，初始化的时候，配置10ms数据采样，然后把中断信号打开。这样mpu6050的INT引脚就会每隔10ms产生一次引脚下降沿，我们可以用这个来判断陀螺仪数据是否准备好了。

c 复制代码

mpu6050_set_rate(100);				//10ms 1000/100 = 10MS       	 	
mpu6050_write_one_byte(MPU6050_ADDR,MPU_INT_EN_REG,0X01);    //打开中断信号
mpu6050_write_one_byte(MPU6050_ADDR,MPU_USER_CTRL_REG,0X00); 
mpu6050_write_one_byte(MPU6050_ADDR,MPU_FIFO_EN_REG,0X00);	  
mpu6050_write_one_byte(MPU6050_ADDR,MPU_INTBP_CFG_REG,0X9C); //中断信号配置
mpu6050_read_one_byte (MPU6050_ADDR,MPU_DEVICE_ID_REG,&res);

一般的数据读取都是直接读取原始值，但是我在读取原始值的基础上加了个陀螺仪初始化校准。校准函数在最开始mpu6050_init函数中调用，获得初始时刻的偏移。

c 复制代码

void mpu_calibration()
{
	int32_t sum_gx = 0, sum_gy = 0, sum_gz = 0;
    int16_t gx, gy, gz;
    for (int i = 0; i < 100; i++)
    {
        mpu6050_get_gyro(&gx, &gy, &gz);
        sum_gx += gx;
        sum_gy += gy;
        sum_gz += gz;
        mpu6050_delay_ms(10); // 稍微延时，确保数据稳定
    }
    gyro_offset[0] = sum_gx / 100;
    gyro_offset[1] = sum_gy / 100;
    gyro_offset[2] = sum_gz / 100;
}

然后在后续读取的时候就减去这一部分偏移。然后剩下的代码就和正点原子教程代码差不多了。

c 复制代码

uint8_t mpu6050_get_gyro(int16_t *gx,int16_t *gy,int16_t *gz)
{
    uint8_t buf[6],res;
	if(gx == NULL || gy == NULL || gz == NULL)
		return 2;
    res=mpu6050_read_bytes(MPU6050_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
    if(res==0)
    {
        *gx=(((uint16_t)buf[0]<<8)|buf[1]);
        *gy=(((uint16_t)buf[2]<<8)|buf[3]);
        *gz=(((uint16_t)buf[4]<<8)|buf[5]);
		
		*gx -= gyro_offset[0];
		*gy -= gyro_offset[1];
		*gz -= gyro_offset[2];
    }
    return res;
}

ESP01S驱动

ESP01S使用的固件是 （1471）ESP8266-AT-1M.bin。这个在我的开源文件中包含。

关于此设备驱动，最重要的是怎么处理它返回值的不定长以及不是及时响应，一个命令可能会隔段时间才会回应，还不连续。所以，我们使用一个环形缓冲区把接收到的内容先暂存，然后再处理。

环形缓冲区这里使用了 RT-Thread 中的环形缓冲区思想，但是只保留了一些常用函数。环形缓冲区可以当作被两个索引管理的数组。我们只谈使用。在串口接收中断中，我们将接收到的数值放到环形缓冲区中，在主任务中进行提取处理。

主要的接收处理如下：

c 复制代码

void USART3_IRQHandler(void)
{
	if (USART_GetITStatus(USART3, USART_IT_RXNE) != RESET)
	{
		uint8_t data = USART_ReceiveData(USART3); 
        ringbuffer_putchar(&esp01s_device.rx_rb, data); // 将数据放入环形缓冲区  esp01s_device.rx_rb 中
		USART_ClearITPendingBit(USART3, USART_IT_RXNE);
	}
}

esp01s_device 是一个管理 ESP01S 变量的结构体变量，rx_rb 就是里面定义的环形缓冲结构体。

然后在主任务中，我的发送指令函数如下：

c 复制代码

int esp_at_cmd(struct esp01s *esp, const char *cmd, const char *expect, uint16_t timeout_ms)
{
    // 清空缓存（很重要）
    int ret = 0;
    ringbuffer_reset(&esp->rx_rb);
    esp_send(cmd);
    esp_send("\r\n");
    ret = esp_wait_response(esp, expect, timeout_ms);
    esp->resp_len = 0;
    return ret;
}

int esp_wait_response(struct esp01s *dev, const char *expect, uint32_t timeout)
{  
    uint16_t delay_ms_counter = 0; 
	dev->resp_len = 0;
    while (delay_ms_counter < timeout)
    {
        uint8_t ch;
        if (ringbuffer_getchar(&dev->rx_rb, &ch) == 1)
        {
            // 保存数据
            if (dev->resp_len < ESP_RESP_BUF_SIZE - 1)
            {
                dev->resp_buf[dev->resp_len++] = ch;
                dev->resp_buf[dev->resp_len] = '\0';
            }
            // 判断成功
            if (strstr((char *)dev->resp_buf, expect))
            {
                return 0;
            }
            // 判断错误
            if (strstr((char *)dev->resp_buf, "ERROR"))
            {
                return -1;
            }
        } else {
            // 没有数据，等待一段时间
            delay_ms_counter++;
            esp_delay_ms(1);
        } 
    }
    return -2; // 超时
}

发送数据完成后，就等待响应，响应就是把串口中断中环形缓冲区得到的值，一个个取出来存到数组中，然后再进行判断，比如判断是否接收到响应字符，是否接收到错误字符。

OLED驱动

OLED 就是使用的中景园电子的，只不过使用的我自己的 IIC 函数。

应用设计

应用设计首先是初始化。然后是while任务轮询，以及中断数据获取。

初始化

在初始化中，值得注意的就是 ESP01S 的初始化，其他驱动初始化比较一般。

我们的 ESP 01s 使用了 MQTT ，所以我们需要连接一些 MQTT 的配置。

首先是 ESP 复位，没什么好说的，只是需要切换到station模式，也就是使用 "AT+CWMODE=1" 步骤如下：

c 复制代码

ret = esp_at_cmd(&esp01s_device, "AT+RST", "OK", 2000);
printf("AT+RESET resp: %s\n", esp01s_device.resp_buf);
if(ret != 0) while(1); 
delay_ms(500);

ret = esp_at_cmd(&esp01s_device, "AT", "OK", 2000);
printf("AT resp: %s\n", esp01s_device.resp_buf); 
if(ret != 0) while(1);

ret = esp_at_cmd(&esp01s_device, "AT+CWMODE=1", "OK", 2000);
printf("AT+CWMODE resp: %s\n", esp01s_device.resp_buf); 
if(ret != 0) while(1);

然后是连接 WIFI。这是我的 WIFI 名和密码。你需要替换成自己的。

c 复制代码

ret = esp_at_cmd(&esp01s_device, "AT+CWJAP=\"CMCC-XJmL\",\"sR62HiPv\"", "OK", 5000);
printf("AT+CWJAP resp: %s\n", esp01s_device.resp_buf); 
if(ret != 0) while(1);

然后是 MQTT 的处理，下面是常用指令：

C 复制代码

AT+MQTTUSERCFG=0,1,"用户ID","账号","密码",0,0,""
# 设置MQTT连接所需要的的参数，包括用户ID（不为空）、
# 账号（admin）以及密码（public）
AT+MQTTCONN=0,"broker.emqx.io",1883,0
AT+MQTTPUB=0,"ESP8266/online","1",0,0
#发布一条topic为"ESP8266/online"，message为"1"的数据， #QOS设置为0
AT+MQTTSUB=0,"ESP8266/EMQX",0
#订阅一条topic为"ESP8266/EMQX"，QOS为0的数据

这里需要注意，需要先配置用户 ID ，不要使用 test 这种 ID 最好，因为我使用的gon公共测试服务器 broker.emqx.io，用 test 这个名字很可能连不上。

c 复制代码

ret = esp_at_cmd(&esp01s_device, "AT+MQTTUSERCFG=0,1,\"user\",\"user\",\"123\",0,0,\"\"", "OK", 2000);
printf("AT+MQTTUSERCFG resp: %s\n", esp01s_device.resp_buf); 
if(ret != 0) while(1);

ret = esp_at_cmd(&esp01s_device, "AT+MQTTCONN=0,\"broker.emqx.io\",1883,0", "OK", 5000);
printf("AT+MQTTCONN resp: %s\n", esp01s_device.resp_buf); 
if(ret != 0) while(1);

中断数据获取

我在中断中读取了陀螺仪的原始参数，但是，我们最好不要在中断中计算，中断的原则应该是快进快出，我们在中断中获取到原始数据后，将其放进一个循环缓冲中，以待在轮询的时候把数据取出来计算，这样也不会丢数据。

由于中断是10ms一次，我在中断中还做了两个标志，一个是界面刷新，count % 10 == 0也就是100ms触发一次，mqtt发送数据标志，1S触发一次。

c 复制代码

void EXTI9_5_IRQHandler(void)
{
	if(EXTI_GetITStatus(EXTI_Line5)!= RESET)
	{ 
		//获得6050原始数据
		count++; 
		if(count % 100 == 0){
		    pushMqttFlag = 1;				
		}
		if(count % 10 == 0){
		    showUiFlag = 1;				
		}
		mpu_sample_t sample;
        mpu6050_get_gyro(&sample.gyro[0], &sample.gyro[1], &sample.gyro[2]);
        mpu6050_get_acc(&sample.acc[0], &sample.acc[1], &sample.acc[2]);
        mpu_sample_rb_put(&mpu_rb, &sample);
		
		EXTI_ClearITPendingBit(EXTI_Line5);
	}
}

while 循环

在任务循环中，首先查看数据缓冲区中是否有数据，有的话就把数据提出来做计算。

c 复制代码

// 一次性处理所有积压的MPU数据
mpu_sample_t sample;
while (mpu_sample_rb_get(&mpu_rb, &sample) == 0)
{ 
	//cal_with_kalman(sample);
	//cal_with_folpf(sample);
	cal_with_ahrs(sample); 
}

然后是 1S 的时候进行MQTT上报。这里发送的是JSON字符串，把想要发送的数据整合到一条发送，方便处理。

为了通过 ESP01S 的 AT+MQTTPUB 命令发送 JSON 数据，需要处理 C 语言转义 → AT 指令转义 两层转换。

比如我们现在有一个目标发送格式

json 复制代码

{"roll":"12.34","pitch":"56.78","yaw":"90.12"}

JSON 整体作为 AT+MQTTPUB 的第三个参数，参数内双引号和逗号必须用 \ 转义，写成\"和\,：

c 复制代码

AT+MQTTPUB=0,"/topic","{\"roll\":\"12.34\"\,\"pitch\":\"56.78\"\,\"yaw\":\"90.12\"}",0,0

然后在 C 语言中，我们要把上面这条指令写成字符串，那么这个字符串中的双引号和\需要用 \ 转义，写成\"和\\，为什么C语言中不用再把,转义一遍，因为那只是AT+MQTTPUB中的转义要求，不是C的转义要求，C中我们的转义只要能获得上面的字符串就可以。就需要如此写入：

c 复制代码

sprintf(mqtt_put_message,
"AT+MQTTPUB=0,\"/topic\",\"{\\\"roll\\\":\\\"12.34\\\"\\,\\\"pitch\\\":\\\"56.78\\\"\\,\\\"yaw\\\":\\\"90.12\\\"}\",0,0"）

为了方便看，我做了换行，换行可以在换行头尾加上"表示续接字符串。也就是下面这种状态了。

c 复制代码

if(pushMqttFlag) // 1S 上报一次 MQTT 状态
{ 
	pushMqttFlag = 0;
	// 上报状态信息
	sprintf(mqtt_put_message,
		"AT+MQTTPUB=0,\"/user/mqtttest/angles\","
		"\"{\\\"roll\\\":\\\"%.2f\\\""
		"\\,\\\"pitch\\\":\\\"%.2f\\\"" 
		"\\,\\\"yaw\\\":\\\"%.2f\\\"}\","
		"0,0",
		mpu6050_data.angleRoll, mpu6050_data.anglePitch, mpu6050_data.angleYaw); 
	esp_at_cmd(&esp01s_device, mqtt_put_message , "OK", 2000);  
	printf("AT+MQTTSUB resp: %s\n", esp01s_device.resp_buf); 
}

所以上面这条信息，真的发送出去的时候就是如下(%.2f是实际值，这里我只是占位置)，然后你把这个发送到ESP01S中后，他会再转义一遍有\的部分

c 复制代码

AT+MQTTPUB=0,"/user/mqtttest/angles","{\"roll\":\"%.2f\"\,\"pitch\":\"%.2f\"\,\"yaw\":\"%.2f\"}",0,0

最后发送到服务器就是：

c 复制代码

{"roll":"%.2f","pitch":"%.2f","yaw":"%.2f"}

然后是100ms UI 刷新。

c 复制代码

if(showUiFlag) // 100MS 更新一次界面
{ 
	showUiFlag = 0;
	// 更新一次界面显示
	OLED_Clear_Buffer(); //清除之前的缓存
	sprintf((char*)oled_show_str,"MPU6050");
	OLED_ShowString(24,0,oled_show_str,16,1);
	sprintf((char*)oled_show_str,"Roll: %.2f",mpu6050_data.angleRoll);
	OLED_ShowString(0,16,oled_show_str,16,1);
	sprintf((char*)oled_show_str,"Pitch: %.2f",mpu6050_data.anglePitch);
	OLED_ShowString(0,32,oled_show_str,16,1);
	sprintf((char*)oled_show_str,"Yaw: %.2f",mpu6050_data.angleYaw);
	OLED_ShowString(0,48,oled_show_str,16,1);
	OLED_Refresh(); //更新
}

mqttfx 连接测试

我们上报的信息，想要快速查看是否真的上报成功，可以使用 mqttfx 工具进行查看。工具在我的开源文件中包含。

1、点击设置，准备创建一个连接。

2、点击此处新建一个连接。

我把这个连接创建为 NewConnect 并且配置访问服务器。

主页面点击连接即可。

然后我们订阅一个主题，主题名自己输入，需要和代码中发布的主题一致。

便可以看到发布的消息了。