How to calculate the Yaw using the magnetometer.

Other Hardware Sensors
1

I have a problem when trying to calculate Yaw(MAg_Heading ). I get bad values, as i consider when they are printed on Serial. I am using Ardupilot 2.5.

#define RADIANI_IN_GRADE         57.32484
#define PI						 3.1415



#define ToRad(x) (x*0.01745329252)  // *pi/180
#define ToDeg(x) (x*57.2957795131)  // *180/pi

AP_Compass_HMC5843 compass;
APM_RC_APM2 radio;

xQueueHandle xQueueIMUData;
xQueueHandle xQueueRadioData;

xTaskHandle xTaskCalculate;

void initTimer()
{
	TIMSK1 = 0x01; // enabled global and timer overflow interrupt;
	TCCR1A = 0x00; // normal operation page 148 (mode0);
	TCNT1 = 0x000; // set initial value to remove time error (16bit counter register)
	TCCR1B = 0x04; // start timer/ set clock
}

volatile unsigned long timer_overflow = 0;
volatile uint16_t last_count = 0;
unsigned long updateCounter()
{
	if(TCNT1 < last_count) //overflow.
		timer_overflow++;
	last_count = TCNT1;
	return (timer_overflow*COUNTER_MAX + (unsigned long)TCNT1);
}

float comp_filter(float *accDataX, float *accDataY, float *gyroDataX, float *gyroDataY)
{
	*gyroDataX = 0.95 * *gyroDataX + 0.05 * *accDataX;
	*gyroDataY = 0.95 * *gyroDataY + 0.05 * *accDataY;

	
}




