Mám za úkol zjistit úhel natočení z akcelerometru/gyroskopu MPU 6050. To není nijak veliký problém. Ovšem když čidlem škubu v osách tak i přes, to že se úhel natočení nemění, se čtený úhel natočení mění. Mohl by mi někdo poradit jakým způsobem mám k problému přistoupit?
Programuji v pythonu pro raspberry pi 3+
V ustáleném stavu a když není škubnutí nijak drastické to funguje dobře.
Kód: Vybrat vše
ax = (sensor.get_accel_data()["x"]) # údaje akcelerometru
ay = (sensor.get_accel_data()["y"])
az = (sensor.get_accel_data()["z"])
grx = (sensor.get_gyro_data()["x"]) # údaje gyroskopu
gry = (sensor.get_gyro_data()["y"])
grz = (sensor.get_gyro_data()["z"])
arx = 57.29 * (math.atan2(-ay, -az) + math.pi) #přepočet na úhel
ary = 57.29 * (math.atan2(-ax, -az) + math.pi)
arz = 57.29 * (math.atan2(-ay, -ax) + math.pi)
AngleX = (0.96 * arx) + (0.04 * grx); # korekce pomocí jakéhosi filtru z internetu
AngleY = (0.96 * ary) + (0.04 * gry);
AngleZ = (0.96 * arz) + (0.04 * grz);
Děkuji
