Java port miscalculating magnetometer values

[gjwo]

Hi Kris, it seems I spoke too soon, I still have some work to do!

It looks like my magnetometer values are wrong, there are a few things I would like your opinions on please. Q1. When moving the sensor during calibration is this a figure of 8 in the horizontal plane only or in all planes? Q2. My calibration values for hard iron are - deviceBias: x: -010.279 y: +024.178 z: +013.916, does this look OK? Q3. My calibration values for soft iron are - deviceScaling: x: +001.153 y: +000.894 z: +000.986 does this look OK? Q4. I have magCalibration: x: +000.176 y: +000.188 z: +000.141 does this look OK? Q5. My output looks line this M x: +014.307 y: -046.344 z: -016.345, which to my mind looks wrong, do you agree? Q6. If Q2-Q4 looks OK, and Q5 data looks wrong, then I think I have made an error in my update routine, or in configuring for normal operation - do you agree?

Java code fragments below, if you can see the problem please point it out for me, I know it's not your language but the maths should be the same as your code, but probably isn't somewhere, but I can't spot it.

    public void configure() throws InterruptedException, IOException {
        if (debugLevel() >=3) System.out.println("initAK8963");
        // First extract the factory calibration for each magnetometer axis

        ro.writeByteRegister(Registers.AK8963_CNTL1,(byte) 0x00); // Power down magnetometer
        Thread.sleep(10);
        ro.writeByteRegister(Registers.AK8963_CNTL1, (byte)0x0F); // Enter Fuse ROM access bits
        Thread.sleep(10);
        byte rawData[] = ro.readByteRegisters(Registers.AK8963_ASAX, 3);  // Read the x-, y-, and z-axis calibration values
        this.magCalibration = new Data3f(((float)(rawData[0] - 128))/256f + 1f,   // Return x-axis sensitivity adjustment values, etc.
                                 ((float)(rawData[1] - 128))/256f + 1f,
                                 ((float)(rawData[2] - 128))/256f + 1f);

        ro.writeByteRegister(Registers.AK8963_CNTL1, (byte)0x00); // Power down magnetometer
        Thread.sleep(10);
        // Configure the magnetometer for continuous read and highest resolution
        // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL1 register,
        // and enable continuous bits data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
        ro.writeByteRegister(Registers.AK8963_CNTL1, (byte)(MagScale.MFS_16BIT.bits | magMode.bits)); // Set magnetometer data resolution and sample ODR ####16bit already shifted
        Thread.sleep(10);
        if (debugLevel() >=3) printState();
        if (debugLevel() >=3) System.out.println("End initAK8963");
    }

    public void updateData()
        { //loop() - readMagData
        byte dataReady = (byte)(ro.readByteRegister(Registers.AK8963_ST1) & 0x01); //DRDY - Data ready bit0 1 = data is ready
        if (dataReady == 0) return; //no data ready

        // data is ready, read it NB bug fix here read was starting from ST1 not XOUT_L
        byte[] buffer = ro.readByteRegisters(Registers.AK8963_XOUT_L, 7); //6 data bytes x,y,z 16 bits stored as little Endian (L/H)

        // Check if magnetic sensor overflow set, if not then report data   
        //roAK.readByteRegister(Registers.AK8963_ST2);// Data overflow bit 3 and data read error status bit 2
        byte status2 = buffer[6]; // Status2 register must be read as part of data read to show device data has been read
        if((status2 & 0x08) == 0) //bit3 HOFL: Magnetic sensor overflow is normal (no Overflow), data is valid
        { 
            lastRawMagX = (short) ((buffer[1] << 8) | buffer[0]); // Turn the MSB and LSB into a signed 16-bit value
            lastRawMagY = (short) ((buffer[3] << 8) | buffer[2]); // Data stored as little Endian
            lastRawMagZ = (short) ((buffer[5] << 8) | buffer[4]);

            //the stored calibration results is applied here as there is no hardware correction stored in the hardware via calibration 
            lastCalibratedReading = scale(new TimestampedData3f( lastRawMagX*magScale.res*magCalibration.getX() - getDeviceBias().getX(),
                                                                     lastRawMagY*magScale.res*magCalibration.getY() - getDeviceBias().getY(),
                                                                     lastRawMagZ*magScale.res*magCalibration.getZ() - getDeviceBias().getZ()));
            this.addValue(lastCalibratedReading);
        }
    }

    enum GyrScale
    {   //Gyroscope Configuration register 1B 27 bits 4:3
        GFS_250DPS((byte)0x00,250),  //Gyro Full Scale Select: 250dps
        GFS_500DPS((byte)0x08,500),  //Gyro Full Scale Select: 500dps
        GFS_1000DPS((byte)0x10,1000),//Gyro Full Scale Select: 1000dps
        GFS_2000DPS((byte)0x18,2000);//Gyro Full Scale Select: 2000dps

        final byte bits;
        final int minMax;
        final static byte bitMask = (byte) 0x18;

        GyrScale(byte bits, int minMax)
        {
            this.bits = bits;
            this.minMax = minMax;
        }

        public float getRes() {return ((float)minMax)/32768.0f;}
    }

