// *********************************************************************************
// ***** Compass_Thread ********
// ***** continous thread that reads and calulates compass heading ********
// ***** (r) void ********
// ***** (1) void *: pointer to any data ********
// *********************************************************************************
void *Compass_Thread(void *ptr)
{
// *********************************************************************************
// ***** Veriable Declaratins ********
int i;
int v_iteration_number;
compass_struct strc_temp_compass_data;
int v_return_test;
int v_data_count;
unsigned char v_Register_Address;
unsigned char v_Compass_Array[6];
int a_compass[6];
// ---------- END ----------------------------------------------------------------
/*(printf("Entering Compass calibration Mode\n");
Init_Calibrate_Compass();
usleep(60000000);
printf("Exiting compass calibration mode\n");
End_Calibrate_Compass();*/
// *********************************************************************************
// ***** Main Thread Loop ********
for(;;)
{
if(!calibrating_compass)
{
// *********************************************************************************
// ***** Read compass ********
v_Register_Address = 'A';
v_data_count = 0;
v_return_test = i2c_write(v_device, COMPASS_ADDRESS, v_Register_Address, v_Compass_Array, v_data_count);
usleep(6000);
v_data_count = 2;
v_return_test = i2c_read(v_device, COMPASS_ADDRESS, SKIP_WRITE_REGISTER, v_Compass_Array, v_data_count);
a_compass[0] = array_to_16bit_int(v_Compass_Array[0], v_Compass_Array[1]);
Calculate_Heading(a_compass, &strc_temp_compass_data);
// ---------- END ----------------------------------------------------------------
strc_compass = strc_temp_compass_data;
v_iteration_number++;
new_compass_data = 1;
}
usleep(100000);
}
// ---------- END ----------------------------------------------------------------
}
int main(int argc, char **argv)
{
int mpu9150_fd;
unsigned int m = 1903;
FILE *from_fd,*to_fd;
int magn_fd;
double *ax,*ay;
double count_B2;//水平磁场分量
mpu9150_fd = open("/dev/mpu9150",O_RDWR);
if (mpu9150_fd < 0){
printf("can't open /dev/mpu9150\n");
return -1;
}else{
printf("Now,you are in /dev/mpu9150\n");
}
magn_fd = open("/dev/magn",O_RDWR);
if (magn_fd < 0){
printf("can't open /dev/magn\n");
return -1;
}else{
printf("Now,you are in /dev/magn\n");
}
ax = (double *)malloc(sizeof (double)* m);
if(NULL==ax){
printf("error to malloc ax\n");
exit(1);
}
ay = (double *)malloc(sizeof (double)* m);
if(NULL==ay){
printf("error to malloc ay\n");
exit(1);
}
from_fd = fopen("/data_file.txt","r");
if(from_fd == NULL){
printf("open '/data_file.txt' failed!");
return -1;
}
for(int i=0;i<m;i++){
fscanf(from_fd,"%lf%*c%lf",&ax[i],&ay[i]); //中间跳过空格,水平分量值读入数组中
}
fclose(from_fd);
count_B2 = ((MAX(ax,m) - MIN(ax,m))/2.0)*((MAX(ax,m) - MIN(ax,m))/2.0);
g_ellipse_parameter.F = -count_B2;
g_equation_temp_value.bb = (double *)malloc(sizeof (double)* 5);
for(int i=0;i<5;i++){
g_equation_temp_value.bb[i] = count_B2; //初始化线性常量
}
g_equation_temp_value = equation_temp_value(ax,ay,g_equation_temp_value,m); //相关运算，得到设定方程的临时参数
free(ay);
ay = NULL;
free(ax);
ax = NULL;
Gauss(g_equation_temp_value.V,g_equation_temp_value.bb,5); //计算得到拟合椭圆参数，存储在bb[0]~bb[4]中
free(g_equation_temp_value.V);
g_ellipse_parameter = ellipse_parameter_calculate(g_equation_temp_value,g_ellipse_parameter);//计算得到椭圆倾斜角等量
free(g_equation_temp_value.bb);
cos_phi=cos(g_ellipse_parameter.orientation_rad);
sin_phi=sin(g_ellipse_parameter.orientation_rad);
to_fd = fopen("/magn_data_output.txt","w+");
if(to_fd == NULL){
printf("open '/magn_data_output.txt' failed!");
return -1;
}
sleep(2);
Accel_Correct(mpu9150_fd); //消除固有误差（放置不水平等问题）
while(1){
Get_AccelGyro_Data(mpu9150_fd);
//Get_9150Magn_Data(mpu9150_fd); //未驱动使用；
Coordinate_Transformation(); //将9150坐标系变换到磁力计lis3mdl坐标系;
Calculate_PitchRoll(); //计算俯仰角和侧倾角
Calculate_Heading(magn_fd,to_fd);
Printf_AccelGyroMagn_Output();
usleep(20*1000);
}
close(mpu9150_fd);
close(magn_fd);
fclose(to_fd);
return 0;
}