/** Perform curve fitting via Levenberg-Marquardt with no bounds/weights. */
int CurveFit::LevenbergMarquardt(FitFunctionType fxnIn, Darray const& Xvals_,
Darray const& Yvals_, Darray& ParamVec,
double tolerance, int maxIterations)
{
return LevenbergMarquardt(fxnIn, Xvals_, Yvals_, ParamVec, std::vector<bool>(),
Darray(), Darray(), Darray(), tolerance, maxIterations);
}
/**********************************************************************************************************
*函 数 名: MagCalibration
*功能说明: 磁力计校准
*形 参: 磁力计原始数据
*返 回 值: 无
**********************************************************************************************************/
void MagCalibration(void)
{
static Vector3f_t samples[6];
static uint32_t cnt_m=0;
static float cali_rotate_angle = 0;
static Vector3f_t new_offset;
static Vector3f_t new_scale;
Vector3f_t magRaw;
static float earthMag = 0;
//计算时间间隔，用于积分
static uint64_t previousT;
float deltaT = (GetSysTimeUs() - previousT) * 1e-6;
previousT = GetSysTimeUs();
if(mag.cali.should_cali)
{
//读取罗盘数据
MagSensorRead(&magRaw);
//校准分两个阶段：1.水平旋转 2.机头朝上或朝下然后水平旋转
//两个阶段分别对飞机的z轴和x轴陀螺仪数据进行积分，记录旋转过的角度
if(mag.cali.step == 1)
{
cali_rotate_angle += GyroGetData().z * deltaT;
}
else if(mag.cali.step == 2)
{
cali_rotate_angle += GyroGetData().x * deltaT;
}
if(cnt_m == 0)
{
mag.cali.step = 1;
cali_rotate_angle = 0;
cnt_m++;
//发送当前校准步骤
MessageSensorCaliFeedbackEnable(MAG, mag.cali.step, mag.cali.success);
}
else if(cnt_m == 1)
{
//初始化磁场强度模值
earthMag = Pythagorous3(magRaw.x, magRaw.y, magRaw.z);
//初始化采样点
samples[MaxX] = samples[MinX] = magRaw;
samples[MaxY] = samples[MinY] = magRaw;
samples[MaxZ] = samples[MinZ] = magRaw;
cnt_m++;
}
else
{
//实时计算磁场强度模值
earthMag = earthMag * 0.998f + Pythagorous3(magRaw.x, magRaw.y, magRaw.z) * 0.002f;
//找到每个轴的最大最小值，并对采样值进行一阶低通滤波
if(Pythagorous3(magRaw.x, magRaw.y, magRaw.z) < earthMag * 1.5f)
{
//找到每个轴的最大最小值，并对采样值进行一阶低通滤波
if(magRaw.x > samples[MaxX].x)
{
LowPassFilter1st(&samples[MaxX], magRaw, 0.3);
}
if(magRaw.x < samples[MinX].x)
{
LowPassFilter1st(&samples[MinX], magRaw, 0.3);
}
if(magRaw.y > samples[MaxY].y)
{
LowPassFilter1st(&samples[MaxY], magRaw, 0.3);
}
if(magRaw.y < samples[MinY].y)
{
LowPassFilter1st(&samples[MinY], magRaw, 0.3);
}
if(magRaw.z > samples[MaxZ].z)
{
LowPassFilter1st(&samples[MaxZ], magRaw, 0.3);
}
if(magRaw.z < samples[MinZ].z)
{
LowPassFilter1st(&samples[MinZ], magRaw, 0.3);
}
}
else
{
earthMag = earthMag;
}
//mavlink发送当前校准进度
if(mag.cali.step == 1)
MavlinkSendNoticeProgress(abs(cali_rotate_angle) / 72);
else if(mag.cali.step == 2)
MavlinkSendNoticeProgress((abs(cali_rotate_angle) + 360) / 72);
//水平旋转一圈
if(mag.cali.step == 1 && abs(cali_rotate_angle) > 360)
{
mag.cali.step = 2;
cali_rotate_angle = 0;
//bsklink发送当前校准步骤
MessageSensorCaliFeedbackEnable(MAG, mag.cali.step, mag.cali.success);
//msklink发送当前校准步骤
MavlinkSendNoticeEnable(CAL_DOWN_DONE);
}
//竖直旋转一圈
if(mag.cali.step == 2 && abs(cali_rotate_angle ) > 360)
{
cnt_m = 0;
mag.cali.should_cali = 0;
mag.cali.step = 3;
cali_rotate_angle = 0;
earthMag = 0;
//计算当地地磁场强度模值均值
for(u8 i=0; i<3; i++)
{
earthMag += Pythagorous3((samples[i*2].x - samples[i*2+1].x) * 0.5f,
(samples[i*2].y - samples[i*2+1].y) * 0.5f,
(samples[i*2].z - samples[i*2+1].z) * 0.5f);
}
earthMag /= 3;
//计算方程解初值
float initBeta[6];
initBeta[0] = (samples[MaxX].x + samples[MinX].x) * 0.5f;
initBeta[1] = (samples[MaxY].y + samples[MinY].y) * 0.5f;
initBeta[2] = (samples[MaxZ].z + samples[MinZ].z) * 0.5f;
initBeta[3] = 1 / earthMag;
initBeta[4] = 1 / earthMag;
initBeta[5] = 1 / earthMag;
//LM法求解传感器误差方程最优解
LevenbergMarquardt(samples, &new_offset, &new_scale, initBeta, earthMag);
//判断校准参数是否正常
if(isnan(new_scale.x) || isnan(new_scale.y) || isnan(new_scale.z))
{
mag.cali.success = false;
}
else if(fabsf(new_scale.x-1.0f) > 0.8f || fabsf(new_scale.y-1.0f) > 0.8f || fabsf(new_scale.z-1.0f) > 0.8f)
{
mag.cali.success = false;
}
else if(fabsf(new_offset.x) > (earthMag * 2) || fabsf(new_offset.y) > (earthMag * 2) || fabsf(new_offset.z) > (earthMag * 2))
{
mag.cali.success = false;
}
else
{
mag.cali.success = true;
}
if(mag.cali.success)
{
mag.cali.offset = new_offset;
mag.cali.scale = new_scale;
mag.earthMag = earthMag;
//保存校准参数
ParamUpdateData(PARAM_MAG_OFFSET_X, &mag.cali.offset.x);
ParamUpdateData(PARAM_MAG_OFFSET_Y, &mag.cali.offset.y);
ParamUpdateData(PARAM_MAG_OFFSET_Z, &mag.cali.offset.z);
ParamUpdateData(PARAM_MAG_SCALE_X, &mag.cali.scale.x);
ParamUpdateData(PARAM_MAG_SCALE_Y, &mag.cali.scale.y);
ParamUpdateData(PARAM_MAG_SCALE_Z, &mag.cali.scale.z);
ParamUpdateData(PARAM_MAG_EARTH_MAG, &mag.earthMag);
//更新mavlink参数
MavParamSetValue(CAL_MAG0_XOFF, mag.cali.offset.x);
MavParamSetValue(CAL_MAG0_YOFF, mag.cali.offset.y);
MavParamSetValue(CAL_MAG0_ZOFF, mag.cali.offset.z);
MavParamSetValue(CAL_MAG0_XSCALE, mag.cali.scale.x);
MavParamSetValue(CAL_MAG0_YSCALE, mag.cali.scale.y);
MavParamSetValue(CAL_MAG0_XSCALE, mag.cali.scale.z);
//mavlink发送校准结果
MavlinkSendNoticeEnable(CAL_DONE);
}
else
{
//mavlink发送校准结果
MavlinkSendNoticeEnable(CAL_FAILED);
}
//bsklink发送校准结果
MessageSensorCaliFeedbackEnable(MAG, mag.cali.step, mag.cali.success);
mag.cali.step = 0;
earthMag = 0;
}
}
}
}