[kriswiner]

You need to sample the entire 3D response surface. Please look here:

https://github.com/kriswiner/MPU-6050/wiki/Simple-and-Effective-Magnetometer -Calibration

Kris -----Original Message----- From: Graham Wood [mailto:notifications@github.com] Sent: November 23, 2016 8:46 AM To: kriswiner/MPU-9250 Subject: [kriswiner/MPU-9250] Java port miscalculating magnetometer values (#90)

Hi Kris, it seems I spoke too soon, I still have some work to do!

It looks like my magnetometer values are wrong, there are a few things I would like your opinions on please. Q1. When moving the sensor during calibration is this a figure of 8 in the horizontal plane only or in all planes? Q2. My calibration values for hard iron are - deviceBias: x: -010.279 y: +024.178 z: +013.916, does this look OK? Q3. My calibration values for soft iron are - deviceScaling: x: +001.153 y: +000.894 z: +000.986 does this look OK? Q4. I have magCalibration: x: +000.176 y: +000.188 z: +000.141 does this look OK? Q5. My output looks line this M x: +014.307 y: -046.344 z: -016.345, which to my mind looks wrong, do you agree? Q6. If Q2-Q4 looks OK, and Q5 data looks wrong, then I think I have made an error in my update routine, or in configuring for normal operation - do you agree?

Java code fragments below, if you can see the problem please point it out for me, I know it's not your language but the maths should be the same as your code, but probably isn't somewhere, but I can't spot it.

public void configure() throws InterruptedException, IOException {
    if (debugLevel() >=3) System.out.println("initAK8963");
    // First extract the factory calibration for each magnetometer axis

    ro.writeByteRegister(Registers.AK8963_CNTL1,(byte) 0x00); // Power

down magnetometer Thread.sleep(10); ro.writeByteRegister(Registers.AK8963_CNTL1, (byte)0x0F); // Enter Fuse ROM access bits Thread.sleep(10); byte rawData[] = ro.readByteRegisters(Registers.AK8963_ASAX, 3); // Read the x-, y-, and z-axis calibration values this.magCalibration = new Data3f( ((float)(rawData[0] - 128))/256f + 1f, // Return x-axis sensitivity adjustment values, etc.

((float)(rawData[1] - 128))/256f + 1f,

((float)(rawData[2] - 128))/256f + 1f);

    ro.writeByteRegister(Registers.AK8963_CNTL1, (byte)0x00); // Power

down magnetometer Thread.sleep(10); // Configure the magnetometer for continuous read and highest resolution // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL1 register, // and enable continuous bits data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates ro.writeByteRegister(Registers.AK8963_CNTL1, (byte)(MagScale.MFS_16BIT.bits | magMode.bits)); // Set magnetometer data resolution and sample ODR ####16bit already shifted Thread.sleep(10); if (debugLevel() >=3) printState(); if (debugLevel() >=3) System.out.println("End initAK8963"); }

public void updateData()
    { //loop() - readMagData
    byte dataReady = (byte)(ro.readByteRegister(Registers.AK8963_ST1) &

0x01); //DRDY - Data ready bit0 1 = data is ready if (dataReady == 0) return; //no data ready

    // data is ready, read it NB bug fix here read was starting from ST1

not XOUT_L byte[] buffer = ro.readByteRegisters(Registers.AK8963_XOUT_L, 7); //6 data bytes x,y,z 16 bits stored as little Endian (L/H)

    // Check if magnetic sensor overflow set, if not then report data

    //roAK.readByteRegister(Registers.AK8963_ST2);// Data overflow bit 3

and data read error status bit 2 byte status2 = buffer[6]; // Status2 register must be read as part of data read to show device data has been read if((status2 & 0x08) == 0) //bit3 HOFL: Magnetic sensor overflow is normal (no Overflow), data is valid { lastRawMagX = (short) ((buffer[1] << 8) | buffer[0]); // Turn the MSB and LSB into a signed 16-bit value lastRawMagY = (short) ((buffer[3] << 8) | buffer[2]); // Data stored as little Endian lastRawMagZ = (short) ((buffer[5] << 8) | buffer[4]);

        //the stored calibration results is applied here as there is

no hardware correction stored in the hardware via calibration lastCalibratedReading = scale(new TimestampedData3f( lastRawMagX_magScale.res_magCalibration.getX() - getDeviceBias().getX(),

lastRawMagY_magScale.res_magCalibration.getY() - getDeviceBias().getY(),

lastRawMagZ_magScale.res_magCalibration.getZ() - getDeviceBias().getZ())); this.addValue(lastCalibratedReading); } }