void vTaskCalculate(void* pvParameters)
{
	float imu_data[IMU_DATA_SIZE];
	float out_data[5];
	unsigned int time_past = 0;
	float new_time = 0;
	unsigned int new_time1 = 0;
	float fXg=0,fYg=0,fZg=0,fXm = 0, fYm = 0, MAG_Heading = 0;
	float x=0;
	

	float temp_out_data[3];
	
	enum
	{
		ACC_roll,
		ACC_pitch,
		G_roll,
		G_pitch,
		G_yaw
	};

	enum 
	{
		Gt1,
		Gt2,
		Gt3
	};
	
	

	while(1)
	{

			if(xQueueReceive(xQueueIMUData,&imu_data,RECEIVE_DELAY))
			{
				//new_time = millis();
				//new_time = new_time - time_past;
				new_time1 = xTaskGetTickCount();
				/*Serial.printf("new_time %u\t",new_time1);*/
				
				new_time = float(new_time1 - time_past); 
				new_time = new_time / 1000;

				fXg=imu_data[ACC_X]*0.05+fXg*0.95;
				fZg=imu_data[ACC_Z]*0.05+fZg*0.95;
				fYg=imu_data[ACC_Y]*0.05+fYg*0.95;

				 //out_data[ACC_roll] = RADIANI_IN_GRADE*atan((float)imu_data[ACC_Y]/sqrt(pow((float)imu_data[ACC_X],2) + pow((float)imu_data[ACC_Z],2)));
				 //out_data[ACC_pitch] = RADIANI_IN_GRADE*atan((float)-imu_data[ACC_X]/sqrt(pow((float)imu_data[ACC_Y],2) + pow((float)imu_data[ACC_Z],2)));	
				
				out_data[ACC_roll]		= (atan2(-imu_data[ACC_Z] , -imu_data[ACC_Y]))*RADIANI_IN_GRADE - 90.0; 
				out_data[ACC_pitch]		= (atan2(-imu_data[ACC_X] , sqrt(imu_data[ACC_Y]*imu_data[ACC_Y]+imu_data[ACC_Z]*imu_data[ACC_Z])))*RADIANI_IN_GRADE; 
				temp_out_data[Gt1]		= imu_data[GYRO_X]*new_time;
				temp_out_data[Gt2]		= imu_data[GYRO_Y]*new_time;
				temp_out_data[Gt3]		= imu_data[GYRO_Z]*new_time;

				temp_out_data[Gt1] *= RADIANI_IN_GRADE; 
				temp_out_data[Gt2] *= RADIANI_IN_GRADE;
				temp_out_data[Gt3] *= RADIANI_IN_GRADE;
				

				out_data[G_roll]	+= temp_out_data[Gt1]; //unghiul din urma				
				out_data[G_pitch]	+= temp_out_data[Gt2];
				out_data[G_yaw]		+= temp_out_data[Gt3];


				comp_filter(&out_data[ACC_roll],&out_data[ACC_pitch],&out_data[G_roll],&out_data[G_pitch]);

				//fXm=imu_data[MAG_X]*cos(out_data[G_pitch])+imu_data[MAG_Y]*sin(out_data[G_roll])*sin(out_data[G_pitch])+imu_data[MAG_Z]*cos(out_data[G_roll])*sin(out_data[G_pitch]);
				//fYm=imu_data[MAG_Z]*cos(out_data[G_roll])-imu_data[MAG_Y]*sin(out_data[G_roll]);

																											fXm=imu_data[MAG_X]*cos(out_data[G_pitch])+imu_data[MAG_Z]*sin(out_data[G_pitch])*sin(out_data[G_roll])+imu_data[MAG_Z]*cos(out_data[G_roll])*sin(out_data[G_pitch]);
				fYm=-imu_data[MAG_Y]*cos(out_data[G_roll])+imu_data[MAG_Z]*sin(out_data[G_roll]);


				 //heading = (float)atan2( (-magneto.Y*cos(rezultat[ruliu]) + magneto.Z*sin(rezultat[ruliu]) ) , (magneto.X*cos(rezultat[tangaj]) + magneto.Z*sin(rezultat[tangaj])*sin(rezultat[ruliu])+ magneto.Z*sin(rezultat[tangaj])*cos(rezultat[ruliu])) ) * RADIANI_IN_GRADE;
    

				MAG_Heading=atan2(fYm,fXm)*RADIANI_IN_GRADE;

				/*if(out_data[ACC_pitch]<=180) out_data[ACC_pitch]=-1.0*out_data[ACC_pitch];
				

				/*if(out_data[ACC_pitch]<=180) out_data[ACC_pitch]=-1.0*out_data[ACC_pitch];
          
				if(out_data[ACC_pitch]>180) out_data[ACC_pitch]=360-out_data[ACC_pitch];*/

				time_past = new_time1;
				//Serial.printf("Diferenta de timp %4.3f\t",new_time);

				new_time = 0;
				
				/*Serial.printf("Incercare=%d\n",x);
*/
				
					
					//Serial.printf("out_data[ACC_roll]= %4.3f\t out_data[ACC_pitch]= %4.3f\t out_data[G_roll]= %4.3f\t out_data[G_pitch]= %4.3f\t out_data[G_yaw]= %4.3f\r\n",out_data[ACC_roll],out_data[ACC_pitch],out_data[G_roll],out_data[G_pitch],out_data[G_yaw]);
					Serial.printf("roll= %4.2f\t pitch = %4.2f, MAG_Heading = %4.3f\r\n",out_data[G_roll],out_data[G_pitch],MAG_Heading);	


				
				vTaskPrioritySet(xTaskCalculate, tskIDLE_PRIORITY-2);
				

				}

	}

}

And some values.

