--Definitionen
id  = 0
sda = 3 --GPIO0
scl = 4 --GPIO2
dev_addr = 0x68 --Addr des Gyros

--print("IIC init")

--I2C initialisieren
i2c.setup(id, sda, scl, i2c.SLOW)

--print("Wake")

--MPU aufwecken
i2c.start(id)
i2c.address(id, dev_addr, i2c.TRANSMITTER)
i2c.write(id, 0x6b)
i2c.write(id, 0x00)

-- Selbsttest Gyro
--i2c.start(id)
--i2c.address(id, dev_addr, i2c.TRANSMITTER)
--i2c.write(id, 0x1b)
--i2c.write(id, 0b01000000)
--i2c.stop(id)

--print("Gyro init")

--Gyro einstellen auf 250Grad/sek
i2c.start(id)
i2c.address(id, dev_addr, i2c.TRANSMITTER)
i2c.write(id, 0x1b)
i2c.write(id, 0)
i2c.stop(id)

--print("Funktion")

--Funktion zum Auslesen eines Registers
function read_reg(reg_addr)
--print("debug")
    i2c.start(id)
    i2c.address(id, dev_addr, i2c.TRANSMITTER)
    i2c.write(id, reg_addr)
    i2c.stop(id)
    i2c.start(id)
    i2c.address(id, dev_addr, i2c.RECEIVER)
    c = i2c.read(id, 1)
    i2c.stop(id)
    return c
end

-- Chance auf Reset (MPU abstecken, neustarten, Programm endet hier
if (string.byte(read_reg(0x75)) == 104) then

while (true) do
Gyro1 = (string.byte(read_reg( 0x45)))*256
Gyro2 = string.byte(read_reg( 0x46))
Gyro = Gyro1 + Gyro2

AccX1 = (string.byte(read_reg( 0x3B)))*256
AccX2 = string.byte(read_reg( 0x3C))
AccX = AccX1 + AccX2

AccZ1 = (string.byte(read_reg( 0x3F)))*256
AccZ2 = string.byte(read_reg( 0x40))
AccZ = AccZ1 + AccZ2

--out = string.format(%f;%f;%f, Gyro, AccX, AccZ)
print(readdata())
tmr.delay(10)
end

end

---##################################
readdata = function()

Gyro1 = (string.byte(read_reg( 0x45)))*256
Gyro2 = string.byte(read_reg( 0x46))
Gyro = fillvalue(Gyro1 + Gyro2)

AccX1 = (string.byte(read_reg( 0x3B)))*256
AccX2 = string.byte(read_reg( 0x3C))
AccX = fillvalue(AccX1 + AccX2)

AccZ1 = (string.byte(read_reg( 0x3F)))*256
AccZ2 = string.byte(read_reg( 0x40))
AccZ = fillvalue(AccZ1 + AccZ2)

out = Gyro .. ";" .. AccX .. ";" .. AccZ

return out
end

---##################################
fillvalue = function(startvalue)
endvalue = startvalue
tostring(endvalue)
while (string.len(endvalue) < 5) do
endvalue = ("0" .. endvalue)
end
return endvalue
end