Raspberry Pi

36) Elektronický kompas (magnetometr HMC5883)
... se signalizací nastaveného azimutu pomocí jedné LEDky a PWM

Po trochu delší době, kdy jsem se věnoval displejům, se vracím k dalším běžně dostupným perifériím.
V tomto článku popíšu zprovoznění tříosého magnetometru HMC5883, pomocí něhož lze vytvořit elektronický kompas.
Zároveň tu trochu nakousnu i softwérové PWM, pomocí kterého budu pomocí jedné LED signalizovat stav, kdy magnetometr natočím na požadovaný azimut.

Předpokládal jsem , že tento magnetometr budu společně s náklonoměrem využívat pro nastavování foťáku při fotografování záblesků družic sítě Iridium. Jak se ale ukázalo, magnetometr je velmi citlivý na okolní kovové předměty, takže se při přiblížení k foťáku rozladí a nedokáže přesně určit azimut.

I tak to ale může být pro někoho zajímavé zařízení.

Základem je obvod HMC5883l, který je možné sehnat asi za 4$ již připájený k malému plošňáčku společně s několika dalšími součástkami.
Někdy je tento obvod na jedné desce s dalšími čidly (tlak, náklon, gyroskop). Pak je ten obvod o něco dražší.
Já ho sehnal přes ebay : http://www.ebay.com/itm/140900053569

Protože čidlo komunikuje přes I²C, je připojení velice jednoduché:
(LEDka je zatím nepovinná, vysvětlím později.)

Vývod čidla, který je označený DRDY by měl signalizovat, že jsou data připravená ke čtení. Já ho ale nepoužívám, tak jsem ho nechal nezapojený.

Ovladače I²C a GPIO pro Python:

Pokud ještě z minulých článků nemáte nainstalovanou podporu I²C a GPIO pro Python, tak si jí nainstalujte podle postupu v článku 7).

Sofrware:

Pythoní příkazy po komunikaci přes I²C jsou vysvětleny tady: http://wiki.erazor-zone.de/wiki:linux:python:smbus:doc

Nejjednodušší program pro vyčítání hodnot ze všech 3 os magnetometru vypadá takto:

#!/usr/bin/python
# -*- encoding: utf-8 -*-
# cteni dat z magnetometru HMC5883
# posledni uprava 26.5.2013 (zmena prepoctu zapornych hodnot)


import smbus
import time


bus = smbus.SMBus(1)  # novejsi varianta RasPi (512MB)
#bus = smbus.SMBus(0)  # starsi varianta RasPi (256MB)

bus.write_byte_data(0x1e,0x00,0x70) # prumerovani=8 vzorku ; rychlost vzorkovani = 15Hz
bus.write_byte_data(0x1e,0x01,0x00) # citlivost +/- 0.88 Ga (nejvyssi citlivost)
bus.write_byte_data(0x1e,0x02,0x00) # nepretrzite mereni

   
while True:

  # cteni dat z cidla pomoci I2C sbernice
  data = bus.read_i2c_block_data(0x1e,0x03)
  x_16bit = (data[0]*256) + data[1]             
  z_16bit = (data[2]*256) + data[3]
  y_16bit = (data[4]*256) + data[5]


  # Prepocty pri zapornych hodnotach registru
  # Hodnoty registru se pohybuji v rozsahu -2048 az +2047
  # Pri prekroceni rozsahu je hodnota -4096

  if(x_16bit & 0xF000):          # pri zaporne hodnote jsou nejvyssi 4 bity ze 16 nastavene na "1"
    x_16bit = x_16bit & 0xFFF    # v tom pripade orezat nejvyssi 4 bity ...
    x_16bit = -0x1000 + x_16bit  # ... a z toho co zbyde spocitat dvojkovy doplnek

  if(y_16bit & 0xF000):          # pri zaporne hodnote jsou nejvyssi 4 bity ze 16 nastavene na "1"
    y_16bit = y_16bit & 0xFFF    # v tom pripade orezat nejvyssi 4 bity ...
    y_16bit = -0x1000 + y_16bit  # ... a z toho co zbyde spocitat dvojkovy doplnek

  if(z_16bit & 0xF000):          # pri zaporne hodnote jsou nejvyssi 4 bity ze 16 nastavene na "1"
    z_16bit = z_16bit & 0xFFF    # v tom pripade orezat nejvyssi 4 bity ...
    z_16bit = -0x1000 + z_16bit  # ... a z toho co zbyde spocitat dvojkovy doplnek



  print x_16bit , y_16bit , z_16bit


  time.sleep(0.5)