MAG_Heading = -8.798
MAG_Heading = -8.774
MAG_Heading = -8.959
MAG_Heading = -8.888
MAG_Heading = -8.905
MAG_Heading = -9.002
MAG_Heading = -9.004
MAG_Heading = -8.998
MAG_Heading = -8.985
MAG_Heading = -8.953
MAG_Heading = -8.957
MAG_Heading = -8.955
MAG_Heading = -8.964
MAG_Heading = -8.950
MAG_Heading = -8.805
MAG_Heading = -8.973
MAG_Heading = -8.967
MAG_Heading = -8.850
MAG_Heading = -9.004
MAG_Heading = -8.893
MAG_Heading = -8.910
MAG_Heading = -9.073
MAG_Heading = -8.925
MAG_Heading = -9.041
MAG_Heading = -9.030
MAG_Heading = -8.897
MAG_Heading = -8.887
MAG_Heading = -8.898
MAG_Heading = -8.894
MAG_Heading = -8.889
MAG_Heading = -8.884
MAG_Heading = -8.843
MAG_Heading = -8.848
MAG_Heading = -8.979
MAG_Heading = -8.877
MAG_Heading = -8.803
MAG_Heading = -8.831
MAG_Heading = -8.946
MAG_Heading = -8.937
MAG_Heading = -8.881
MAG_Heading = -8.896
MAG_Heading = -8.946
MAG_Heading = -8.824
MAG_Heading = -8.925
MAG_Heading = -8.924
MAG_Heading = -8.889
MAG_Heading = -8.912
MAG_Heading = -8.797
MAG_Heading = -8.827
MAG_Heading = -9.093
MAG_Heading = -8.980
MAG_Heading = -9.098
MAG_Heading = -9.086
MAG_Heading = -8.958
MAG_Heading = -8.971
MAG_Heading = -9.092
MAG_Heading = -9.076
MAG_Heading = -8.983
MAG_Heading = -9.091
MAG_Heading = -9.098
MAG_Heading = -9.128
MAG_Heading = -8.997
MAG_Heading = -9.026
MAG_Heading = -9.150

MAG_Heading = -9.119
MAG_Heading = -9.011
MAG_Heading = -9.138
MAG_Heading = -9.122
MAG_Heading = -8.966
MAG_Heading = -8.814
MAG_Heading = -9.081
MAG_Heading = -9.109
MAG_Heading = -9.128
MAG_Heading = -9.020
MAG_Heading = -9.130
MAG_Heading = -9.104
MAG_Heading = -8.990
MAG_Heading = -8.983
MAG_Heading = -9.089
MAG_Heading = -9.107
MAG_Heading = -9.163
MAG_Heading = -9.187
MAG_Heading = -9.173
MAG_Heading = -9.180
MAG_Heading = -9.187
MAG_Heading = -9.051
MAG_Heading = -9.126
MAG_Heading = -9.112
MAG_Heading = -8.999
MAG_Heading = -8.992
MAG_Heading = -8.977
MAG_Heading = -8.999
MAG_Heading = -9.165
MAG_Heading = -9.195
MAG_Heading = -9.189
MAG_Heading = -9.148
MAG_Heading = -8.990
MAG_Heading = -8.986
MAG_Heading = -9.121
MAG_Heading = -9.089
MAG_Heading = -9.090
MAG_Heading = -8.972
MAG_Heading = -8.966
MAG_Heading = -8.973
MAG_Heading = -9.096
MAG_Heading = -8.987
MAG_Heading = -9.103
MAG_Heading = -9.103
MAG_Heading = -8.978
MAG_Heading = -8.971
MAG_Heading = -9.108
MAG_Heading = -9.087
MAG_Heading = -8.965
MAG_Heading = -8.790
MAG_Heading = -9.046
MAG_Heading = -9.047
MAG_Heading = -9.085
MAG_Heading = -9.091
MAG_Heading = -8.964
MAG_Heading = -8.972
MAG_Heading = -8.973
MAG_Heading = -8.980
MAG_Heading = -9.076
MAG_Heading = -9.089
MAG_Heading = -8.989
MAG_Heading = -9.000
MAG_Heading = -9.114
MAG_Heading = -9.116
MAG_Heading = -9.122
MAG_Heading = -9.022
MAG_Heading = -9.017
MAG_Heading = -9.025
MAG_Heading = -8.832
MAG_Heading = -8.707
MAG_Heading = -8.817
MAG_Heading = -8.845
MAG_Heading = -8.840
MAG_Heading = -8.960
MAG_Heading = -8.969
MAG_Heading = -8.847
MAG_Heading = -9.007
MAG_Heading = -1.614
MAG_Heading = 1.141
MAG_Heading = 3.750

2

Have you calibrated the magnetometer? It is almost always necessary. Here is a link to my favorite procedure: Sailboat Instruments: Improved magnetometer calibration (Part 1)