173 lines
4.2 KiB
C
173 lines
4.2 KiB
C
#include "gyro.h"
|
||
|
||
|
||
|
||
#define GYRO_CALIBRATION_THRESHOLD 4.0f /* 陀螺仪校准阈值 */
|
||
|
||
#define GYRO_LPF_CUTOFF_FREQ 200.0f /* 低通滤波参数 */
|
||
|
||
/*传感器对齐定义的机体坐标系的安装方向*/
|
||
#define GYRO_ALIGN CW0_DEG
|
||
#define GYRO_SCALE 16.384f
|
||
|
||
|
||
//陀螺仪校准结构体参数
|
||
gyroCalibration_t gyroCalibration =
|
||
{
|
||
.cycleCount = CALIBRATING_GYRO_CYCLES,
|
||
};
|
||
|
||
Axis3i16 gyroADCRaw; //陀螺仪原始AD数据
|
||
Axis3i16 gyroADC; //校准的AD数据
|
||
Axis3f gyrof; //转换单位为°/s的数据
|
||
|
||
|
||
biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];//二阶低通滤波器
|
||
|
||
|
||
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
|
||
{
|
||
gyroCalibration.cycleCount = calibrationCyclesRequired;
|
||
}
|
||
|
||
|
||
void performGyroCalibration(Axis3i16 gyroADCSample)
|
||
{
|
||
for (int axis = 0; axis < 3; axis++)
|
||
{
|
||
//复位数据
|
||
if (gyroCalibration.cycleCount == CALIBRATING_GYRO_CYCLES)
|
||
{
|
||
gyroCalibration.gyroSum.axis[axis] = 0;
|
||
devClear(&gyroCalibration.var[axis]);
|
||
|
||
}
|
||
|
||
//陀螺数据累加
|
||
gyroCalibration.gyroSum.axis[axis] += gyroADCSample.axis[axis];
|
||
devPush(&gyroCalibration.var[axis], gyroADCSample.axis[axis]);
|
||
|
||
//复位零偏
|
||
gyroCalibration.gyroZero.axis[axis] = 0;
|
||
printf(" cycleCount:%d \r\n ",gyroCalibration.cycleCount);
|
||
if (gyroCalibration.cycleCount == 1)
|
||
{
|
||
const float stddev = devStandardDeviation(&gyroCalibration.var[axis]);
|
||
//检测方差值是否大于陀螺仪受到移动的阈值
|
||
//如果大于设定阈值则返回重新校准
|
||
printf(" axis:%d ",axis);
|
||
printf(" stddev:%f ",stddev);
|
||
printf(" \r\n ");
|
||
|
||
if ((stddev > GYRO_CALIBRATION_THRESHOLD) || (stddev == 0))
|
||
{
|
||
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
|
||
gyroCalibration.ok = 1;
|
||
return;
|
||
}
|
||
|
||
//校准完成
|
||
gyroCalibration.gyroZero.axis[axis] = gyroCalibration.gyroSum.axis[axis] / CALIBRATING_GYRO_CYCLES;
|
||
gyroCalibration.ok = 0;
|
||
gyroCalibration.gyroZeroFlashSign = 111;
|
||
printf(" gyroCalibration.gyroSum.axis[axis]:%d ",gyroCalibration.gyroSum.axis[axis]);
|
||
printf(" gyroZerox:%d ",gyroCalibration.gyroZero.axis[X]);
|
||
printf(" gyroZeroy:%d ",gyroCalibration.gyroZero.axis[Y]);
|
||
printf(" gyroZeroz:%d ",gyroCalibration.gyroZero.axis[Z]);
|
||
printf(" \r\n ");
|
||
|
||
|
||
|
||
|
||
}
|
||
}
|
||
|
||
gyroCalibration.cycleCount--;
|
||
|
||
if(gyroCalibration.cycleCount==0)
|
||
{
|
||
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
|
||
}
|
||
}
|
||
|
||
bool GYRO_Calibration(void)
|
||
{
|
||
if(gyroCalibration.ok == 0)
|
||
return 1;
|
||
return 0;
|
||
}
|
||
|
||
void GYRO_Calibration_write(uint8_t data)
|
||
{
|
||
gyroCalibration.ok = data;
|
||
}
|
||
|
||
bool gyroInit(float gyroUpdateRate)//
|
||
{
|
||
//初始化陀螺仪零偏校准
|
||
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
|
||
gyroCalibration.ok = 0;// 1 校准陀螺仪
|
||
//初始化二阶低通滤波
|
||
for (int axis = 0; axis < 3; axis++)
|
||
{
|
||
biquadFilterInitLPF(&gyroFilterLPF[axis],GYRO_LPF_CUTOFF_FREQ, gyroUpdateRate);
|
||
|
||
}
|
||
return true;
|
||
}
|
||
|
||
|
||
void gyroUpdate(Axis3f *gyro)
|
||
{
|
||
if(getAGT()>=0)//读取原始数据
|
||
{
|
||
gyroADCRaw.x= gyrX1();
|
||
gyroADCRaw.y= gyrY1();
|
||
gyroADCRaw.z= gyrZ1();
|
||
|
||
}
|
||
|
||
//传感器数据方向对齐
|
||
applySensorAlignment(gyroADCRaw.axis, gyroADCRaw.axis, CW90_DEG);
|
||
//陀螺仪校准
|
||
if (gyroCalibration.ok==1)
|
||
{
|
||
performGyroCalibration(gyroADCRaw);
|
||
|
||
gyrof.x = 0.0f;
|
||
gyrof.y = 0.0f;
|
||
gyrof.z = 0.0f;
|
||
*gyro = gyrof;
|
||
return;
|
||
}
|
||
|
||
//计算gyroADC值，减去零偏
|
||
gyroADC.x = gyroADCRaw.x - gyroCalibration.gyroZero.x;
|
||
gyroADC.y = gyroADCRaw.y - gyroCalibration.gyroZero.y;
|
||
gyroADC.z = gyroADCRaw.z - gyroCalibration.gyroZero.z;
|
||
|
||
//板对齐
|
||
// applyBoardAlignment(gyroADC.axis);
|
||
|
||
//转换为单位 °/s
|
||
gyrof.x = (float)gyroADC.x / GYRO_SCALE;
|
||
gyrof.y = (float)gyroADC.y / GYRO_SCALE;
|
||
gyrof.z = (float)gyroADC.z / GYRO_SCALE;
|
||
|
||
//printf(" Xs:%f ",gyrof.x);
|
||
//printf(" Ys:%f ",gyrof.y);
|
||
//软件二阶低通滤波
|
||
// for (int axis = 0; axis < 3; axis++)
|
||
// {
|
||
// gyrof.axis[axis] = biquadFilterApply(&gyroFilterLPF[axis], gyrof.axis[axis]);
|
||
// }
|
||
|
||
*gyro = gyrof;
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
|