enum GyrScale
{   //Gyroscope Configuration register 1B 27 bits 4:3
    GFS_250DPS((byte)0x00,250),  //Gyro Full Scale Select: 250dps
    GFS_500DPS((byte)0x08,500),  //Gyro Full Scale Select: 500dps
    GFS_1000DPS((byte)0x10,1000),//Gyro Full Scale Select: 1000dps
    GFS_2000DPS((byte)0x18,2000);//Gyro Full Scale Select: 2000dps

    final byte bits;
    final int minMax;
    final static byte bitMask = (byte) 0x18;

    GyrScale(byte bits, int minMax)
    {
        this.bits = bits;
        this.minMax = minMax;
    }

    public float getRes() {return ((float)minMax)/32768.0f;}
}

You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU-9250/issues/90 , or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qhPx05JOTY62h5F-xWMtO U95VOMZks5rBG24gaJpZM4K6z7J . https://github.com/notifications/beacon/AGY1qosqcGbOMDPItdIPzyZ0ZzUAJezsks5 rBG24gaJpZM4K6z7J.gif

[gjwo]

Kris, your views on what's normal Q2-Q4 are what I really need, Q5 is just confirmation of what I think you said on an earlier thread, Anything else would be a bonus. I't best seen online not via email as the formatting is better.

[kriswiner]

Normal depends on your particular sensor and location. I really cannot answer these questions.

