144 lines
3.0 KiB
C
144 lines
3.0 KiB
C
#include "accelerometer.h"
|
||
|
||
|
||
typedef struct accCalibration_s
|
||
{
|
||
Axis3i32 accSum;
|
||
Axis3i16 accZero;
|
||
stdev_t var[3];
|
||
uint16_t cycleCount;
|
||
int ok;
|
||
uint16_t accZeroFlashSign;
|
||
} accCalibration_t;
|
||
|
||
|
||
//软件二阶低通滤波参数（单位Hz）
|
||
#define ACCEL_LPF_CUTOFF_FREQ 200.0f
|
||
|
||
//传感器对齐定义的机体坐标系的安装方向
|
||
#define ACCEL_ALIGN CW0_DEG
|
||
|
||
//mpu6000初始化加速度量程为FSR_8G，ACC_1G_ADC = 65536 / (2 * 16) = 4096
|
||
#define ACC_1G_ADC 4096.0f
|
||
|
||
Axis3i16 accADCRaw; //加速度原始AD数据
|
||
Axis3i16 accADC; //校准后的AD数据
|
||
Axis3f accf; //转换单位为G的数据
|
||
|
||
static accCalibration_t accCalibration; //加速度校准结构体参数
|
||
|
||
biquadFilter_t accFilterLPF[XYZ_AXIS_COUNT];//二阶低通滤波器
|
||
|
||
|
||
|
||
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
|
||
{
|
||
accCalibration.cycleCount = calibrationCyclesRequired;
|
||
}
|
||
|
||
|
||
//执行校准
|
||
static void performAcclerationCalibration(Axis3i16 accADCSample)
|
||
{
|
||
for (int axis = 0; axis < 3; axis++)
|
||
{
|
||
//复位数据
|
||
if (accCalibration.cycleCount == CALIBRATING_ACC_CYCLES)
|
||
{
|
||
accCalibration.accSum.axis[axis] = 0;
|
||
devClear(&accCalibration.var[axis]);
|
||
|
||
}
|
||
|
||
//陀螺数据累加
|
||
accCalibration.accSum.axis[axis] += accADCSample.axis[axis];
|
||
|
||
//复位零偏
|
||
accCalibration.accZero.axis[axis] = 0;
|
||
|
||
if (accCalibration.cycleCount == 1)
|
||
{
|
||
//校准完成
|
||
accCalibration.accZero.axis[axis] = (accCalibration.accSum.axis[axis] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
|
||
if(axis==2)
|
||
accCalibration.accZero.axis[axis] -= (int)ACC_1G_ADC;
|
||
accCalibration.ok = 0;
|
||
//EEPROM_write(3);
|
||
}
|
||
}
|
||
accCalibration.cycleCount--;
|
||
|
||
}
|
||
|
||
|
||
bool ACC_Calibration(void)
|
||
{
|
||
if(accCalibration.ok == 0)
|
||
return 1;
|
||
return 0;
|
||
}
|
||
|
||
|
||
|
||
bool accInit(float accUpdateRate)
|
||
{
|
||
|
||
//初始化软件二阶低通滤波
|
||
for (int axis = 0; axis < 3; axis++)
|
||
{
|
||
biquadFilterInitLPF(&accFilterLPF[axis], ACCEL_LPF_CUTOFF_FREQ , accUpdateRate);
|
||
}
|
||
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
|
||
accCalibration.ok = 0;
|
||
|
||
return true;
|
||
}
|
||
|
||
|
||
|
||
|
||
void accUpdate(Axis3f *acc)
|
||
{
|
||
|
||
//读取原始数据
|
||
accADCRaw.x= accX1();
|
||
accADCRaw.y= accY1();
|
||
accADCRaw.z= accZ1();
|
||
|
||
//传感器数据方向对齐
|
||
applySensorAlignment(accADCRaw.axis, accADCRaw.axis, CW90_DEG);
|
||
|
||
//加速度计校准
|
||
if (accCalibration.ok==1)
|
||
{
|
||
performAcclerationCalibration(accADCRaw);
|
||
return;
|
||
}
|
||
|
||
//计算accADC值，减去零偏
|
||
accADC.x = accADCRaw.x - accCalibration.accZero.x;
|
||
accADC.y = accADCRaw.y - accCalibration.accZero.y;
|
||
accADC.z = accADCRaw.z - accCalibration.accZero.z;
|
||
|
||
////板对齐
|
||
//applyBoardAlignment(accADC.axis);
|
||
|
||
//转换为单位 g (9.8m/s^2)
|
||
accf.x = (float)accADC.x / ACC_1G_ADC;
|
||
accf.y = (float)accADC.y / ACC_1G_ADC;
|
||
accf.z = (float)accADC.z / ACC_1G_ADC;
|
||
|
||
// //软件低通滤波
|
||
// for (int axis = 0; axis < 3; axis++)
|
||
// {
|
||
// accf.axis[axis] = biquadFilterApply(&accFilterLPF[axis], accf.axis[axis]);
|
||
// }
|
||
|
||
*acc = accf;
|
||
|
||
}
|
||
|
||
|
||
|
||
|