Pomocí změn v registrech 0x00 a 0x01 si můžete zkoušet, jak se bude měnit citlivost čidla a stabilita naměřených hodnot.
Všechny parametry jsou vysvětleny v katalogovém listu.

Po spuštění programu se začnou zobrazovat hodnoty ze všech tří os magnetometru, které se budou měnit podle natočení čidla.

Aby bylo možné určit azimut, je třeba tyto 3 hodnoty ještě přepočítat.
Pokud zajistíte, že bude čidlo vodorovně, je to to snadné. V tom případě stačí z hodnot směrů X a Y vypočítat výslednici magnetické síly (stačí využít matematickou funkci atan2() ). Osa Z se pak může ignorovat.
Upravený program, který využívá výpočtu ze stránky: http://think-bowl.com/... umožňuje i zadání tzv. magnetické deklinace - to je rozdíl směrů mezi magnetickým a geografickým pólem. U nás (v ČR) je to asi 3,5°.

#!/usr/bin/python
# -*- encoding: utf-8 -*-
# cteni dat z magnetometru HMC5883 a prepocet na azimut (cidlo je vodorovne)
# posledni uprava 26.5.2013 (zmena prepoctu zapornych hodnot)


import smbus
import time
import math

bus = smbus.SMBus(1)  # novejsi varianta RasPi (512MB)
#bus = smbus.SMBus(0)  # starsi varianta RasPi (256MB)

#==============
# tento podprogram byl opsan z:
#       https://bitbucket.org/thinkbowl/i2clibraries/

#Returns heading in degrees and minutes
def getHeading(osa_1,osa_2):
		
  headingRad = math.atan2(osa_2, osa_1)
  headingRad += 3.5  * ( math.pi / 180)  # mistni posun magnetickeho polu vuci severnimu polu je 3,5 stupne

  # Correct for reversed heading
  if(headingRad < 0):
    headingRad += 2*math.pi
		
# Check for wrap and compensate
  if(headingRad > 2*math.pi):
    headingRad -= 2*math.pi
			
# Convert to degrees from radians
  headingDeg = headingRad * 180/math.pi
  degrees = int(math.floor(headingDeg))
  minutes = int(round(((headingDeg - degrees) * 60)))
  return (degrees, minutes)
#==============




bus.write_byte_data(0x1e,0x00,0x70) # prumerovani=8 vzorku ; rychlost vzorkovani = 15Hz
bus.write_byte_data(0x1e,0x01,0x00) # citlivost +/- 0.88 Ga (nejvyssi citlivost)
bus.write_byte_data(0x1e,0x02,0x00) # nepretrzite mereni


while True:

  # cteni dat z cidla pomoci I2C sbernice
  data = bus.read_i2c_block_data(0x1e,0x03)
  x = (data[0]*256) + data[1]             
  z = (data[2]*256) + data[3] 
  y = (data[4]*256) + data[5]


  # Prepocty pri zapornych hodnotach registru
  # Hodnoty registru se pohybuji v rozsahu -2048 az +2047
  # Pri prekroceni rozsahu je hodnota -4096

  if(x & 0xF000):    # pri zaporne hodnote jsou nejvyssi 4 bity ze 16 nastavene na "1"
    x = x & 0xFFF    # v tom pripade orezat nejvyssi 4 bity ...
    x = -0x1000 + x  # ... a z toho co zbyde spocitat dvojkovy doplnek

  if(y & 0xF000):    # pri zaporne hodnote jsou nejvyssi 4 bity ze 16 nastavene na "1"
    y = y & 0xFFF    # v tom pripade orezat nejvyssi 4 bity ...
    y = -0x1000 + y  # ... a z toho co zbyde spocitat dvojkovy doplnek

  if(z & 0xF000):    # pri zaporne hodnote jsou nejvyssi 4 bity ze 16 nastavene na "1"
    z = z & 0xFFF    # v tom pripade orezat nejvyssi 4 bity ...
    z = -0x1000 + z  # ... a z toho co zbyde spocitat dvojkovy doplnek


# pri otaceni vodorovne polozeneho plosnacku se prepocitavaji osy "X" a "Y". Osa "Z" se ignoruje.
# Pokud mate cidlo orientovane jinym smerem, pouzijte jine osy
  natoceni = getHeading(x , y)

  print  str(natoceni[0]) + "° " + str(natoceni[1]) + "'"      

  time.sleep(0.5)

Ukázka činnosti programu:

Dalším vylepšením programu je řízení LED diody, která se postupně rozsvěcí podle toho, jak se magnetometr natáčí do požadovaného směru. To může být výhodné v případě, že nemáte k RasPi připojený displej, nebo když nemůžete sledovat údaj na displeji a zároveň někam mířit čidlem.
Kvůli této funkci jsem využil PWM. Vysvětlení toho, co je pulzně šířková modulace (PWM) naleznete třeba tady na té stránce:
http://www.raspi.cz/2013/03/io-jave-rychle-io-pwm-modulace-tak-dale/