-----Original Message----- From: Graham Wood [mailto:notifications@github.com] Sent: November 23, 2016 1:30 PM To: kriswiner/MPU-9250 Cc: Kris Winer; Comment Subject: Re: [kriswiner/MPU-9250] Java port miscalculating magnetometer values (#90)

Kris, your views on what's normal Q2-Q4 are what I really need, Q5 is just confirmation of what I think you said on an earlier thread, Anything else would be a bonus. I't best seen online not via email as the formatting is better.

You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU-9250/issues/90#issuecomment-262634599 , or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qjJLG7BgkM4B5H_iXQkYK 8Ib2SGGks5rBLBzgaJpZM4K6z7J . https://github.com/notifications/beacon/AGY1qhNn6GnxCN-TvxVK8G-W-y56L52Fks5 rBLBzgaJpZM4K6z7J.gif

[gjwo]

Kris, I have found the problem in update data as expected. It was another nasty "Java not supporting unsigned integers" issue. Java was auto casting the lower byte to int and extending the top bit (as a sign bit), I just needed to mask to fix this. see code below. (#KW are references to your code with line numbers)

My line graphs for yaw pitch and roll are now fine but for one thing, when I rotate clockwise the yaw angles rotate anticlockwise, do you have any idea what sort of thing can cause this? In all other respects the movements seem to be handled correctly, 90 degree turns are shown correctly (but for direction). Would I be right to be looking in the sensor fusion calculations or might it be caused in the magnetometer code?

    public void updateData()
    { //#KW loop() L490 calls - readMagData L812
        byte dataReady = (byte)(ro.readByteRegister(Registers.AK8963_ST1) & 0x01); //DRDY - Data ready bit0 1 = data is ready
        if (dataReady == 0) return; //no data ready

        // #KW 494 readMagData - data is ready, read it NB bug fix here read was starting from ST1 not XOUT_L
        byte[] buffer = ro.readByteRegisters(Registers.AK8963_XOUT_L, 7); // #KW L815 6 data bytes x,y,z 16 bits stored as little Endian (L/H)
        // Check if magnetic sensor overflow set, if not then report data   
        //roAK.readByteRegister(Registers.AK8963_ST2);// Data overflow bit 3 and data read error status bit 2
        byte status2 = buffer[6]; // Status2 register must be read as part of data read to show device data has been read
        if((status2 & 0x08) == 0) //#KW 817 bit3 HOFL: Magnetic sensor overflow is normal (no Overflow), data is valid
        {   //#KW L818-820
            lastRawMagX = (short) ((buffer[1] << 8) | (buffer[0]&0xFF)); // Turn the MSB and LSB into a signed 16-bit value
            lastRawMagY = (short) ((buffer[3] << 8) | (buffer[2]&0xFF)); // Data stored as little Endian
            lastRawMagZ = (short) ((buffer[5] << 8) | (buffer[4]&0xFF)); // mask to prevent sign extension in LSB (bug fix)

            //the stored calibration results is applied here as there is no hardware correction stored in the hardware via calibration
            //#KW L496-L501. scale() does the multiplication by magScale L499-501
            lastCalibratedReading = scale(new TimestampedData3f(    lastRawMagX*magScale.res*magCalibration.getX() - getDeviceBias().getX(),
                                                                    lastRawMagY*magScale.res*magCalibration.getY() - getDeviceBias().getY(),
                                                                    lastRawMagZ*magScale.res*magCalibration.getZ() - getDeviceBias().getZ()));
            this.addValue(lastCalibratedReading); //store the result
        }
    }

[gjwo]

Found some missing minus signs in calling MadgwickQuaternionUpdate, now rotating in the correct direction! MadgwickQuaternionUpdate(-ax, ay, az, gxPI/180.0f, -gyPI/180.0f, -gz*PI/180.0f, my, -mx, mz); #KW L521

I am doing more tests!

[kriswiner]

You probably need to think about how you are passing the sensor data to the fusion algorithm. You have to use a consistent NED orientation if you want the YPR to respond intuitively.

-----Original Message----- From: Graham Wood [mailto:notifications@github.com] Sent: November 24, 2016 6:38 AM To: kriswiner/MPU-9250 Cc: Kris Winer; Comment Subject: Re: [kriswiner/MPU-9250] Java port miscalculating magnetometer values (#90)

Kris, I have found the problem in update data as expected. It was another nasty "Java not supporting unsigned integers" issue. Java was auto casting the lower byte to int and extending the top bit (as a sign bit), I just needed to mask to fix this. see code below. (#KW are references to your code with line numbers)

My line graphs for yaw pitch and roll are now fine but for one thing, when I rotate clockwise the yaw angles rotate anticlockwise, do you have any idea what sort of thing can cause this? In all other respects the movements seem to be handled correctly, 90 degree turns are shown correctly (but for direction). Would I be right to be looking in the sensor fusion calculations or might it be caused in the magnetometer code?

public void updateData()
{ //#KW loop() L490 calls - readMagData L812
    byte dataReady = (byte)(ro.readByteRegister(Registers.AK8963_ST1) &

0x01); //DRDY - Data ready bit0 1 = data is ready if (dataReady == 0) return; //no data ready

    // #KW 494 readMagData - data is ready, read it NB bug fix here read

was starting from ST1 not XOUT_L byte[] buffer = ro.readByteRegisters(Registers.AK8963_XOUT_L, 7); //

KW L815 6 data bytes x,y,z 16 bits stored as little Endian (L/H)

    // Check if magnetic sensor overflow set, if not then report data

    //roAK.readByteRegister(Registers.AK8963_ST2);// Data overflow bit 3

and data read error status bit 2 byte status2 = buffer[6]; // Status2 register must be read as part of data read to show device data has been read if((status2 & 0x08) == 0) //#KW 817 bit3 HOFL: Magnetic sensor overflow is normal (no Overflow), data is valid { //#KW L818-820 lastRawMagX = (short) ((buffer[1] << 8) | (buffer[0]&0xFF)); // Turn the MSB and LSB into a signed 16-bit value lastRawMagY = (short) ((buffer[3] << 8) | (buffer[2]&0xFF)); // Data stored as little Endian lastRawMagZ = (short) ((buffer[5] << 8) | (buffer[4]&0xFF)); // mask to prevent sign extension in LSB (bug fix)

        //the stored calibration results is applied here as there is

no hardware correction stored in the hardware via calibration //#KW L496-L501. scale() does the multiplication by magScale L499-501 lastCalibratedReading = scale(new TimestampedData3f( lastRawMagXmagScale.resmagCalibration.getX() - getDeviceBias().getX(),

lastRawMagYmagScale.resmagCalibration.getY() - getDeviceBias().getY(),

lastRawMagZmagScale.resmagCalibration.getZ() - getDeviceBias().getZ())); this.addValue(lastCalibratedReading); //store the result } }

- You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU-9250/issues/90#issuecomment-262790579 , or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qnNR-BziakeyMWY1l3Npw 3dzg4hUks5rBaFjgaJpZM4K6z7J . https://github.com/notifications/beacon/AGY1qr-kYgUS6JELA6VGmkH6m_4YnsFZks5 rBaFjgaJpZM4K6z7J.gif

[gjwo]

Kris, Yaw and roll are now responding instantly and perfectly, but pitch is coming out at half the magnitude it should do. When doubled the settled angles are correct, but it takes a few seconds to settle. Any ideas about what may be causing this?

[gjwo]

Kris, apart from the scaling issue, the problem occurs when pitching 90 degrees up and down, and at that point both yaw & roll are affected, I think the yaw is correct because as it passed through -90 or +90 the front direction flips by 180 degrees, I guess the same is true for roll so on reflection these are OK. So I think the only issue is one of magnitude of pitch being half what it should be. See graph below do you agree?

[gjwo]

Yaw graph

[gjwo]

Roll graph