#define ALPHA 0.98f // 根据实际需求调整
float filtered_angle = 0; // 初始化过滤后的角度
void updateFilter(float gyro_rate, float accel_angle, float dt) {
// 计算陀螺仪提供的角度增量
float gyro_angle = gyro_rate * dt;
// 更新滤波后的角度
float new_angle = filtered_angle + gyro_angle;
// 使用加速度计修正角度
filtered_angle = new_angle * ALPHA + accel_angle * (1.0f - ALPHA);
}
int main(void) {
// 假设已经配置好定时器和传感器读取功能
while(1) {
float current_accel_angle = read_accelerometer_angle(); // 获取加速度计角度
float current_gyro_rate = read_gyroscope_rate(); // 获取陀螺仪速率
float delta_time = get_delta_time(); // 获取两次读数间的时间间隔
updateFilter(current_gyro_rate, current_accel_angle, delta_time);
delay_ms(1); // 根据需求调整循环频率
}
}
|
