写一段基于stc32芯片上将mpu6050原始数据转换成角度的C语言代码

以下是一个基于STC32芯片上将MPU6050原始数据转换成角度的C语言代码示例：

#include <STC32.h>
#include <stdio.h>
#include <math.h>

#define MPU_ADDR 0x68
#define ACCEL_SCALE 16384.0
#define GYRO_SCALE 131.0

float accel_x, accel_y, accel_z;
float gyro_x, gyro_y, gyro_z;

void IIC_start(void)
{
    IIC_SDA = 1;
    IIC_SCL = 1;
    delay_us(2);
    IIC_SDA = 0;
    delay_us(2);
    IIC_SCL = 0;
}

void IIC_stop(void)
{
    IIC_SDA = 0;
    IIC_SCL = 1;
    delay_us(2);
    IIC_SDA = 1;
    delay_us(2);
    IIC_SCL = 0;
}

void IIC_send_ack(void)
{
    IIC_SDA = 0;
    IIC_SCL = 1;
    delay_us(2);
    IIC_SCL = 0;
    IIC_SDA = 1;
}

void IIC_send_nack(void)
{
    IIC_SDA = 1;
    IIC_SCL = 1;
    delay_us(2);
    IIC_SCL = 0;
}

unsigned char IIC_receive_byte(void)
{
    unsigned char i, value = 0;
    for (i = 0; i < 8; i++) {
        value <<= 1;
        IIC_SCL = 1;
        delay_us(2);
        if (IIC_SDA) {
            value |= 0x01;
        }
        IIC_SCL = 0;
        delay_us(2);
    }
    return value;
}

void IIC_send_byte(unsigned char dat)
{
    unsigned char i;
    for (i = 0; i < 8; i++) {
        if (dat & 0x80) {
            IIC_SDA = 1;
        } else {
            IIC_SDA = 0;
        }
        IIC_SCL = 1;
        delay_us(2);
        IIC_SCL = 0;
        dat <<= 1;
        delay_us(2);
    }
}

void IIC_write_reg(unsigned char reg, unsigned char dat)
{
    IIC_start();
    IIC_send_byte(MPU_ADDR << 1);
    IIC_send_byte(reg);
    IIC_send_byte(dat);
    IIC_stop();
}

unsigned char IIC_read_reg(unsigned char reg)
{
    unsigned char dat;
    IIC_start();
    IIC_send_byte(MPU_ADDR << 1);
    IIC_send_byte(reg);
    IIC_start();
    IIC_send_byte((MPU_ADDR << 1) | 1);
    dat = IIC_receive_byte();
    IIC_send_ack();
    IIC_stop();
    return dat;
}

void IIC_read_regs(unsigned char reg, unsigned char len, unsigned char *buf)
{
    unsigned char i;
    IIC_start();
    IIC_send_byte(MPU_ADDR << 1);
    IIC_send_byte(reg);
    IIC_start();
    IIC_send_byte((MPU_ADDR << 1) | 1);
    for (i = 0; i < len - 1; i++) {
        buf[i] = IIC_receive_byte();
        IIC_send_ack();
    }
    buf[len - 1] = IIC_receive_byte();
    IIC_send_nack();
    IIC_stop();
}

void MPU6050_init(void)
{
    IIC_write_reg(0x6B, 0x00); // PWR_MGMT_1 = 0 to wake up MPU6050
    IIC_write_reg(0x1C, 0x08); // ACCEL_CONFIG = 0x08 to set full-scale range to ±4g
    IIC_write_reg(0x1B, 0x08); // GYRO_CONFIG = 0x08 to set full-scale range to ±500°/s
}

void MPU6050_read_accel(void)
{
    unsigned char buf[6];
    IIC_read_regs(0x3B, 6, buf);
    accel_x = ((short)(buf[0] << 8 | buf[1])) / ACCEL_SCALE;
    accel_y = ((short)(buf[2] << 8 | buf[3])) / ACCEL_SCALE;
    accel_z = ((short)(buf[4] << 8 | buf[5])) / ACCEL_SCALE;
}

void MPU6050_read_gyro(void)
{
    unsigned char buf[6];
    IIC_read_regs(0x43, 6, buf);
    gyro_x = ((short)(buf[0] << 8 | buf[1])) / GYRO_SCALE;
    gyro_y = ((short)(buf[2] << 8 | buf[3])) / GYRO_SCALE;
    gyro_z = ((short)(buf[4] << 8 | buf[5])) / GYRO_SCALE;
}

float MPU6050_get_roll(void)
{
    MPU6050_read_accel();
    float roll = atan2(accel_y, accel_z) * 180.0 / M_PI;
    return roll;
}

float MPU6050_get_pitch(void)
{
    MPU6050_read_accel();
    float pitch = atan2(-accel_x, sqrt(accel_y * accel_y + accel_z * accel_z)) * 180.0 / M_PI;
    return pitch;
}

float MPU6050_get_yaw(void)
{
    MPU6050_read_gyro();
    static float yaw = 0.0;
    yaw += gyro_z * 0.001;
    return yaw;
}

void main()
{
    IIC_SDA = 1;
    IIC_SCL = 1;
    MPU6050_init();
    while (1) {
        float roll = MPU6050_get_roll();
        float pitch = MPU6050_get_pitch();
        float yaw = MPU6050_get_yaw();
        printf("Roll:%.2f Pitch:%.2f Yaw:%.2f\n", roll, pitch, yaw);
        delay_ms(10);
    }
}