PICAXE kompas z magnetometru

Odpovědět
AlesH
Příspěvky: 323
Registrován: 25 úno 2013, 09:18

PICAXE kompas z magnetometru

Příspěvek od AlesH »

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.
petr-kubac
Příspěvky: 96
Registrován: 24 úno 2013, 15:43
Bydliště: Frydek - Mistek
Kontaktovat uživatele:

Re: PICAXE kompas z magnetometru

Příspěvek od petr-kubac »

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"
"The best computer language is a solder" - "Nejlepší programovací jazyk je pájka" - Bob Pease
http://petr-kubac.blog.cz/
petr-kubac
Příspěvky: 96
Registrován: 24 úno 2013, 15:43
Bydliště: Frydek - Mistek
Kontaktovat uživatele:

Re: PICAXE kompas z magnetometru

Příspěvek od petr-kubac »

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 ;-))))
"The best computer language is a solder" - "Nejlepší programovací jazyk je pájka" - Bob Pease
http://petr-kubac.blog.cz/
AlesH
Příspěvky: 323
Registrován: 25 úno 2013, 09:18

Re: PICAXE kompas z magnetometru

Příspěvek od AlesH »

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.
AlesH
Příspěvky: 323
Registrován: 25 úno 2013, 09:18

Re: PICAXE kompas z magnetometru

Příspěvek od AlesH »

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.

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.
AlesH
Příspěvky: 323
Registrován: 25 úno 2013, 09:18

Re: PICAXE kompas z magnetometru

Příspěvek od AlesH »

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.
PICAXE08M2kompas-schema.png
PICAXE08M2kompas-schema.png (6.44 KiB) Zobrazeno 6684 x
PICAXE08M2kompas.jpg
http://www.youtube.com/watch?v=lV113aCnGS4
Odpovědět