Pro větší komfort při určování správného nastavení azimutu jsem zvolil následující funkci pro jas LEDky v závislosti na rozdílu požadovaného a skutečného azimutu (je to upravený kosínus rozdílu úhlů):

Když čidlo míří na opačnou stranu, než je požadovaný azimut, LED je zhaslá. Čím více se aktuální azimut čidla přibližuje k požadovanému azimutu, tím více se LED rozsvítí. Protože je však obtížné určit změnu jasu, když LED svítí skoro na 100%, doplnil jsem tam podmínku, že pokud bude rozdíl úhlů menší, než asi +/- 1° (v příkladu je to acos(0.9998)), LEDka se rozbliká frekvencí 2Hz.

Tato třetí verze programu s LED signalizací vypadá následovně:

#!/usr/bin/python
# -*- encoding: utf-8 -*-
# cteni dat z magnetometru HMC5883 a zmena jasu LED pri priblizeni k pozadovanemu azimutu
# posledni uprava 26.5.2013 (zmena prepoctu zapornych hodnot)


import smbus
import time
import math
import RPi.GPIO as GPIO  # Ovladani GPIO vystupu v RasPi


bus = smbus.SMBus(1)  # novejsi varianta RasPi (512MB)
#bus = smbus.SMBus(0)  # starsi varianta RasPi (256MB)

GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)   # cislovani podle GPIO pinu (1 az 26)

pin_led = 26   # ledka, ktera se rozsveci pri dosazeni uhlu je pripojena na GPIO7 (pin26)

GPIO.setup(pin_led, GPIO.OUT)  # zakladni nastaveni a spusteni PWM
p1 = GPIO.PWM(pin_led, 50)     # 50Hz
p1.start(0)                    # 0%


pozadavek = 90    # pozadovana hodnota azimutu ve stupnich (90°=vychod)


#==============
# tento podprogram byl opsan z:
#       https://bitbucket.org/thinkbowl/i2clibraries/

#Returns heading in degrees and minutes
def getHeading(osa_1,osa_2):
		
  headingRad = math.atan2(osa_2, osa_1)
  headingRad += 3.5  * ( math.pi / 180)  # mistni posun magnetickeho polu vuci severnimu polu je 3,5 stupne

  # Correct for reversed heading
  if(headingRad < 0):
    headingRad += 2*math.pi
		
# Check for wrap and compensate
  if(headingRad > 2*math.pi):
    headingRad -= 2*math.pi
			
# Convert to degrees from radians
  headingDeg = headingRad * 180/math.pi
  degrees = int(math.floor(headingDeg))
  minutes = int(round(((headingDeg - degrees) * 60)))
  return (degrees, minutes, headingDeg)
#==============




bus.write_byte_data(0x1e,0x00,0x70) # prumerovani=8 vzorku ; rychlost vzorkovani = 15Hz
bus.write_byte_data(0x1e,0x01,0x00) # citlivost +/- 0.88 Ga (nejvyssi citlivost)
bus.write_byte_data(0x1e,0x02,0x00) # nepretrzite mereni

   



while True:


  # cteni dat z cidla pomoci I2C sbernice
  data = bus.read_i2c_block_data(0x1e,0x03)
  x = (data[0]*256) + data[1]             
  z = (data[2]*256) + data[3] 
  y = (data[4]*256) + data[5]

  # Prepocty pri zapornych hodnotach registru
  # Hodnoty registru se pohybuji v rozsahu -2048 az +2047
  # Pri prekroceni rozsahu je hodnota -4096

  if(x & 0xF000):    # pri zaporne hodnote jsou nejvyssi 4 bity ze 16 nastavene na "1"
    x = x & 0xFFF    # v tom pripade orezat nejvyssi 4 bity ...
    x = -0x1000 + x  # ... a z toho co zbyde spocitat dvojkovy doplnek

  if(y & 0xF000):    # pri zaporne hodnote jsou nejvyssi 4 bity ze 16 nastavene na "1"
    y = y & 0xFFF    # v tom pripade orezat nejvyssi 4 bity ...
    y = -0x1000 + y  # ... a z toho co zbyde spocitat dvojkovy doplnek

  if(z & 0xF000):    # pri zaporne hodnote jsou nejvyssi 4 bity ze 16 nastavene na "1"
    z = z & 0xFFF    # v tom pripade orezat nejvyssi 4 bity ...
    z = -0x1000 + z  # ... a z toho co zbyde spocitat dvojkovy doplnek



