Ahoj. Pokouším se postavit malý elektronický kompas (pro robota) s využitím PICAXE 08M2 a tříosého I2C magnetometru s čipem HMC5883L. Umím už z magnetometru dostat surová změřená data (šestnáctibitové hodnoty mg. pole v osách X, Y a Z), ale teď je samozřejmě potřebuju kalibrovat (data nejsou symetricky k nule), normalizovat (mají různé rozsahy v rúzných osách), filtrovat (i bez pohybu čidla se data trochu mění ["šumí"]) a hlavně přepočítat ty hodnoty na reálný azimut. K tomu výpočtu azimutu bych potřeboval goniometrické funkce jako SIN nebo COS, ale ty PICAXE 08M2 neumí, takže asi budu muset použít nějaký přibližný výpočet nebo převodní tabulku. Před časem jsem se už o podobný výpočet pokoušel, ale výsledek nebyl nic moc (vypočítaný azimut se měnil hodně nelineárně se skutečným natočením). Ani na internetu se mi zatím nic rozumného nepodařilo najít.
Znáte někdo vhodný způsob, jak prostředky PICAXE 08M2 ze dvou magnetických os spočítat azimut? Tedy něco jako AZ = nejakaFunkce(X,Y) [ale jen s využitím možností PICAXE 08M2 (jen celočíselná matematika)]. Prosím o radu nebo alespoň nasměrování, kde hledat inspiraci.
PICAXE kompas z magnetometru
-
- Příspěvky: 96
- Registrován: 24 úno 2013, 15:43
- Bydliště: Frydek - Mistek
- Kontaktovat uživatele:
Re: PICAXE kompas z magnetometru
Každý kdo viděl kompas podléhá oblíbenému omylu, že siločáry magnetického pole jdou pěkně po povrchu Zeměkoule od Severu k Jihu
Problém je v tom, že magnetické pole generuje rozžhavené a železné jádro planety nad kterým je ještě pár tisíc kilometrů magneticky (téměř) inertní horniny. Takže siločáry nejdou jen tak od severu k jihu, ale v našich zeměpisných šířkách se "zapichují do země" pod úhlem skoro 60 stupňů
Pepa Hanzal mi kdysi posílal Link na něco v tomto stylu http://ve6sbs.sbszoo.com/projects/compa ... n00022.pdf
Jinak pokud se jedná o výpočty trigonometrických funkcí jenom pomocí sčítání a bitových posunů = heslo pro BING je "CORDIC"
Problém je v tom, že magnetické pole generuje rozžhavené a železné jádro planety nad kterým je ještě pár tisíc kilometrů magneticky (téměř) inertní horniny. Takže siločáry nejdou jen tak od severu k jihu, ale v našich zeměpisných šířkách se "zapichují do země" pod úhlem skoro 60 stupňů
Pepa Hanzal mi kdysi posílal Link na něco v tomto stylu http://ve6sbs.sbszoo.com/projects/compa ... n00022.pdf
Jinak pokud se jedná o výpočty trigonometrických funkcí jenom pomocí sčítání a bitových posunů = heslo pro BING je "CORDIC"
"The best computer language is a solder" - "Nejlepší programovací jazyk je pájka" - Bob Pease
http://petr-kubac.blog.cz/
http://petr-kubac.blog.cz/
-
- Příspěvky: 96
- Registrován: 24 úno 2013, 15:43
- Bydliště: Frydek - Mistek
- Kontaktovat uživatele:
Re: PICAXE kompas z magnetometru
Omlouvám se za druhou reakci ale vaše "nějaká funkce" je 4 kvadrantový arkustangens - dokonce jako ATAN2 (X, Y) je i v Open Office Calcu (a myslím, že i v Excelu)
Rozdíl od prostého Arcustangens (X) je v tom, že "prostá verze" vrací jenom úhly 0-90 st - zatímco ATAN2 bere v potaz i znaménko X a Y a podle toho určí i ve kterém kvadratnu to je
neboli pokud X a Y jsou obe 1 je to azimut 45 stupňů zatímco pokud jsou obě -1 (jejich podíl je pořád -1/-1 = 1) je to azimut 225 stupňů
Zbývá už jenom abych vám naprogramoval CORDIC verzi pro algoritmus ATAN2 - ale to nechám z nedostatku času (a píle) na Vás )))
Rozdíl od prostého Arcustangens (X) je v tom, že "prostá verze" vrací jenom úhly 0-90 st - zatímco ATAN2 bere v potaz i znaménko X a Y a podle toho určí i ve kterém kvadratnu to je
neboli pokud X a Y jsou obe 1 je to azimut 45 stupňů zatímco pokud jsou obě -1 (jejich podíl je pořád -1/-1 = 1) je to azimut 225 stupňů
Zbývá už jenom abych vám naprogramoval CORDIC verzi pro algoritmus ATAN2 - ale to nechám z nedostatku času (a píle) na Vás )))
"The best computer language is a solder" - "Nejlepší programovací jazyk je pájka" - Bob Pease
http://petr-kubac.blog.cz/
http://petr-kubac.blog.cz/
Re: PICAXE kompas z magnetometru
Díky za náměty a informace. Ano, vím že siločáry magnetického pole nejsou u nás vodorovně a pokusím se to v programu zohlednit. Svoje výsledky sem pak napíšu.
Re: PICAXE kompas z magnetometru
Tak jsem dnes zprovoznil první použitelnou verzi programu (určení azimutu z hodnot magnetometru). Zatím je výpočet ATAN udělán interpolací mezi předpočítanými hodnotami z tabulky. Kód je níže. Třeba se to někomu bude hodit. Přesnost vypadá pro mne přijatelná (lepší než cca 10°, možná i pod 5°). Náklon ale není nijak kompenzován, takže je to použitelné jen pro vodorovné povrchy (např. pro indoor soutěže jako při Robotickém dni v Praze). O víkendu se pokusím doplnit ještě fotky a snad i video z mých experimentů.
P.S.: Podle mého měření dokáže PICAXE-08M2 v tomto programu a nastavení (32 MHz) přepočítat X/Y složky mg. pole na azimut cca 300x za sekundu (jeden přepočet za 3 - 4 ms), což je pro moje účely zcela vyhovující, takže jiný způsob přepočtu asi ani nebudu zkoušet.
P.S.: Podle mého měření dokáže PICAXE-08M2 v tomto programu a nastavení (32 MHz) přepočítat X/Y složky mg. pole na azimut cca 300x za sekundu (jeden přepočet za 3 - 4 ms), což je pro moje účely zcela vyhovující, takže jiný způsob přepočtu asi ani nebudu zkoušet.
Kód: Vybrat vše
#picaxe 08M2
;#no_data
; zpracování dat z I2C magnetometru HMC5883L (modul GY-271)
; C.0 - sertxd (sériový výstup k PC nebo k dalšímu PICAXE)
; C.1 - SCL (k magnetometru)
; C.2 - SDA (k magnetometru)
; C.3 - in (-- nepoužito --)
; C.4 - out (LED, připojena přes odpor cca 800 ohm k zemi nebo k + napájení)
; C.5 - serin (-- nepoužito --)
; napájení 5V (4x AA NiMH nabíjitelné akumulátory) [pro PICAXE i modul současně]
; měřený rozsah v každé ose magnetometru je cca 1200 (+-600), šum cca 30
eeprom 0, (0,6,11,17,22,27,31,35,39,42,45) ; předpočítaná tabulka úhlů (atan)
symbol xm = w0 ; měřená osa X magnetometru
symbol ym = w1 ; měřená osa Y magnetometru
symbol zm = w2 ; měřená osa Z magnetometru
symbol cas = w3 ; pro sledování změn proměnné "time"
symbol cyklu = w4 ; počítadlo cyklů (pro měření rychlosti zpracování dat)
symbol azimut = w5 ; výsledný přepočítaný azimut
symbol uhel0 = w6 ; pomocný úhel podle kvadrantu z hodnot X a Y
symbol uhel = b14 ; výsledný úhel po výpočtu arctan
symbol uhel2 = b15 ; pomocný úhel určený z tabulky
symbol tan100 = b16 ; tangens * 100 (X / Y * 100)
symbol index = b17 ; vypočítaný index do tabulky
symbol offset = b18 ; vypočítaný posun mezi body indexu
symbol kvadrant = b19 ; kvadrant azimutu podle hodnot X a Y
symbol x = w10 ; zkalibrovaná a normalizovaná hodnota X (0 - 200)
symbol y = w11 ; zkalibrovaná a normalizovaná hodnota Y (0 - 200)
symbol z = w12 ; zkalibrovaná a normalizovaná hodnota Z (0 - 200)
setfreq m32 ; zvýšení frekvence procesoru (max. 32 MHz [8x normal])
pause 100 ; počkáme na ustálení napájení a vstupů po startu
sertxd ("PICAXE 08M2 kompas s HMC5883L",CR,LF)
hi2csetup i2cmaster, $3C, i2cslow_32, i2cbyte ; inicializace I2C na adrese 3C
hi2cout 0, ( %00010000 ) ; Config A Default - 15Hz updates, Normal measurement
hi2cout 1, ( %00100000 ) ; Config B Default - +/1Ga gain
hi2cout 2, ( %00000000 ) ; Mode - Continuous conversion
sertxd ("setup OK",CR,LF)
do
inc cyklu
if time>cas then ; měříme a vypisujeme po každé změně "time" (2x za sec.)
cas = time
hi2cin 3, ( b1,b0 , b3,b2 , b5,b4 ) ; načtení holých dat z magnetometru (x,y,z)
xm = xm + 800 / 8 ; normalizace X do max. rozsahu 0 - 200 se středem 100
ym = ym + 800 / 8 ; normalizace Y do max. rozsahu 0 - 200 se středem 100
zm = zm + 800 / 8 ; normalizace Z do max. rozsahu 0 - 200 se středem 100
x = xm
y = 200 - ym min 0 max 200
z = zm
gosub atan2 ; výpočet azimutu
sertxd ("t:",#time," c:",#cyklu," az:",#azimut," q:",#kvadrant," t:",#tan100,CR,LF)
toggle C.4 ; kontrolní blikání LEDkou
cyklu = 0
endif
gosub atan2 ; jen pro odhad rychlosti výpočtu
loop
atan2:
; výpočet azimutu podle X a Y (arcustangens a kvadrant)
; hodnoty X a Y musí být předem normalizovány do rozsahu 0 - 200
; nula je na hodnotě 100 (pro X i pro Y)
if x>=100 then
x = x - 100
if y>=100 then
y = y - 100
kvadrant = 0
else
y = 100 - y
kvadrant = 3
endif
else
x = 100 - x
if y>=100 then
y = y - 100
kvadrant = 1
else
y = 100 - y
kvadrant = 2
endif
endif
tan100 = 100
if x>y then
tan100 = 100 * y / x
else if y>x then
tan100 = 100 * x / y
endif
; výpočet atan interpolací mezi dvěma předpočítanými hodnotami z tabulky (0 - 45°)
index = tan100 / 10
offset = tan100 % 10
read index,uhel
inc index
read index,uhel2
azimut = uhel2 - uhel * offset / 10 + uhel
; posun azimutu do správného kvadrantu
if y>=x then
azimut = 90 - azimut
endif
if kvadrant = 1 or kvadrant = 3 then
azimut = kvadrant * 90 - azimut + 90
else
azimut = kvadrant * 90 + azimut
endif
return
Naposledy upravil(a) AlesH dne 22 zář 2013, 10:12, celkem upraveno 1 x.
Re: PICAXE kompas z magnetometru
Tak sem doplním ještě schéma el. kompasu, fotku zapojení na nepájivém poli a video z mého testování programu v PICAXE-08M2.
http://www.youtube.com/watch?v=lV113aCnGS4
http://www.youtube.com/watch?v=lV113aCnGS4