GeekIMU/4.Software/GeekIMU Manager GUI 1.2/GeekIMUDriver 1.2/Src/MagCalibrate.cpp

/******************** (C) COPYRIGHT 2023 Geek************************************
* File Name          : MagCalibrate.cpp
* Department         : Sensor Algorithm Team
* Current Version    : V1.0
* Author             : zhanli
* Date of Issued     : 2023.09.10
* Comments           : GeekIMU <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У׼ģ<EFBFBD><EFBFBD>
********************************************************************************/
/* Header File Including -----------------------------------------------------*/

#include <stdio.h>
#include <math.h>
#include <conio.h>
#include "MagCalibrate.h"
#include <fstream>
#include "Log.h"

#define ENABLE_DEBUG_LOG  1

MagData mdata[4000];

// <20><><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD>ģֵ<C4A3><D6B5><EFBFBD><EFBFBD>
bool checkMagnetNorm(float x, float y, float z);

MagCaliParam CalculateMagCalibrationParams(MagData _mdata[], int dataSize)
{
	MagCaliParam param, lastparam;

	srand((unsigned)time(NULL));

	float	xmin = _mdata[0].x;
	float	ymin = _mdata[0].y;
	float	zmin = _mdata[0].z;
	float	xmax = _mdata[0].x;
	float	ymax = _mdata[0].y;
	float	zmax = _mdata[0].z;
	float err, errnew;

	for (int i = 0; i < dataSize; i++)
	{
		if (xmax < _mdata[i].x)
			xmax = _mdata[i].x;
		if (ymax < _mdata[i].y)
			ymax = _mdata[i].y;
		if (zmax < _mdata[i].z)
			zmax = _mdata[i].z;
		if (xmin > _mdata[i].x)
			xmin = _mdata[i].x;
		if (ymin > _mdata[i].y)
			ymin = _mdata[i].y;
		if (zmin > _mdata[i].z)
			zmin = _mdata[i].z;
	}

	lastparam.xOffset = 0.5f * (xmax + xmin);
	lastparam.yOffset = 0.5f * (ymax + ymin);
	lastparam.zOffset = 0.5f * (zmax + zmin);
	lastparam.xScale = 0.5f * fabs(xmax - xmin);
	lastparam.yScale = 0.5f * fabs(ymax - ymin);
	lastparam.zScale = 0.5f * fabs(zmax - zmin);
	err = 0;
	for (int i = 0; i < dataSize; i++)
		err = err + fabs(powf((_mdata[i].x - lastparam.xOffset), 2) / powf(lastparam.xScale, 2) + powf((_mdata[i].y - lastparam.yOffset), 2) / powf(lastparam.yScale, 2) + powf((_mdata[i].z - lastparam.zOffset), 2) / powf(lastparam.zScale, 2) - 1);
	//printf("xc = %f yc = %f zc = %f, a = %f b = %f c= %f,<2C><>ʼ<EFBFBD><CABC>InitErr = %f\n", lastparam.xOffset, lastparam.yOffset, lastparam.zOffset, lastparam.xScale, lastparam.yScale, lastparam.zScale, err);
	int starttime = clock();
	for (int j = 0; j < 5000; j++)
	{
		param.xOffset = lastparam.xOffset + rand() / (RAND_MAX + 1.0f) - 0.5f;
		param.yOffset = lastparam.yOffset + rand() / (RAND_MAX + 1.0f) - 0.5f;
		param.zOffset = lastparam.zOffset + rand() / (RAND_MAX + 1.0f) - 0.5f;
		param.xScale = fabs(lastparam.xScale + rand() / (RAND_MAX + 1.0f) - 0.5f);
		param.yScale = fabs(lastparam.yScale + rand() / (RAND_MAX + 1.0f) - 0.5f);
		param.zScale = fabs(lastparam.zScale + rand() / (RAND_MAX + 1.0f) - 0.5f);
		errnew = 0;
		for (int i = 0; i < dataSize; i++)
			errnew = errnew + fabs(powf((_mdata[i].x - param.xOffset), 2) / powf(param.xScale, 2) + powf((_mdata[i].y - param.yOffset), 2) / powf(param.yScale, 2) + powf((_mdata[i].z - param.zOffset), 2) / powf(param.zScale, 2) - 1);
		if (errnew < err)
		{
			lastparam.xOffset = param.xOffset;
			lastparam.yOffset = param.yOffset;
			lastparam.zOffset = param.zOffset;
			lastparam.xScale = param.xScale;
			lastparam.yScale = param.yScale;
			lastparam.zScale = param.zScale;
			err = errnew;
		}
	}
	param.xScale = 200 / param.xScale;
	param.yScale = 200 / param.yScale;
	param.zScale = 200 / param.zScale;
	int durtime = clock() - starttime;

	printf("%f, %f, %f, %f, %f, %f, %f\n cost:%dms\n", param.xOffset, param.yOffset, param.zOffset, param.xScale, param.yScale, param.zScale, err, durtime);

	return param;
}


