Where did the calibration constants that are in that code come from?
Neither of the following sets of matrix coefficients are correct. They must be symmetrical about the diagonal.
Xa_cal = 0.986992*Xa_off + 0.001993*Ya_off - 0.004377*Za_off; //X-axis correction for combined scale factors (Default: positive factors)
Ya_cal = 0.025189*Xa_off + 0.998259*Ya_off - 0.000417*Za_off; //Y-axis correction for combined scale factors
Za_cal = -0.010287*Xa_off - 0.000417*Ya_off + 0.942771*Za_off; //Z-axis correction for combined scale factors
In this one, it is very likely that the 0.000982 coefficient on Xm_off wrong by a couple of orders of magnitude. It should be about 1.
Xm_cal = 0.000982*Xm_off + 0.006185*Ym_off + 0.015063*Zm_off; //X-axis correction for combined scale factors (Default: positive factors)
Ym_cal = 0.000039*Xm_off + 0.950124*Ym_off + 0.003084*Zm_off; //Y-axis correction for combined scale factors
Zm_cal = 0.000005*Xm_off + 0.003084*Ym_off + 0.880435*Zm_off; //Z-axis correction for combined scale factors
What do you mean by "not successful"?
Note that one of these two equations is wrong:
// Pitch and roll
roll = atan2(fYa, sqrt(fXa*fXa + fZa*fZa));
pitch = atan2(fXa, sqrt(fYa*fYa + fZa*fZa));
I prefer the definitions given here, correct for the R-xyz convention, which result in the following equations for pitch and roll (in degrees, not radians as above):
roll = atan2(y_Buff , z_Buff) * 57.3;
pitch = atan2((- x_Buff) , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.3;