Calcular el acimut de LSM303DLHC

Según una hoja de datos, he preparado el código que permite obtener datos del sensor magnético LSM303DLHC y calcular el acimut de la siguiente manera

Primero, configuré un acelerador y un magnetómetro

void LSM303DLHCAcc_Config(void)
{
    LSM303DLHCAcc_InitStructure.AccFull_Scale = LSM303DLHC_NORMAL_MODE;
    LSM303DLHCAcc_InitStructure.AccOutput_DataRate = LSM303DLHC_ODR_200_HZ;
    LSM303DLHCAcc_InitStructure.Axes_Enable = LSM303DLHC_AXES_ENABLE;
    LSM303DLHCAcc_InitStructure.BlockData_Update = LSM303DLHC_BlockUpdate_Continous;
    LSM303DLHCAcc_InitStructure.Endianness = LSM303DLHC_BLE_MSB;
    LSM303DLHCAcc_InitStructure.High_Resolution = LSM303DLHC_HR_ENABLE;
    LSM303DLHCAcc_InitStructure.Power_Mode = LSM303DLHC_BOOT_NORMALMODE;
    LSM303DLHC_AccInit(&LSM303DLHCAcc_InitStructure);
}

void LSM303DLHCAcc_Read(float *convertedData_Acc)
{
    uint8_t i, XYZ[6] = {0};
    int16_t RawData[3];
    float LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_2g;

    LSM303DLHC_Read(ACC_I2C_ADDRESS,LSM303DLHC_OUT_X_L_A, XYZ, 1);
    LSM303DLHC_Read(ACC_I2C_ADDRESS,LSM303DLHC_OUT_X_H_A, XYZ+1, 1);
    LSM303DLHC_Read(ACC_I2C_ADDRESS,LSM303DLHC_OUT_Y_L_A, XYZ+2, 1);
    LSM303DLHC_Read(ACC_I2C_ADDRESS,LSM303DLHC_OUT_Y_H_A, XYZ+3, 1);
    LSM303DLHC_Read(ACC_I2C_ADDRESS,LSM303DLHC_OUT_Z_L_A, XYZ+4, 1);
    LSM303DLHC_Read(ACC_I2C_ADDRESS,LSM303DLHC_OUT_Z_H_A, XYZ+5, 1);


    for(i=0;i<3;i++)
    {
        RawData[i] = ((int16_t)((uint16_t)XYZ[2*i+1] << 8) + XYZ[2*i]);
    }

    for(i=0;i<3;i++)
    {
        convertedData_Acc[i] = (float) RawData[i]/LSM_Acc_Sensitivity;
    }
}

void LSM303DLHCMag_Config(void)
{
    LSM303DLHCMag_InitStructure.Temperature_Sensor = LSM303DLHC_TEMPSENSOR_DISABLE;
    LSM303DLHCMag_InitStructure.MagFull_Scale = LSM303DLHC_FS_1_9_GA;
    LSM303DLHCMag_InitStructure.MagOutput_DataRate = LSM303DLHC_ODR_220_HZ;
    LSM303DLHCMag_InitStructure.Working_Mode = LSM303DLHC_CONTINUOS_CONVERSION;
    LSM303DLHC_MagInit(&LSM303DLHCMag_InitStructure);
}

void LSM303DLHCMag_Read(float *convertedData_Mag)
{
    uint8_t XYZ[6] = {0};

    LSM303DLHC_Read(MAG_I2C_ADDRESS,LSM303DLHC_OUT_X_H_M, XYZ, 1);
    LSM303DLHC_Read(MAG_I2C_ADDRESS,LSM303DLHC_OUT_X_L_M, XYZ+1, 1);
    LSM303DLHC_Read(MAG_I2C_ADDRESS,LSM303DLHC_OUT_Y_H_M, XYZ+2, 1);
    LSM303DLHC_Read(MAG_I2C_ADDRESS,LSM303DLHC_OUT_Y_L_M, XYZ+3, 1);
    LSM303DLHC_Read(MAG_I2C_ADDRESS,LSM303DLHC_OUT_Z_H_M, XYZ+4, 1);
    LSM303DLHC_Read(MAG_I2C_ADDRESS,LSM303DLHC_OUT_Z_L_M, XYZ+5, 1);

    convertedData_Mag[0] = (float) ((int16_t)(((uint16_t)XYZ[0] << 8) + XYZ[1]) * 1000) / LSM303DLHC_M_SENSITIVITY_XY_1_9Ga;
    convertedData_Mag[1] = (float) ((int16_t)(((uint16_t)XYZ[2] << 8) + XYZ[3]) * 1000) / LSM303DLHC_M_SENSITIVITY_XY_1_9Ga;
    convertedData_Mag[2] = (float) ((int16_t)(((uint16_t)XYZ[4] << 8) + XYZ[5]) * 1000) / LSM303DLHC_M_SENSITIVITY_Z_1_9Ga;
}

Cálculo de la función de acimut de acuerdo con las ecuaciones del apéndice                    

void LSM303DLHCMag_Azimuth (float *roll, float *pitch, float *azimuth, float *convertedData_Mag, float *convertedData_Acc)
{
    float cospitch, sinpitch, cosroll, sinroll, Mz2, My2, Mx2;

    *pitch = asin(-convertedData_Acc[0]); //rho
    *roll = asin(-convertedData_Acc[1]/cos(*pitch)); //gamma

    cospitch = cos(*pitch)*180/M_PI; //cos rho
    sinpitch = sin(*pitch)*180/M_PI; //sin rho
    cosroll = cos(*roll)*180/M_PI;   //cos gamma
    sinroll = sin(*roll)*180/M_PI;   //sin gamma

    Mx2 =  convertedData_Mag[0]*cospitch + convertedData_Mag[2]*sinpitch;
    My2 =  convertedData_Mag[0]*sinroll*sinpitch + convertedData_Mag[1]*cosroll - convertedData_Mag[2]*sinroll*cospitch;
    Mz2 = -convertedData_Mag[0]*cosroll*sinpitch + convertedData_Mag[1]*sinroll + convertedData_Mag[2]*cosroll+cospitch;

    if (Mx2 > 0 && My2 >= 0)
        *azimuth = atan2(My2*M_PI/180, Mx2*M_PI/180)*180/M_PI;

    else if (Mx2 < 0)
        *azimuth = 180.0 + atan2(My2*M_PI/180, Mx2*M_PI/180)*180/M_PI;

    else if (Mx2 > 0 && My2 <= 0)
        *azimuth = 360.0 + atan2(My2*M_PI/180, Mx2*M_PI/180)*180/M_PI;

    else if (Mx2 == 0 && My2 < 0)
        *azimuth = 90.0;

    else if (Mx2 == 0 && My2 > 0)
        *azimuth = 270.0;
}

Mi problema es que obtengo NaN al calcular los valores de roll y pitch . ¿Qué he hecho mal al leer los valores del acelerómetro?