MPU9150 Magnetometer Calibration

Hi,

I'm working with the MPU9150 and I'm trying to estimate yaw through pitch, roll and mag data.
Roll and pitch are working perfectly (I'm not using quaternions).
At this point I'm reading the mag data (adjusting the raw data with the sensytive values ASAX, ASAY, ASAZ), but I'm not sure how to calibrate magnetometer data.

My intention is to use the following equations to estimate yaw:

mag_x = mX*cosP + mY*senP*senR + mZ*senP*cosR;
mag_y = -mY*cosR + mZ*senR;
yaw = (atan2(mag_y,mag_x))* rad_to_deg;

where mX, mY and mZ are the calibrated data of the magnetometer.
Also, I've aligned the magnetometer axis with the accelerometer and gyroscope axis previously:
MagYAxis = -mX
MagXAxis = mY;
MagZAxis = mZ;

I've tried some options from sparkfun and panseti (github libraries), but I haven't achieved any reasonable yaw data.
Any help will be welcome.

Thanks,
Javier.