C++ (Cpp) Mag_get_b Exemples

Exemple #1

Fichier : Mag.c Projet : bearxiong99/QUAD32

void Calibrate_Comp(MAG_XYZ * COMPAS)
 {		
	
	short MAX_X=-20000;
	short MAX_Y=-20000;
	short MAX_Z=-20000;
	
	short MIN_X=20000;
	short MIN_Y=20000;
	short MIN_Z=20000;
	 
	long long Kvadranty=0;
    LCD_Queue LCD;
	 
			 
	do 
	{
		 Mag_get_b(COMPAS);
	 
		 COMPAS->X=(short)((COMPAS->X_H<<8) | COMPAS->X_L);
		 COMPAS->Y=(short)((COMPAS->Y_H<<8) | COMPAS->Y_L);
		 COMPAS->Z=(short)((COMPAS->Z_H<<8) | COMPAS->Z_L);
		
		if (COMPAS->X==0)
   		{
   			if (COMPAS->Y>0)
   			{
   				COMPAS->fHeadingx=90;
   			}else
   			{
   				COMPAS->fHeadingx=270;
   			}
   		}
   		else
   		{	
			COMPAS->fHeadingx=(atan2f(COMPAS->Y,COMPAS->X)*(180/PI));//*(180/PI);
 		}
		
		 if (COMPAS->fHeadingx<0)
		 {
			 COMPAS->fHeadingx+=360;
		 }
		 		 		 
				 
		if (MAX_Y<COMPAS->Y)MAX_Y=COMPAS->Y;						//MIN_X=-146
		if (MAX_X<COMPAS->X)MAX_X=COMPAS->X;
		if (MAX_Z<COMPAS->Z)MAX_Z=COMPAS->Z;
	  
		if (MIN_X>COMPAS->X)MIN_X=COMPAS->X;
		if (MIN_Y>COMPAS->Y)MIN_Y=COMPAS->Y;
		if (MIN_Z>COMPAS->Z)MIN_Z=COMPAS->Z;
	  Kvadranty++;
	  delay_ms(1);
				  
	} while (Kvadranty<15000);//0b111111111111111111111111111111111111
	 
	COMPAS->Nasobek1=(double)(abs(MAX_X)+abs(MIN_X))/(abs(MAX_Y)+abs(MIN_Y));
	COMPAS->Nasobek2=(double)(abs(MAX_X)+abs(MIN_X))/(abs(MAX_Z)+abs(MIN_Z));
	
	 COMPAS->X_Offset=MIN_X+MAX_X;	//posunuti
	 COMPAS->Y_Offset=MIN_Y+MAX_Y;
	 COMPAS->Z_Offset=MIN_Z+MAX_Z;
	 
	 COMPAS->X_Offset-=(COMPAS->X_Offset)/2;
	 COMPAS->Y_Offset-=(COMPAS->Y_Offset)/2;
	 COMPAS->Z_Offset-=(COMPAS->Z_Offset)/2;
		
 }

Exemple #2

Fichier : Mag.c Projet : witc/test_mergovani

void Calibrate_Comp(MAG_XYZ * COMPAS)
 {		
	
	short MAX_X=-20000;
	short MAX_Y=-20000;
	short MAX_Z=-20000;
	
	short MIN_X=20000;
	short MIN_Y=20000;
	short MIN_Z=20000;
	 
	long long Kvadranty=0;
    LCD_Queue LCD;
	 
			 
	do 
	{
		 Mag_get_b(COMPAS);
	 
		 COMPAS->X=(short)((COMPAS->X_H<<8) | COMPAS->X_L);
		 COMPAS->Y=(short)((COMPAS->Y_H<<8) | COMPAS->Y_L);
		 COMPAS->Z=(short)((COMPAS->Z_H<<8) | COMPAS->Z_L);
		
		if (COMPAS->X==0)
   		{
   			if (COMPAS->Y>0)
   			{
   				COMPAS->fHeadingx=90;
   			}else
   			{
   				COMPAS->fHeadingx=270;
   			}
   		}
   		else
   		{	
			COMPAS->fHeadingx=(atan2f(COMPAS->Y,COMPAS->X)*(180/M_PI));//*(180/PI);
 		}
		
		 if (COMPAS->fHeadingx<0)
		 {
			 COMPAS->fHeadingx+=360;
		 }
		 		 		 
				 
		if (MAX_Y<COMPAS->Y)MAX_Y=COMPAS->Y;						//MIN_X=-146
		if (MAX_X<COMPAS->X)MAX_X=COMPAS->X;
		if (MAX_Z<COMPAS->Z)MAX_Z=COMPAS->Z;
	  
		if (MIN_X>COMPAS->X)MIN_X=COMPAS->X;
		if (MIN_Y>COMPAS->Y)MIN_Y=COMPAS->Y;
		if (MIN_Z>COMPAS->Z)MIN_Z=COMPAS->Z;
	  
		 	 		 
		  LCD.Mag.Heading=(short)COMPAS->fHeadingx;//(short)(Suma/5);
		  LCD.Mag.Min_X=(short)MIN_X;
		  LCD.Mag.Max_X=(short)MAX_X;
		  LCD.Mag.Calib=0x63;
 		  LCD.Task_id=COMPASS_i;
 		  vTaskDelay(50/portTICK_RATE_MS);
		   
		 COMPAS->fHeadingx/=10;
		 
		 Kvadranty|=(1<<(short)COMPAS->fHeadingx);
		  
	} while (Kvadranty!=-1);//0b111111111111111111111111111111111111
	 
	COMPAS->Nasobek1=(double)(abs(MAX_X)+abs(MIN_X))/(abs(MAX_Y)+abs(MIN_Y));
	COMPAS->Nasobek2=(double)(abs(MAX_X)+abs(MIN_X))/(abs(MAX_Z)+abs(MIN_Z));
	
	 COMPAS->X_Offset=MIN_X+MAX_X;	//posunuti
	 COMPAS->Y_Offset=MIN_Y+MAX_Y;
	 COMPAS->Z_Offset=MIN_Z+MAX_Z;
	 
	 COMPAS->X_Offset-=(COMPAS->X_Offset)/2;
	 COMPAS->Y_Offset-=(COMPAS->Y_Offset)/2;
	 COMPAS->Z_Offset-=(COMPAS->Z_Offset)/2;
		
 }