Czujnik, hmc5883l, kompensacja, przechylen (C w avr)

Witam.

Czy ma ktoś pomysł jak mógłbym zrobić kompensację przechyleń w czujniku HMC5883L to co znalazłem na necie nie za bardzo działa.
Układ pochylany będzie tylko w jednej osi, czyli wokół osi y. Znany jest kąt przechylenia i wartości x,y,z z kompasu.
Nie za bardzo rozumiem pomiary z x, wg. mnie powinno być tak, że wartość x jest taka sama dla przechylenia +30 stopni i -30 stopni. Niestety tak nie jest.
W załączniku jest plik z pomiarami w excelu, pomiary były robione z płytką skierowaną cały czas w tą samą stronę tak więc kąt (kompasu) powinien być taki sam cały czas.

Ja przeliczałem to w ten sposób:

int getHeadingHMC_TiltComp(float pitch, float roll)
{
	uint8_t raw_xh = 0;
	uint8_t raw_xl = 0;
	
	uint8_t raw_yh = 0;
	uint8_t raw_yl = 0;
	
	uint8_t raw_zh = 0;
	uint8_t raw_zl = 0;
	long x,y,z;

	i2c_start(HMC5883L_WRITE);
	i2c_write(0x03); //set pointer to X-axis MSB
	i2c_stop();

	i2c_rep_start(HMC5883L_READ); 

	raw_xh = i2c_readAck();
	raw_xl = i2c_readAck();

	raw_zh = i2c_readAck();
	raw_zl = i2c_readAck();

	raw_yh = i2c_readAck();
	raw_yl = i2c_readNak();

	i2c_stop();
	
	x = raw_xl|(raw_xh<<8);
	y = raw_yl|(raw_yh<<8);
	z = raw_zl|(raw_zh<<8);
	
	//float err = 0.5 * pitch;
	
	double Xh = (double)x * cos(M_PI*pitch/180) + (double)z * sin(M_PI*pitch/180);
	double Yh = (double)y;
	int headingDegrees = (double)(atan2(Yh,Xh)* 57 + 180);

	char buf[40];
	sprintf(buf,"%4.i : %4.i : %4.i : %4.i\n",(int)x, (int)y, (int)z, (int)pitch);
	USART_send_string(buf);

	return headingDegrees;
}

Ale nie za bardzo się to sprawdziło.

Pomyślałem żeby zrobić kalibrację czujnika ale nie bardzo wiem jak ona działa. Mam taki kod:

void HMC5883L_ScaleFactorCalib(void)
{
	int16_t raw_x = 0;
	int16_t raw_y = 0;
	int16_t raw_z = 0;

	int err=i2c_start(HMC5883L_WRITE); 
	i2c_write(0x00); // set pointer to CRA
	i2c_write(0x11); // Self test operation
	i2c_stop();
	
	i2c_start(HMC5883L_WRITE); 
	i2c_write(0x01); // set pointer to CRA
	i2c_write(0x20); // Self test operation
	i2c_stop();
	
	i2c_start(HMC5883L_WRITE); 
	i2c_write(0x02); // set pointer to measurement mode
	i2c_write(0x01); // Single measurment mode
	i2c_stop();


	i2c_start(HMC5883L_WRITE);
	i2c_write(0x03); //set pointer to X-axis MSB
	i2c_stop();

	i2c_rep_start(HMC5883L_READ); 

	raw_x = ((uint16_t)i2c_readAck())<<8;
	raw_x |= i2c_readAck();

	raw_z = ((uint16_t)i2c_readAck())<<8;
	raw_z |= i2c_readAck();

	raw_y = ((uint16_t)i2c_readAck())<<8;
	raw_y |= i2c_readNak();

	i2c_stop();
	
	char buf[40];
	sprintf(buf,"Calib: %4.i : %4.i : %4.i\n",raw_x, raw_z, raw_y);
	USART_send_string(buf);
	
	return 0;
}	

Dostaję jakieś tam wyniki ale nie wiem co z nimi zrobić :).