/* This subroutine resets the the soft iron transformation to unity matrix and disable the soft iron transformation process by default.  */
void inv_reset_compass_soft_iron_matrix(void)
/* This subroutine resets the the soft iron transformation to unity matrix and disable the soft iron transformation process by default.  */
void inv_reset_compass_soft_iron_matrix(void)
{
        sensors.soft_iron.matrix_f[i] = 0.0f;
    }
    memset(&sensors.soft_iron.matrix_d,0,sizeof(sensors.soft_iron.matrix_d));
    for (i=0; i<3; i++)  {
        sensors.soft_iron.matrix_f[i*4] = 1.0;
        sensors.soft_iron.matrix_d[i*4] = ROT_MATRIX_SCALE_LONG;
    }
    inv_disable_compass_soft_iron_matrix();
}