# pri otaceni vodorovne polozeneho plosnacku se prepocitavaji osy "X" a "Y". Osa "Z" se ignoruje.
  natoceni = getHeading(x , y)
  aktualne = natoceni[2]          # v promenne natocen[2] je azimut ve stupnich (desetinne cislo)
  
  rozdil = aktualne - pozadavek   # 'dotoceni' rozdilu pri prekroceni nuloveho azimutu
  if (rozdil < 0): rozdil = rozdil + 360
  if (rozdil >= 360): rozdil = rozdil -360
    
# Kolem nuloveho rozdilu uhlu je vysledek co nejvetsi cislo (100%).
# Na obe strany postupne klesa az do rozdilu 90° (0%). 
  kosinus = (math.cos((rozdil * math.pi) / 180)) * 100  
  if (kosinus < 0) : kosinus = 0      # kdyz je cidlo namireno na opacnou stranu, LED nesviti vubec


# Signalizace priblizeni pozadovanemu azimutu pomoci PWM rizeni jasu LEDky
# cim presnejsi je natoceni cidla k pozadovane hodnote, tim je sirsi sirka impulzu do LEDky (tim vic LED sviti)
# kdyz presnost prekroci urcitou hranici, neni uz mozne rozpoznat zmenu jasu LED, a proto se LED rozblika

  if (kosinus > 99.98):          # sirka pasma signalizace presneho nastaveni
                                 #    (odchylka od pozadovaneho uhlu = acos(0.9998) = asi 1.15°)
    p1.ChangeFrequency(2)        # blikani LED s frekvenci 2Hz ...
    p1.ChangeDutyCycle(50)       # ...  a stridou 50%
  else:
    p1.ChangeFrequency(50)       # blikani LED s frekvenci 50Hz ...
    p1.ChangeDutyCycle(kosinus)  # ... a stridou zavislou na rozdilu pozadovane a aktualni hodnoty natoceni


  time.sleep(0.01)

Video s ukázkou signalizace

Neúspěch :-(

Všechny předchozí příklady předpokládaly, že je čidlo umístěné vodorovně. V tom případě se při výpočtu azimutu ignorovala osa Z.

Pokud ale není možné zajistit vodorovnou polohu, je třeba provést ještě další vylepšení. To se týkalo hlavně mého případu, kdy jsem chtěl pomocí magnetometru mířit foťákem na nějaké místo na obloze.

Toto vylepšení vyžaduje připojení náklonoměru k magnetometru (funkci náklonoměru jsem popisoval tady).
Podle naměřených údajů o naklonění čidla se pak do výpočtu azimutu ještě zapojuje kromě os X a Y, ještě třetí osa (Z)

Při tvorbě programu jsem vycházel z tohoto článku:
https://www.loveelectronics.co.uk/Tutorials/13/tilt-compensated-compass-arduino-tutorial

Vytvořil jsem si model letadla, na který jsem přidělal magnetometr a čidlo náklonu:

Proti příkladu na stránkách loveelectronics.... jsem měl obě čidla posazené kolmo k vodorovné rovině.
Výpočet úhlů natočení (roll a pitch) nebyl problém. V běžných mezích se mi úhly natočení měnily od -90° do +90°. V první fázi jsem se nezabýval tím, že by bylo "letadlo" vzhůru nohama - tam se mi ty úhly počítaly špatně. Stačilo by tam ale dodat ještě jednu podmínku, při které by se při překročení 90° ještě to "PI / 2" odečetlo.

Za žádnou cenu se mi ale nepodařilo zprovoznit kompas tak, aby při různém náklonu ukazoval pořád stejný azimut.
Tady je video, které ukazuje naměřené a vypočtené hodnoty polohy a směru.

V prvním sloupci je vypočtená hodnota "roll" - naklonění letadla vlevo, nebo vpravo (v radiánech) - to se zdá být v pořádku.
Druhý sloupec ukazuje "pitch" - naklonění letadla nahoru, nebo dolu (také v radiánech) - i tohle je správné.
Třetí až pátý sloupec jsou neupravené hodnoty z magnetomeru (osa X, osa Y a osa Z) - je vidět, že při vodorovné poloze se při otáčení "yaw" mění hodnoty v osách X a Z. Osa Y zůstává téměř beze změny.
Poslední sloupec zobrazuje vypočtený azimut (ve stupních) - to už je špatně. Při rotacích "pitch" nebo "roll" se ten azimut výrazně mění.

Nefunkční program vypadá takhle: magnet4.py

Jestli máte někdo nějaké nápady, jak to zprovoznit, napište mi...