基于tc2646轴一阶互补滤波代码

以下是基于TC2646轴的一阶互补滤波代码示例：

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "tft_display.h"
#include "tc264_io.h"
#include "tc264_spi.h"
#include "tc264_uart.h"

#define PI 3.14159265358979323846

float gyro_x, gyro_y, gyro_z;
float accel_x, accel_y, accel_z;
float angle_x, angle_y, angle_z;
float alpha = 0.98; // 互补滤波系数

void read_gyro_accel(float* gyro, float* accel)
{
    // 读取陀螺仪数据
    gyro[0] = (float)TC264_SPI_Read(0x43) * 0.01;
    gyro[1] = (float)TC264_SPI_Read(0x45) * 0.01;
    gyro[2] = (float)TC264_SPI_Read(0x47) * 0.01;

    // 读取加速度计数据
    accel[0] = (float)TC264_SPI_Read(0x3B) * 0.01;
    accel[1] = (float)TC264_SPI_Read(0x3D) * 0.01;
    accel[2] = (float)TC264_SPI_Read(0x3F) * 0.01;
}

void update_angle(float* gyro, float* accel, float dt)
{
    // 计算加速度计测量角度
    float accel_pitch = atan2(accel[1], sqrt(accel[0] * accel[0] + accel[2] * accel[2])) * 180 / PI;
    float accel_roll = atan2(-accel[0], sqrt(accel[1] * accel[1] + accel[2] * accel[2])) * 180 / PI;

    // 计算互补滤波后的角度
    angle_x = alpha * (angle_x + gyro[0] * dt) + (1 - alpha) * accel_pitch;
    angle_y = alpha * (angle_y + gyro[1] * dt) + (1 - alpha) * accel_roll;
    angle_z = angle_z + gyro[2] * dt;
}

int main()
{
    // 初始化SPI、UART等外设
    TC264_SPI_Init();
    TC264_UART_Init();
    TFT_Display_Init();

    // 初始化角度为0
    angle_x = 0;
    angle_y = 0;
    angle_z = 0;

    while (1) {
        // 读取陀螺仪和加速度计数据
        read_gyro_accel(&gyro_x, &accel_x);
        read_gyro_accel(&gyro_y, &accel_y);
        read_gyro_accel(&gyro_z, &accel_z);

        // 计算时间间隔
        float dt = 0.01; // 假设采样率为100Hz

        // 更新角度
        update_angle(&gyro_x, &accel_x, dt);
        update_angle(&gyro_y, &accel_y, dt);
        update_angle(&gyro_z, &accel_z, dt);

        // 在TFT屏幕上显示角度
        TFT_Display_Clear();
        TFT_Display_Text("Angle X: ", 10, 10);
        TFT_Display_Float(angle_x, 10, 30);
        TFT_Display_Text("Angle Y: ", 10, 50);
        TFT_Display_Float(angle_y, 10, 70);
        TFT_Display_Text("Angle Z: ", 10, 90);
        TFT_Display_Float(angle_z, 10, 110);

        // 将角度发送到串口
        char buf[50];
        sprintf(buf, "X:%.2f Y:%.2f Z:%.2f\r\n", angle_x, angle_y, angle_z);
        TC264_UART_SendString(buf);

        // 等待一段时间后继续循环
        TC264_DelayMs(10);
    }

    return 0;
}

该代码实现了一个基于TC2646轴的一阶互补滤波器，可以通过陀螺仪和加速度计读取姿态角，并通过互补滤波算法得到更加平稳的姿态角度。该代码还使用了TFT显示屏和串口通信功能，可以将角度数据实时显示在屏幕上并通过串口发送给其他设备。