Why doesn't yaw calculating axis change after board reboot?

Question

I have an MPU9250 on my board and I want to change the yaw calculating axis after reboot depending on how this board is placed, like this:

And I thought it would change if I reboot the board, but it doesn't. I'm not very good at this kind of math, but I really hope you can help me.

Here is the code that I use to get φ, θ, ψ. In my main.c loop:

Process_IMU();

q[0] = q0;
q[1] = q1;
q[2] = q2;
q[3] = q3;
a12 =   2.0f * (q[1] * q[2] + q[0] * q[3]);
a22 =   q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3];
a31 =   2.0f * (q[0] * q[1] + q[2] * q[3]);
a32 =   2.0f * (q[1] * q[3] - q[0] * q[2]);
a33 =   q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
float sinp = a32;
if (abs(sinp) >= 1)
    pitch = copysign(M_PI/2,sinp);
else
    pitch = asin(sinp);
//pitch = -asinf(a32);
roll  = atan2f(a31, a33);
yaw   = atan2f(a12, a22) * 2;
pitch *= 180.0f / pi;
yaw = atan2f(sinf(yaw),cosf(yaw));
yaw   *= 180.0f / pi;
roll  *= 180.0f / pi;

Process_IMU():

void Process_IMU()
{
    //read raw data
    uint8_t data[14];
    uint8_t reg = ACCEL_XOUT_H;
    uint8_t mpu_address = MPU9250_ADDRESS_DEFAULT;


    while(HAL_I2C_Master_Transmit(_MPU9250_I2C,(uint16_t)mpu_address,&reg,1,1000) != HAL_OK);
    while(HAL_I2C_Master_Receive(_MPU9250_I2C, (uint16_t)mpu_address, data, 14, 1000) != HAL_OK);

    /*-------- Accel ---------*/
    Accel_x = (int16_t)((int16_t)( data[0] << 8 ) | data[1]);
    Accel_y = (int16_t)((int16_t)( data[2] << 8 ) | data[3]);
    Accel_z = (int16_t)((int16_t)( data[4] << 8 ) | data[5]);

    /*-------- Gyrometer --------*/
    Gyro_x = (int16_t)((int16_t)( data[8] << 8  ) | data[9]);
    Gyro_y = (int16_t)((int16_t)( data[10] << 8 ) | data[11]);
    Gyro_z = (int16_t)((int16_t)( data[12] << 8 ) | data[13]);


    Accel_X = 10*(float)((int32_t)Accel_x - Accel_x_bias)/(float)accel_sensitivity;
    Accel_Y = 10*(float)((int32_t)Accel_y - Accel_y_bias)/(float)accel_sensitivity;
    Accel_Z =  10*(float)((int32_t)Accel_z - Accel_z_bias)/(float)accel_sensitivity ;

    Gyro_X =  (float)(((int32_t)Gyro_x - Gyro_x_bias)/(float)gyro_sensitivity)*M_PI/180.0f;
    Gyro_Y =  (float)(((int32_t)Gyro_y - Gyro_y_bias)/(float)gyro_sensitivity)*M_PI/180.0f;
    Gyro_Z =  (float)(((int32_t)Gyro_z - Gyro_z_bias)/(float)gyro_sensitivity)*M_PI/180.0f;

    // Get data of Magnetometer
    Get_magnetometer();

    MadgwickAHRSupdateFixed(Gyro_X,Gyro_Y,Gyro_Z,Accel_X,Accel_Y,Accel_Z,Mag_X_calib,Mag_Y_calib,-Mag_Z_calib);


}

Get_magnetometer():

void Get_magnetometer()
{
    uint8_t raw_data[7];
    uint8_t reg_ST1 = ST1;
    uint8_t mag_address = MAG_ADDRESS_DEFAULT;
    uint8_t reg = XOUT_L;
    while(HAL_I2C_Master_Transmit(_MPU9250_I2C,(uint16_t)mag_address,&reg_ST1,1,1000) != HAL_OK);
    while(HAL_I2C_Master_Receive(_MPU9250_I2C,(uint16_t)mag_address,raw_data,1,1000) != HAL_OK);
    if (raw_data[0] & 0x01)
    {
        while(HAL_I2C_Master_Transmit(_MPU9250_I2C,(uint16_t)mag_address,&reg,1,1000) != HAL_OK);
        while(HAL_I2C_Master_Receive(_MPU9250_I2C,(uint16_t)mag_address,raw_data,7,1000) != HAL_OK);
        // Read the six raw data and ST2 registers sequentially into data arra
        if(!(raw_data[6] & 0x08))// Check if magnetic sensor overflow set, if not then report data
        {
            Mag_x = (int16_t)((raw_data[1]<<8) | raw_data[0] );
            Mag_y = (int16_t)((raw_data[3]<<8) | raw_data[2] );
            Mag_z = (int16_t)((raw_data[5]<<8) | raw_data[4] );
        }


        Mag_X_calib = (float)Mag_x * asax * mag_sensitivity - mag_offset[0];
        Mag_Y_calib = (float)Mag_y * asay * mag_sensitivity - mag_offset[1];
        Mag_Z_calib = (float)Mag_z * asaz * mag_sensitivity - mag_offset[2];

        Mag_X_calib *=scale_x;
        Mag_Y_calib *=scale_y;
        Mag_Z_calib *=scale_z;

    }
}

