103 lines
4.1 KiB
C
103 lines
4.1 KiB
C
#include "correction.h"
|
|
//#include "fmac.h"
|
|
//#include "cordic.h"
|
|
|
|
// 外部的 FMAC 和 CORDIC 句柄
|
|
//extern FMAC_HandleTypeDef hfmac;
|
|
//extern CORDIC_HandleTypeDef hcordic;
|
|
|
|
/**
|
|
* @brief 初始化校正参数（示例）
|
|
* @param params: 参数结构体指针
|
|
* @retval None
|
|
*/
|
|
void Init_CorrectionParams(CorrectionParams_t *params)
|
|
{
|
|
// --- 请在这里填充你从校准中得到的实际参数 ---
|
|
// 偏移
|
|
params->offset_x = 0.0f;
|
|
params->offset_y = 0.0f;
|
|
params->offset_z = 0.0f;
|
|
|
|
// 灵敏度/非正交矩阵 (这里假设为单位矩阵)
|
|
params->scale_xx = 1.0f; params->scale_xy = 0.0f; params->scale_xz = 0.0f;
|
|
params->scale_yx = 0.0f; params->scale_yy = 1.0f; params->scale_yz = 0.0f;
|
|
params->scale_zx = 0.0f; params->scale_zy = 0.0f; params->scale_zz = 1.0f;
|
|
}
|
|
|
|
/**
|
|
* @brief 应用三分量校正
|
|
* @param raw_x, raw_y, raw_z: 原始 ADC 数据
|
|
* @param corrected_x, corrected_y, corrected_z: 校正后数据的指针
|
|
* @param params: 校正参数
|
|
* @retval None
|
|
*/
|
|
void Apply_Correction(int32_t raw_x, int32_t raw_y, int32_t raw_z,
|
|
float *corrected_x, float *corrected_y, float *corrected_z,
|
|
CorrectionParams_t *params)
|
|
{
|
|
// 1. 将原始数据转换为浮点数
|
|
float x_f = (float)raw_x;
|
|
float y_f = (float)raw_y;
|
|
float z_f = (float)raw_z;
|
|
float temp_x, temp_y, temp_z;
|
|
|
|
// 2. 灵敏度/非正交校正 (矩阵乘法)
|
|
// temp_X = Sxx*x_f + Sxy*y_f + Sxz*z_f
|
|
// temp_Y = Syx*x_f + Syy*y_f + Syz*z_f
|
|
// temp_Z = Szx*x_f + Szy*y_f + Szz*z_f
|
|
// 使用 FMAC 加速 (Q1.15 格式)
|
|
// FMAC 通常处理定点数，我们需要将 float 转为 Q1.15
|
|
// 或者，如果 FMAC 配置支持浮点，可以直接用浮点
|
|
// 这里先用纯 C 实现，并标记出 FMAC 可用的地方
|
|
|
|
temp_x = params->scale_xx * x_f + params->scale_xy * y_f + params->scale_xz * z_f;
|
|
temp_y = params->scale_yx * x_f + params->scale_yy * y_f + params->scale_yz * z_f;
|
|
temp_z = params->scale_zx * x_f + params->scale_zy * y_f + params->scale_zz * z_f;
|
|
|
|
// 3. 偏移校正
|
|
*corrected_x = temp_x - params->offset_x;
|
|
*corrected_y = temp_y - params->offset_y;
|
|
*corrected_z = temp_z - params->offset_z;
|
|
|
|
// --- 使用 FMAC 的示例 (需要配置 FMAC 和数据格式转换) ---
|
|
/*
|
|
// 假设输入 x_f, y_f, z_f 和系数 params->scale_** 已转换为 Q1.15
|
|
int16_t input_vector[3] = { (int16_t)(x_f * 32768.0f), (int16_t)(y_f * 32768.0f), (int16_t)(z_f * 32768.0f) };
|
|
int16_t coeffs_x[3] = { (int16_t)(params->scale_xx * 32768.0f), (int16_t)(params->scale_xy * 32768.0f), (int16_t)(params->scale_xz * 32768.0f) };
|
|
int16_t coeffs_y[3] = { (int16_t)(params->scale_yx * 32768.0f), (int16_t)(params->scale_yy * 32768.0f), (int16_t)(params->scale_yz * 32768.0f) };
|
|
int16_t coeffs_z[3] = { (int16_t)(params->scale_zx * 32768.0f), (int16_t)(params->scale_zy * 32768.0f), (int16_t)(params->scale_zz * 32768.0f) };
|
|
int16_t result_vector[3];
|
|
|
|
// 计算 X
|
|
FMAC_FilterConfig(&hfmac, coeffs_x, 3, input_vector, result_vector, ...); // 配置 FIR
|
|
HAL_FMAC_FilterStart(&hfmac, ...);
|
|
while(HAL_FMAC_PollForTransfer(&hfmac, 10) != HAL_OK);
|
|
temp_x = (float)result_vector[0] / 32768.0f; // 转换回 float (近似)
|
|
|
|
// 计算 Y
|
|
FMAC_FilterConfig(&hfmac, coeffs_y, 3, input_vector, result_vector, ...);
|
|
HAL_FMAC_FilterStart(&hfmac, ...);
|
|
while(HAL_FMAC_PollForTransfer(&hfmac, 10) != HAL_OK);
|
|
temp_y = (float)result_vector[0] / 32768.0f;
|
|
|
|
// 计算 Z
|
|
FMAC_FilterConfig(&hfmac, coeffs_z, 3, input_vector, result_vector, ...);
|
|
HAL_FMAC_FilterStart(&hfmac, ...);
|
|
while(HAL_FMAC_PollForTransfer(&hfmac, 10) != HAL_OK);
|
|
temp_z = (float)result_vector[0] / 32768.0f;
|
|
|
|
*corrected_x = temp_x - params->offset_x;
|
|
*corrected_y = temp_y - params->offset_y;
|
|
*corrected_z = temp_z - params->offset_z;
|
|
*/
|
|
|
|
// --- 使用 CORDIC 的示例 (例如，计算校正后向量的模和角度) ---
|
|
/*
|
|
// 伪代码
|
|
CORDIC_Config(&hcordic, ...); // 配置为计算模和相位
|
|
HAL_CORDIC_Calculate(&hcordic, input_coords, output_coords, ...);
|
|
// 等待 CORDIC 完成
|
|
*/
|
|
}
|