void light_led_number (int number) {
static int done = 0;
if (done == 0) {
HAL_Init();
BSP_LED_Init(LED3);
BSP_LED_Init(LED4);
BSP_LED_Init(LED5);
BSP_LED_Init(LED6);
BSP_LED_Init(LED7);
BSP_LED_Init(LED8);
BSP_LED_Init(LED9);
BSP_LED_Init(LED10);
done++;
}
BSP_LED_Off(LED3);
BSP_LED_Off(LED4);
BSP_LED_Off(LED5);
BSP_LED_Off(LED6);
BSP_LED_Off(LED7);
BSP_LED_Off(LED8);
BSP_LED_Off(LED9);
BSP_LED_Off(LED10);
if (number & 0x1)
BSP_LED_On(LED3);
if (number & 0x2)
BSP_LED_On(LED5);
if (number & 0x4)
BSP_LED_On(LED7);
if (number & 0x8)
BSP_LED_On(LED9);
if (number & 0x10)
BSP_LED_ON(LED10);
if (number & 0x20)
BSP_LED_ON(LED8);
if (number & 0x30)
BSP_LED_ON(LED6);
if (number & 0x40)
BSP_LED_ON(LED4);
}
void runCalibration2 ( AttitudeSensor* sensor )
{
int i ;
entryCount ++ ;
if ( entryCount >= SAMPLE_STEP )
entryCount = 0 ;
if ( entryCount != 0 )
return ;
if ( sensor->AllReady )
{
BSP_LED_ON(LED_R) ; // 点亮红灯提示
BSP_LED_OFF (LED_G) ; // 关绿灯
BSP_LED_OFF(LED_B) ; // 关蓝灯
return ;
}
// 读取加速度计的低通+滑动平均滤波+跟随预测值
int ax, ay, az , mx , my, mz ;
ax = getAccXSlideAverage(sensor);
ay = getAccYSlideAverage(sensor);
az = getAccZSlideAverage(sensor);
mx = sensor->mx ;
my = sensor->my ;
mz = sensor->mz ;
// 更新加速度的历史值队列
for (i=0;i<N-1;i++)
{
recentAx[i] = recentAx[i+1] ;
recentAy[i] = recentAy[i+1] ;
recentAz[i] = recentAz[i+1] ;
recentMx[i] = recentMx[i+1] ;
recentMy[i] = recentMy[i+1] ;
recentMz[i] = recentMz[i+1] ;
}
recentAx [N-1] = ax ;
recentAy [N-1] = ay ;
recentAz [N-1] = az ;
recentMx [N-1] = mx ;
recentMy [N-1] = my ;
recentMz [N-1] = mz ;
if ( validSamples < N )
{
validSamples ++ ;
return ;
}
// 找到当前加速度的主轴
int16_t* recentAp [3] = { recentAx, recentAy, recentAz };
int16_t ra [3] = {ax,ay,az} ;
int axis = 0 ;
int16_t max = ra[axis] ;
if ( max*max < ay*ay )
{
axis = 1 ;
max = ra[axis] ;
}
if ( max*max < az*az )
{
axis = 2 ;
max = ra[axis] ;
}
for ( i=0 ; i<3 ; i++ )
{
if ( i == axis )
continue ;
float temp = (float)ra[i] / ra[axis] ;
temp *= temp ;
if ( temp > (0.15*0.15) )
{
axis = -1 ;
break ;
}
}
// 检查主轴的加速度是否趋于平稳
if ( axis != -1 && axis == stableAxis )
{
bool stable = true ;
int delta = 0 ;
for ( i=0; i<N; i++ )
{
int temp = recentAp[axis][i] - recentAp[axis][0] ;
if ( temp < 0 )
temp = -temp ;
if ( temp > deltaThr )
{
stable = false ;
break ;
}
}
if ( stable )
stableCount ++ ;
else
stableCount = 0 ;
}
else
{
stableAxis = axis ;
stableCount = 0 ;
}
// 如果主轴已经足够平稳，则采纳主轴的值
if ( stableCount > N )
{
int sum = 0 , sumMx=0, sumMy=0, sumMz=0 ;
int ave , aveMx, aveMy, aveMz ;
for ( i=0; i<N; i++ )
{
sum += recentAp[axis][i] ;
sumMx += recentMx[i] ;
sumMy += recentMy[i] ;
sumMz += recentMz[i] ;
}
ave = sum / N ;
aveMx = sumMx / N ;
aveMy = sumMy / N ;
aveMz = sumMz / N ;
if ( ave > 0 )
{
if ( ! sensor->AccMaxReady[axis] )
{
sensor->AccMax[axis] = ave ;
goodMagSamples [2*axis][0] = aveMx ;
goodMagSamples [2*axis][1] = aveMy ;
goodMagSamples [2*axis][2] = aveMz ;
sensor->AccMaxReady[axis] = true ;
}
else
{
sensor->AccMax[axis] = ave*2/100 + (int32_t)(sensor->AccMax[axis])*98/100 ;
goodMagSamples [2*axis][0] = aveMx*2/100 + (int32_t)(goodMagSamples [2*axis][0])*98/100 ;
goodMagSamples [2*axis][1] = aveMy*2/100 + (int32_t)(goodMagSamples [2*axis][1])*98/100 ;
goodMagSamples [2*axis][2] = aveMz*2/100 + (int32_t)(goodMagSamples [2*axis][2])*98/100 ;
sensor->AccMaxReady[axis] = true ;
}
}
else
{
if ( ! sensor->AccMinReady[axis] )
{
sensor->AccMin[axis] = ave ;
goodMagSamples [2*axis+1][0] = aveMx ;
goodMagSamples [2*axis+1][1] = aveMy ;
goodMagSamples [2*axis+1][2] = aveMz ;
sensor->AccMinReady[axis] = true ;
}
else
{
sensor->AccMin[axis] = ave*2/100 + (int32_t)(sensor->AccMin[axis])*98/100 ;
goodMagSamples [2*axis+1][0] = aveMx*2/100 + (int32_t)(goodMagSamples [2*axis+1][0])*98/100 ;
goodMagSamples [2*axis+1][1] = aveMy*2/100 + (int32_t)(goodMagSamples [2*axis+1][1])*98/100 ;
goodMagSamples [2*axis+1][2] = aveMz*2/100 + (int32_t)(goodMagSamples [2*axis+1][2])*98/100 ;
sensor->AccMinReady[axis] = true ;
}
}
BSP_LED_OFF (LED_G) ; // 关绿灯
BSP_LED_ON(LED_B) ; // 亮蓝灯
}
//else if ( stableCount > 0 )
else if ( axis != -1 )
{
BSP_LED_ON (LED_G) ; // 亮绿灯
BSP_LED_OFF(LED_B) ; // 关蓝灯
}
else
{
BSP_LED_OFF (LED_G) ; // 关绿灯
BSP_LED_OFF(LED_B) ; // 关蓝灯
}
//#define AccMagCalibrateTogether
#ifndef AccMagCalibrateTogether
// 如果加速度的三个轴都已校正过，则校正完毕
if ( sensor->AccMaxReady[0] && sensor->AccMinReady[0] &&
sensor->AccMaxReady[1] && sensor->AccMinReady[1] &&
sensor->AccMaxReady[2] && sensor->AccMinReady[2] )
{
// 对各字段赋值
for ( i=0; i<3; i++ )
{
calibrateParam.data.info.aMax[i] = sensor->AccMax[i] ;
calibrateParam.data.info.aMin[i] = sensor->AccMin[i] ;
}
// 计算校验和
uint32_t sum = 0 ;
for ( i=0; i<sizeof(calibrateParam.data)/sizeof(uint32_t); i++ )
{
sum += calibrateParam.data.raw[i] ;
}
calibrateParam.sum = sum ;
calibrateParam.sum_inv = ~sum ;
// 存储参数到 flash
if ( restoreCalibrateParamToFlash ( &calibrateParam ) )
{
sensor->AllReady = true ;
BSP_LED_ON(LED_R) ; // 点亮红灯提示
BSP_LED_OFF (LED_G) ; // 关绿灯
BSP_LED_OFF(LED_B) ; // 关蓝灯
}
}
#else
// 如果加速度的三个轴都已校正过，再开始校正磁场
if ( sensor->AccMaxReady[0] && sensor->AccMinReady[0] &&
sensor->AccMaxReady[1] && sensor->AccMinReady[1] &&
sensor->AccMaxReady[2] && sensor->AccMinReady[2] )
{
if ( goodMagSamplesCount < 6 )
{
goodMagSamplesCount = 6 ; // 加速度校完后，三个主轴各有两个好的磁场采样点
}
// 如果当前的采样值与之前记录的所有采样值相比明显不同，则认可当前采样值
int16_t dx, dy, dz ;
float minRatio = 0.3f ;
for ( i=0; i<goodMagSamplesCount; i++ )
{
dx = goodMagSamples[i][0] - mx ;
dy = goodMagSamples[i][1] - my ;
dz = goodMagSamples[i][2] - mz ;
// 差向量的长度小于当前向量本身长度的 minRatio 倍时，认为当前向量与其他向量的间距太近。
if ( dx*dx + dy*dy + dz*dz < (minRatio*minRatio) * ( mx*mx + my*my + mz*mz ) )
break ;
}
if ( i == goodMagSamplesCount )
{
// 采纳当前采样值
goodMagSamples [ goodMagSamplesCount ][0] = mx ;
goodMagSamples [ goodMagSamplesCount ][1] = my ;
goodMagSamples [ goodMagSamplesCount ][2] = mz ;
goodMagSamplesCount ++ ;
}
// 如果好的采样点数量已足够，则计算拟合方程
if ( goodMagSamplesCount >= MaxGoodMagSamplesCount )
{
// 计算原始方程的因子
float (*p) [6] = MagOriginEquationFactors ;
int16_t (*s) [3] = goodMagSamples ;
for ( i=0; i<MaxGoodMagSamplesCount; i++ )
{
float weight = 1 ;
if ( i < 6 )
weight *= MainAxis_MagWeight ; // 前 6 行是主轴方向测量的数据，为主轴方向的数据分配高权重
p[i][0] = s[i][0] * s[i][0] * weight;
p[i][1] = -2 * s[i][0] * weight;
p[i][2] = s[i][1] * s[i][1] * weight;
p[i][3] = -2 * s[i][1] * weight;
p[i][4] = s[i][2] * s[i][2] * weight;
p[i][5] = -2 * s[i][2] * weight;
}
// 计算拟合方程的因子
float (*x) [6] = MagEquationFactors ;
float *y = MagEquationResults ;
for (i=0; i<6; i++)
{
int j , k ;
// 计算 x[i][:]
for (j=0; j<6; j++)
{
x[i][j] = 0 ;
for ( k=0; k<MaxGoodMagSamplesCount; k++ )
{
x[i][j] += p[k][j] * p[k][i] ;
}
}
// 计算 y[i]
y[i] = 0 ;
for ( k=0; k<MaxGoodMagSamplesCount; k++ )
{
float weight = 1 ;
if ( k < 6 )
weight *= MainAxis_MagWeight ; // 前 6 行是主轴方向测量的数据，为主轴方向的数据分配高权重
y[i] += p[k][i] * weight ;
}
y[i] *= MagMeasuredRadius * MagMeasuredRadius ; // 乘一个因子
}
// 求解拟合的方程
if ( SolveLinearEquations ( (float*)x, y, 6, 6, y ) )
{
// 计算校正参数
float ratioX, ratioY, ratioZ ;
float offsetX, offsetY, offsetZ ;
offsetX = y[1]/y[0] ;
offsetY = y[3]/y[2] ;
offsetZ = y[5]/y[4] ;
ratioX = sqrt(y[0]/y[4]) ;
ratioY = sqrt(y[2]/y[4]) ;
ratioZ = 1 ; // 比例都以 Z 轴为参考
sensor->MagMax[0] = MagMeasuredRadius / ratioX + offsetX ;
sensor->MagMin[0] = (-MagMeasuredRadius) / ratioX + offsetX ;
sensor->MagMax[1] = MagMeasuredRadius / ratioY + offsetY ;
sensor->MagMin[1] = (-MagMeasuredRadius) / ratioY + offsetY ;
sensor->MagMax[2] = MagMeasuredRadius / ratioZ + offsetZ ;
sensor->MagMin[2] = (-MagMeasuredRadius) / ratioZ + offsetZ ;
sensor->MagReady = true ;
}
}
}
// 如果校准已完成，点亮红灯提示
bool allReady ;
allReady = sensor->AccMaxReady[0] && sensor->AccMinReady[0] &&
sensor->AccMaxReady[1] && sensor->AccMinReady[1] &&
sensor->AccMaxReady[2] && sensor->AccMinReady[2] &&
sensor->MagReady ;
if ( allReady )
{
// 清零校正参数结构体 不再进行清零操作，使calibrateParam保持之前的值
// for ( i=0; i<sizeof(calibrateParam.data)/sizeof(uint32_t); i++ )
// {
// calibrateParam.data.raw[i] = 0 ;
// }
// 对各字段赋值
for ( i=0; i<3; i++ )
{
calibrateParam.data.info.aMax[i] = sensor->AccMax[i] ;
calibrateParam.data.info.aMin[i] = sensor->AccMin[i] ;
calibrateParam.data.info.mMax[i] = sensor->MagMax[i] ;
calibrateParam.data.info.mMin[i] = sensor->MagMin[i] ;
}
// 计算校验和
uint32_t sum = 0 ;
for ( i=0; i<sizeof(calibrateParam.data)/sizeof(uint32_t); i++ )
{
sum += calibrateParam.data.raw[i] ;
}
calibrateParam.sum = sum ;
calibrateParam.sum_inv = ~sum ;
// 存储参数到 flash
if ( restoreCalibrateParamToFlash ( &calibrateParam ) )
{
sensor->AllReady = true ;
BSP_LED_ON(LED_R) ; // 点亮红灯提示
BSP_LED_OFF (LED_G) ; // 关绿灯
BSP_LED_OFF(LED_B) ; // 关蓝灯
}
}
#endif
}
void runCalibration4 ( AttitudeSensor* sensor )
{
int i ;
entryCount ++ ;
if ( entryCount >= SAMPLE_STEP )
entryCount = 0 ;
if ( entryCount != 0 )
return ;
if ( sensor->AllReady )
{
BSP_LED_ON(LED_R) ; // 点亮红灯提示
BSP_LED_OFF(LED_B) ; // 关蓝灯
return ;
}
// 读取加速度计的低通+滑动平均滤波+跟随预测值
int ax, ay, az , mx , my, mz ;
ax = getAccXSlideAverage(sensor);
ay = getAccYSlideAverage(sensor);
az = getAccZSlideAverage(sensor);
// 更新加速度的历史值队列
for (i=0;i<N-1;i++)
{
recentAx[i] = recentAx[i+1] ;
recentAy[i] = recentAy[i+1] ;
recentAz[i] = recentAz[i+1] ;
}
recentAx [N-1] = ax ;
recentAy [N-1] = ay ;
recentAz [N-1] = az ;
if ( validSamples < N )
{
validSamples ++ ;
return ;
}
// 找到当前加速度的主轴
int16_t* recentAp [3] = { recentAx, recentAy, recentAz };
// 检查加速度是否趋于平稳
bool stable = true ;
for ( i=0; i<N; i++ )
{
int dx = recentAx[i] - recentAx[0] ;
int dy = recentAy[i] - recentAy[0] ;
int dz = recentAz[i] - recentAz[0] ;
dx = dx < 0 ? -dx : dx ;
dy = dy < 0 ? -dy : dy ;
dz = dz < 0 ? -dz : dz ;
if ( dx > deltaThr || dy > deltaThr || dz > deltaThr )
{
stable = false ;
break ;
}
}
if ( stable )
stableCount ++ ;
else
stableCount = 0 ;
// 如果加速度已经足够平稳，说明Sensor已处于静止状态，开始记录陀螺的值
if ( stableCount > N )
{
gyroCaliSum [0] += sensor->gx ;
gyroCaliSum [1] += sensor->gy ;
gyroCaliSum [2] += sensor->gz ;
BSP_LED_ON(LED_B) ; // 亮灯提示
}
else
{
BSP_LED_OFF(LED_B) ; // 关闭指示灯
}
// 如果校准已完成，点亮红灯提示
bool allReady = false ;
if ( stableCount > 6*N )
{
gyroCaliSum[0] /= 5*N ;
gyroCaliSum[1] /= 5*N ;
gyroCaliSum[2] /= 5*N ;
allReady = true ;
}
if ( allReady )
{
// 清零校正参数结构体 不再进行清零操作，使calibrateParam保持之前的值
// for ( i=0; i<sizeof(calibrateParam.data)/sizeof(uint32_t); i++ )
// {
// calibrateParam.data.raw[i] = 0 ;
// }
// 对各字段赋值
for ( i=0; i<3; i++ )
{
calibrateParam.data.info.g_offset[i] = gyroCaliSum[i] ;
}
// 计算校验和
uint32_t sum = 0 ;
for ( i=0; i<sizeof(calibrateParam.data)/sizeof(uint32_t); i++ )
{
sum += calibrateParam.data.raw[i] ;
}
calibrateParam.sum = sum ;
calibrateParam.sum_inv = ~sum ;
// 存储参数到 flash
if ( restoreCalibrateParamToFlash ( &calibrateParam ) )
{
sensor->AllReady = true ;
BSP_LED_ON(LED_R) ; // 点亮红灯提示
BSP_LED_OFF(LED_B) ; // 关蓝灯
}
}
}
