Датчик компаса не дает правильные значения

Question

Я сейчас работаю с датчиком компаса HMC5883L от Honeywell, и я не могу понять, как получить правильные значения.Я реализовал оси X, Y, Z в своем коде и понял, что формулой, которую я использую для измерения, является формула atan2 (y, x) https://en.wikipedia.org/wiki/Atan2.Но у меня есть 3 оси ... Кто-нибудь знает, что формула для 3d-измерения в коде C?

См. Мой код ниже:

#include <unistd.h> 
#include <stdio.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>
#include <math.h>

int main()

{

unsigned short int xl, yl, xh, yh, zh, zl;
short int x,y,z;    // size of a short is 2 bytes !!
int fd;
float declination = 1;
double angle;


fd=wiringPiI2CSetup(0x1E);
usleep(50000);  //delay 0,5 seconds


wiringPiI2CWriteReg8(fd, 0x00, 0x70);
wiringPiI2CWriteReg8(fd, 0x01, 0xA0);
wiringPiI2CWriteReg8(fd, 0x02, 0x01);

while(1)
{

    // start a new single measurement 
    wiringPiI2CWriteReg8(fd, 0x02, 0x01);
    usleep(50000);  //delay 0,5 seconds

    xh = wiringPiI2CReadReg8(fd, 0x03); // x msb
    xl = wiringPiI2CReadReg8(fd, 0x04); // x lsb
    zh = wiringPiI2CReadReg8(fd, 0x05); // z msb
    zl = wiringPiI2CReadReg8(fd, 0x06); // z lsb
    yh = wiringPiI2CReadReg8(fd, 0x07); // y msb
    yl = wiringPiI2CReadReg8(fd, 0x08); // y lsb


    x = (xh << 8) | xl;
    y = (yh << 8) | yl;
    z = (zh << 8) | zl;

    angle = atan2((double)y, (double)x) + declination;
    if(angle > 2*pi) angle = angle - 2*M_PI;
    if(angle < 0) angle = angle + 2*M_PI;
    float heading_grade = angle * 180/M_PI;

    printf("\nAngle is: %f \n",heading_grade); 
    printf("X is: %d \n", x);
    printf("Y is: %d \n", y);
    printf("Z is: %d \n", z);

    usleep(50000); // delay 5 seconds  
}

return 0;


}

Вывод, который я получаю, составляет около+/- 320 градусов, когда датчик горизонтально закручен.Значение Y остается неизменным, Z изменяется от -50 до +900, когда я перемещаю датчик, а X также изменяется от -50 до +700 во время движения.

Помощь будет очень признательнапотому что я не могу понять это.