This is the doc i was referring to:
http://www.ssec.honeywell.com/magnetic/datasheets/lowcost.pdf
In the previous listed code the z vector-component wasn't taken in account so in normal writing you'd need:
Xh = Xcos(f) + Ysin(q)sin(f) - Zcos(q)sin(f)
Yh = Ycos(q) + Z*sin(q)
and
Heading for (Xh <0) = 180 - arcTan(Yh/Xh)
for (Xh >0, Yh <0) = - arcTan(Yh/Xh)
for (Xh >0, Yh >0) = 360 - arcTan(Yh/Xh)
for (Xh =0, Yh <0) = 90
for (Xh =0, Yh >0) = 270
This is almost like combining both codes listed 