MadgwickAHRSupdateFixed():

void MadgwickAHRSupdateFixed(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
    float recipNorm;
    float s0, s1, s2, s3;
    float qDot1, qDot2, qDot3, qDot4;
    float hx, hy;
    float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _8bx, _8bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;

    // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
    if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
        MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
        return;
    }

    // Rate of change of quaternion from gyroscope
    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);

    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

        // Normalise accelerometer measurement
        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
        ax *= recipNorm;
        ay *= recipNorm;
        az *= recipNorm;

        // Normalise magnetometer measurement
        recipNorm = invSqrt(mx * mx + my * my + mz * mz);
        mx *= recipNorm;
        my *= recipNorm;
        mz *= recipNorm;

        // Auxiliary variables to avoid repeated arithmetic
        _2q0mx = 2.0f * q0 * mx;
        _2q0my = 2.0f * q0 * my;
        _2q0mz = 2.0f * q0 * mz;
        _2q1mx = 2.0f * q1 * mx;
        _2q0 = 2.0f * q0;
        _2q1 = 2.0f * q1;
        _2q2 = 2.0f * q2;
        _2q3 = 2.0f * q3;
        _2q0q2 = 2.0f * q0 * q2;
        _2q2q3 = 2.0f * q2 * q3;
        q0q0 = q0 * q0;
        q0q1 = q0 * q1;
        q0q2 = q0 * q2;
        q0q3 = q0 * q3;
        q1q1 = q1 * q1;
        q1q2 = q1 * q2;
        q1q3 = q1 * q3;
        q2q2 = q2 * q2;
        q2q3 = q2 * q3;
        q3q3 = q3 * q3;

        // Reference direction of Earth's magnetic field
        hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
        hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
        _2bx = sqrt(hx * hx + hy * hy);
        _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
        _4bx = 2.0f * _2bx;
        _4bz = 2.0f * _2bz;
        _8bx = 2.0f * _4bx;
        _8bz = 2.0f * _4bz;

        // Gradient decent algorithm corrective step
        s0= -_2q2*(2*(q1q3 - q0q2) - ax)    +   _2q1*(2*(q0q1 + q2q3) - ay)   +  -_4bz*q2*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)   +   (-_4bx*q3+_4bz*q1)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)    +   _4bx*q2*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
        s1= _2q3*(2*(q1q3 - q0q2) - ax) +   _2q0*(2*(q0q1 + q2q3) - ay) +   -4*q1*(2*(0.5 - q1q1 - q2q2) - az)    +   _4bz*q3*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)   + (_4bx*q2+_4bz*q0)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)   +   (_4bx*q3-_8bz*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
        s2= -_2q0*(2*(q1q3 - q0q2) - ax)    +     _2q3*(2*(q0q1 + q2q3) - ay)   +   (-4*q2)*(2*(0.5 - q1q1 - q2q2) - az) +   (-_8bx*q2-_4bz*q0)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(_4bx*q1+_4bz*q3)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q0-_8bz*q2)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
        s3= _2q1*(2*(q1q3 - q0q2) - ax) +   _2q2*(2*(q0q1 + q2q3) - ay)+(-_8bx*q3+_4bz*q1)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(-_4bx*q0+_4bz*q2)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
        recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
        s0 *= recipNorm;
        s1 *= recipNorm;
        s2 *= recipNorm;
        s3 *= recipNorm;

        // Apply feedback step
        qDot1 -= beta * s0;
        qDot2 -= beta * s1;
        qDot3 -= beta * s2;
        qDot4 -= beta * s3;
    }

    // Integrate rate of change of quaternion to yield quaternion
    q0 += qDot1 * (1.0f / sampleFreq);
    q1 += qDot2 * (1.0f / sampleFreq);
    q2 += qDot3 * (1.0f / sampleFreq);
    q3 += qDot4 * (1.0f / sampleFreq);

    // Normalise quaternion
    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
}

I have tried to find information about this, but couldn't. Only this book: https://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.110.5134&rep=rep1&type=pdf but I'm not sure that it will help me. So, the final question is - how to change the yaw calculating axis after board reboot? Thanks for any advice!

Answer 1

The x, y and z orientation of the device is fixed. Extract from the data sheet:

Even when you flip the board into a different orientation and repower, these axes will remain the same relative to the device; they are not dynamically allocated at power on.

What you need is a Flip value. This value would then be used to map the device axes to axes that match the orientation of the board. Using the NED orientation system, yaw is calculated as rotation around the z axis:

So what you need to do is determine what your board axes need and map your magnetometer axes to this. For example, mag_x_flipped = mag_y_unflipped. Without knowing what orientation the magnetometer is on your board I can't say what values you would need for each of your three scenarios.

The flip value needs to either be set as a user setting or, I can imagine it could be dynamically set at power on by looking at your accelerometer axes values. In each of your three scenarios, one axis will measure 1g while the others will measure 0g (or there abouts).