Magnetometer init fixed
Added gyro manip with inaccurate constants