 	}
 	
 	// MARK : **** API
+	func crc8(data : [UInt8], Len : Int) -> UInt8
+	{
+		var i = Int(0)
+		var e = UInt8(0)
+		var f = UInt8(0)
+		var crc = UInt8(0)
+		
+		//for (i = 0; i < Len; i += 1)
+		while i < Len {
+			e = crc ^ data[i];
+			f = e ^ (e >> 4) ^ (e >> 7);
+			crc = (f << 1) ^ (f << 4);
+			i += 1
+		}
+	
+		return crc;
+	}
+
 	// Debug
 	func getDataPortState() {
 		if (isDeviceReady() == false) {

		
		pkbuf[0] = UInt8((NEB_CTRL_PKTYPE_CMD << 5) | NEB_CTRL_SUBSYS_MOTION_ENG) //0x41
		pkbuf[1] = UInt8(sizeof(Fusion_DataPacket_t))
		pkbuf[2] = 0
		pkbuf[3] = UInt8(MAG_Data)	// Cmd
		
		if Enable == true

		
		pkbuf[0] = UInt8((NEB_CTRL_PKTYPE_CMD << 5) | NEB_CTRL_SUBSYS_MOTION_ENG) //0x41
		pkbuf[1] = UInt8(sizeof(Fusion_DataPacket_t))
		pkbuf[2] = 0xFF
		pkbuf[3] = UInt8(LockHeadingRef)	// Cmd
		
		pkbuf[2] = crc8(pkbuf, Len: Int(pkbuf[1]) + 4)

		device.writeValue(NSData(bytes: UnsafeMutablePointer<Void>(pkbuf), length: 20), forCharacteristic: ctrlChar, type: CBCharacteristicWriteType.WithoutResponse)
	}
	