// <20><><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD>ظ<EFBFBD><D8B8>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
bool isDuplicateData(float x, float y, float z, int index) {

	if (index < 0 || index >= 4000) {
		return false;
	}
	// <20><><EFBFBD><EFBFBD>x, y, z<><7A>ȫ<EFBFBD><C8AB>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD>
	if (mdata[index].x == x && mdata[index].y == y && mdata[index].z == z) {
#if ENABLE_DEBUG_LOG
		WriteLog(Info, "MagCalibrate: %.6f,%.6f,%.6f ==> Mag Data Duplicate\n",x,y,z);
#endif
		return true;
	}

	return false;
}

MagCaliParam GetMagDataFormFile()
{
	int maxdatasize = 4000;
	std::ifstream fin("data.txt");
	float x, y, z;
	int i = 0;
	clearMagData();
#if ENABLE_DEBUG_LOG
	WriteLog(Info, "MagCalibrate: Get mag data form file\n");
#endif
	while (fin) {
		fin >> x >> y >> z;
		// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		fin.get(); 

		// ʹ<><CAB9>checkMagnetNorm<72><6D><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>Ϊ<EFBFBD>쳣ֵ && 
		// <20><><EFBFBD>鵱ǰ<E9B5B1><C7B0>ȡ<EFBFBD><C8A1><EFBFBD>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD>xyz<79>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ȫ<EFBFBD><C8AB>ͬ
		if (checkMagnetNorm(x, y, z) || isDuplicateData(x, y, z, i - 1)) {
			// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>쳣ֵ<ECB3A3><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>
			continue;
		}
		WriteLog(Info, "MagCalibrate: Load Magnet Data: %f,%f,%f\n",x,y,z);
		mdata[i].x = x;
		mdata[i].y = y;
		mdata[i].z = z;
		i++;

		if (i == maxdatasize)
			break;
	}

	MagCaliParam p = CalculateMagCalibrationParams(mdata, i);

	return p;
}

// ʹ<><CAB9>memset<65><74><EFBFBD><EFBFBD>mdata<74><61><EFBFBD><EFBFBD>
void clearMagData() {
	std::memset(mdata, 0, sizeof(mdata));  // <20><>mdata<74><61><EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD><EFBFBD><EFBFBD>
}

// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD>ģ<EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>
bool checkMagnetNorm(float x, float y, float z) {
	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݵ<EFBFBD>ģ
	float norm = std::sqrt(x * x + y * y + z * z);

#if ENABLE_DEBUG_LOG
	if (norm > 2000.0f) {
		WriteLog(Info, "Magnet data norm > 2000.0f\n");
	}
	if (norm == 0.0f) {
		WriteLog(Info, "Magnet data norm = 0.0f\n");
	}
#endif
	// <20><><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>2000<30><30><EFBFBD>ߵ<EFBFBD><DFB5><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>쳣
	return (norm > 2000.0f || norm == 0.0f);
}