Чтение данных магнитометра MPU9250 AK8963

#define MPU_ADDRESS 0x68                            //MPU9250 adress
#define I2C_FILE "/dev/i2c-1"                       //Locale i2c file
 
#define MPU_POWER1 0x6b
#define MPU_POWER2 0x6c

int main(){
    int16_t gx, gy, gz;
    int16_t ax, ay, az;
    int16_t mx, my, mz;
    //MPU9250
    fd = open(I2C_FILE, O_RDONLY | O_WRONLY);
    ioctl(fd, I2C_SLAVE, MPU_ADDRESS);
    //AK8963
    fd_ak8963 = open(I2C_FILE, O_RDONLY | O_WRONLY);
    ioctl(fd_ak8963, I2C_SLAVE, 0x0C);
 
    HxMagnetometer *magn = new HxMagnetometer(fd);
 
    while(1){
        magn->magn_get_raw(&mx, &my, &mz);
        printf("mx = %d my = %d mz = %d\n", mx, my, mz);
        printf("%s\n", _buffer);
        //printf("GYRO: x = %d y = %d z = %d\n", gyro->gyro_get_x_raw(), gyro->gyro_get_y_raw(), gyro->gyro_get_z_raw());
        delay(100);
    }
 
    return 0;
}

#include "HxMagnetometer.hpp"
 
HxMagnetometer::HxMagnetometer(int32_t id){
    device_id = id;
 
    i2c_smbus_write_byte_data(device_id, AK8963_CNTL, 0x00);
    std::cout << "Power down magnetometer\n";
 
    i2c_smbus_write_byte_data(device_id, AK8963_CNTL, 0x0F);
    std::cout << "Enter Fuse ROM access mode\n";
    delay(10);
 
    std::cout << "X: " << i2c_smbus_read_byte_data(device_id, AK8963_ASAX) << std::endl;
    std::cout << "Y: " << i2c_smbus_read_byte_data(device_id, AK8963_ASAY) << std::endl;
    std::cout << "Z: " << i2c_smbus_read_byte_data(device_id, AK8963_ASAZ) << std::endl;
 
    i2c_smbus_write_byte_data(device_id, AK8963_CNTL, MFS_16BITS << 4 | 0x02);
    std::cout << "Set magnetometer data resolution and sample ODR:" << std::endl;
 
    i2c_smbus_write_byte_data(device_id, AK8963_CNTL, 0x00);
    std::cout << "Power down magnetometer\n";
 
 
}
 
void HxMagnetometer::magn_get_raw(int16_t* _x_magn_raw, int16_t* _y_magn_raw, int16_t* _z_magn_raw){
 
    i2c_smbus_write_byte_data(device_id, AK8963_CNTL, 0x0F);
    if (i2c_smbus_read_byte_data(device_id, AK8963_ST1) & 0x01)
    {
        std::cout << "Read status: ok\n";
        *_x_magn_raw = magn_get_x_raw();
        //Get y raw: 0, 1, 0
        *_y_magn_raw = magn_get_y_raw();
        //Get x raw: 0, 0, 1
        *_z_magn_raw = magn_get_z_raw();
    }
    i2c_smbus_write_byte_data(device_id, AK8963_CNTL, 0x00);
    //Get x raw: 1, 0, 0
    // i2c_smbus_read_byte_data(device_id, AK8963_ST2);
    // i2c_smbus_write_byte_data(device_id, AK8963_CNTL1,AK8963_FUSE_ROM);
 
    // i2c_smbus_write_byte_data(device_id, AK8963_CNTL1,AK8963_PWR_DOWN);
}
 
//Get x raw
int16_t HxMagnetometer::magn_get_x_raw(void){
    return BITWISE_SHIFT(i2c_smbus_read_byte_data(device_id, 0x03), i2c_smbus_read_byte_data(device_id, 0x04));
}
 
//Get y raw
int16_t HxMagnetometer::magn_get_y_raw(void){
    return BITWISE_SHIFT(i2c_smbus_read_byte_data(device_id, 0x05), i2c_smbus_read_byte_data(device_id, 0x06));
}
 
//Get z raw
int16_t HxMagnetometer::magn_get_z_raw(void){
    return BITWISE_SHIFT(i2c_smbus_read_byte_data(device_id, 0x07), i2c_smbus_read_byte_data(device_id, 0x08));
}