ESP32函数返回错误

Question

在ESP32学习中发现ESP32函数内部打印的数据和返回出去的数据完全不一样，在函数内部打印如下：

float get_angle()
{
    byte head[2];
    byte raw_data[9];
    if (Serial2.available())
    {
        Serial2.readBytes(head,2);
        if(head[0]==0x55&&head[1]==0x53)
        {
            Serial2.readBytes(raw_data,9);
        }
        uint16_t x_raw = ((uint16_t)raw_data[1]<<8)|(uint16_t)raw_data[0];
        uint16_t y_raw = ((uint16_t)raw_data[3]<<8)|(uint16_t)raw_data[2];
        uint16_t z_raw = ((uint16_t)raw_data[5]<<8)|(uint16_t)raw_data[4];
        float x = (float)x_raw/32768.0*180.0;
        float y = (float)y_raw/32768.0*180.0;
        float z = (float)z_raw/32768.0*180.0;
        //Serial.write((uint8_t)z);
       if(x>180.0)
        {
            x -= 360.0;
        }
        if(y>180.0)
        {
            y -= 360.0;
        }
        if(z>180.0)
        {
            z -= 360.0;
        }
        Serial.println(z);
        return z;
    }

打印的是返回值z，此时打印正常。

然后将其return，在主函数里面读取返回值：

float z1;
float pid_u;
float target_angle_z=0;
//extern int BASE_SPEED;
uint8_t kkk=0;
float sta=0;

void setup() {
  //BASE_SPEED=0;
  usart2_init();
  //delay(3000);
  z_zero();
  //z1=get_angle();
  //init_pid(0.07,0.001,0.1);
  //setupMotors();
  //setMotorSpeed(BASE_SPEED);  // 初始速度设置*/
}

void loop() {
  z1=0;
  z1=get_angle();
  Serial.println(z1);
  //pid_u=compute_pid(0,get_actvalue(target_angle_z,z1));
  //if(4*pid_u>100)
  //{
    //pid_u=25;
  //}
  //if(4*pid_u<100)
  //{
   /// pid_u=-25;
  //}
  //setLEFTspeed(BASE_SPEED+(int)(2*sta*pid_u));
  //setRIGHTspeed(BASE_SPEED-(int)(2*sta*pid_u));
  //kkk=Serial.read();
  //controlcar();
}

此时会打印一个固定值，无论如何转动陀螺仪都不会改变：

如果让两者同时输出，就会在一大堆错误的值里面打印一点正确的值，整体如下：

代码如下：
main.cpp:

#include <Arduino.h>
//#include "Motoriese_aandrywing.h"
#include "usart2.h"
//#include "pid.h"
//#include "usart.h"

float z1;
float pid_u;
float target_angle_z=0;
//extern int BASE_SPEED;
uint8_t kkk=0;
float sta=0;

void setup() {
  //BASE_SPEED=0;
  usart2_init();
  //delay(3000);
  z_zero();
  //z1=get_angle();
  //init_pid(0.07,0.001,0.1);
  //setupMotors();
  //setMotorSpeed(BASE_SPEED);  // 初始速度设置*/
}

void loop() {
  z1=0;
  z1=get_angle();
  Serial.println(z1);
  //pid_u=compute_pid(0,get_actvalue(target_angle_z,z1));
  //if(4*pid_u>100)
  //{
    //pid_u=25;
  //}
  //if(4*pid_u<100)
  //{
   /// pid_u=-25;
  //}
  //setLEFTspeed(BASE_SPEED+(int)(2*sta*pid_u));
  //setRIGHTspeed(BASE_SPEED-(int)(2*sta*pid_u));
  //kkk=Serial.read();
  //controlcar();
}

usart2.h:

#include <Arduino.h>

void usart2_init();
float get_angle();
void z_zero();

usart.cpp:

#include "usart2.h"

void usart2_init()
{
    Serial2.begin(9600);
    Serial.begin(9600);
}

float get_angle()
{
    byte head[2];
    byte raw_data[9];
    float z=0;
    if (Serial2.available())
    {
        Serial2.readBytes(head,2);
        if(head[0]==0x55&&head[1]==0x53)
        {
            Serial2.readBytes(raw_data,9);
        }
        uint16_t x_raw = ((uint16_t)raw_data[1]<<8)|(uint16_t)raw_data[0];
        uint16_t y_raw = ((uint16_t)raw_data[3]<<8)|(uint16_t)raw_data[2];
        uint16_t z_raw = ((uint16_t)raw_data[5]<<8)|(uint16_t)raw_data[4];
        float x = (float)x_raw/32768.0*180.0;
        float y = (float)y_raw/32768.0*180.0;
        z = (float)z_raw/32768.0*180.0;
        //Serial.write((uint8_t)z);
       if(x>180.0)
        {
            x -= 360.0;
        }
        if(y>180.0)
        {
            y -= 360.0;
        }
        if(z>180.0)
        {
            z -= 360.0;
        }
        Serial.print(z);
        Serial.print(" ");
        return z;
    }
    return z;
}

void z_zero()
{
    byte send_data[5];
    send_data[0]=0xff;
    send_data[1]=0xaa;
    send_data[2]=0x69;
    send_data[3]=0x88;
    send_data[4]=0xb5;
    Serial2.write(send_data, sizeof(send_data));
    delay(200);
    send_data[2]=0x01;
    send_data[3]=0x04;
    send_data[4]=0x00;
    Serial2.write(send_data, sizeof(send_data));
    delay(3000);
    send_data[2]=0x00;
    send_data[3]=0x00;
    send_data[4]=0x00;
    Serial2.write(send_data, sizeof(send_data));
    delay(1000);
}

请问一下各位朋友这是如何回事？怎么改进呢？

Answer 1

让【宝码香车】来帮你解答，本回答参考DeepSeek编写，并整理提供。
如果还有疑问可以点击头像关注私信或评论。如果答案让您满意，请采纳、点赞、关注，非常感谢！

ESP32函数返回值异常问题分析

根据您描述的问题，get_angle()函数内部打印的值与返回后在主函数中打印的值不一致，这通常是由于以下几个原因造成的：

问题分析

1. 未正确处理串口数据：当Serial2.available()为false时，函数返回未初始化的z值
2. 数据校验不足：只检查了头部0x55和0x53，但没有验证数据完整性
3. 缓冲区污染：可能在读取数据时发生了缓冲区溢出或数据错位

解决方案

1. 修改get_angle()函数

float get_angle() {
    byte head[2];
    byte raw_data[9];
    float z = 0.0f; // 明确初始化
    
    // 确保有足够数据可读 (2字节头 + 9字节数据)
    if (Serial2.available() >= 11) {
        Serial2.readBytes(head, 2);
        
        // 验证数据头
        if(head[0] == 0x55 && head[1] == 0x53) {
            // 读取完整数据包
            if(Serial2.readBytes(raw_data, 9) == 9) {
                uint16_t x_raw = ((uint16_t)raw_data[1]<<8) | raw_data[0];
                uint16_t y_raw = ((uint16_t)raw_data[3]<<8) | raw_data[2];
                uint16_t z_raw = ((uint16_t)raw_data[5]<<8) | raw_data[4];
                
                // 计算角度
                z = (float)z_raw / 32768.0f * 180.0f;
                
                // 角度归一化到[-180,180]
                if(z > 180.0f) z -= 360.0f;
                if(z < -180.0f) z += 360.0f;
                
                // 调试输出
                Serial.print("Raw Z: ");
                Serial.println(z);
                
                return z;
            }
        } else {
            // 如果不是有效数据头，清空缓冲区
            while(Serial2.available() > 0) Serial2.read();
        }
    }
    
    return z; // 返回默认值
}

2. 修改主循环

void loop() {
    static unsigned long lastPrint = 0;
    if(millis() - lastPrint >= 100) { // 每100ms读取一次
        lastPrint = millis();
        
        float z1 = get_angle();
        Serial.print("Returned Z: ");
        Serial.println(z1);
    }
}

改进建议

1. 添加校验和：检查数据包的校验和以确保数据完整性
2. 错误处理：添加更完善的错误处理机制
3. 缓冲区管理：确保在读取失败时清空串口缓冲区
4. 调试信息：添加更详细的调试输出以帮助诊断问题

完整示例

// usart2.h
#include <Arduino.h>

void usart2_init();
float get_angle();
void z_zero();

// usart2.cpp
#include "usart2.h"

void usart2_init() {
    Serial2.begin(9600);
    Serial.begin(115200); // 使用更高的波特率以获得更好的调试输出
}

float get_angle() {
    byte head[2];
    byte raw_data[9];
    float z = 0.0f;
    
    if (Serial2.available() >= 11) {
        Serial2.readBytes(head, 2);
        
        if(head[0] == 0x55 && head[1] == 0x53) {
            if(Serial2.readBytes(raw_data, 9) == 9) {
                // 计算校验和
                byte checksum = 0x55 + 0x53;
                for(int i = 0; i < 8; i++) checksum += raw_data[i];
                
                if(checksum == raw_data[8]) { // 校验和正确
                    uint16_t z_raw = ((uint16_t)raw_data[5]<<8) | raw_data[4];
                    z = (float)z_raw / 32768.0f * 180.0f;
                    
                    if(z > 180.0f) z -= 360.0f;
                    if(z < -180.0f) z += 360.0f;
                    
                    Serial.print("Debug Z: ");
                    Serial.println(z, 4); // 打印4位小数
                    
                    return z;
                } else {
                    Serial.println("Checksum error");
                }
            }
        } else {
            // 清空无效数据
            while(Serial2.available() > 0) Serial2.read();
        }
    }
    
    return z;
}

这些修改应该能解决您遇到的返回值不一致问题。如果问题仍然存在，可能需要检查硬件连接或传感器本身是否有问题。