Aus RN-Wissen.de
Version vom 11. Mai 2013, 13:24 Uhr von Robonooby (Diskussion | Beiträge) (→‎1. Grundlagen zum Koordinatensystem:)

Wechseln zu: Navigation, Suche
Rasenmaehroboter Test

Da ich jetzt nach langer Zeit meinen LSM303DLH Sensor zum laufen gebracht habe und kaum Informationen für Bascom hierzu zu finden sind, möchte ich euch hiermit mein erlangtes Wissen zur Verfügung stellen. Dies mein erster RN-Wissensbeitrag, daher möchte ich mich hiermit schonmal für Fehler und unschöne Formulierungen entschuldigen.

Das Sensormodul:

Verwendet hab ich das Board von Pololu welches den Sensor LSM303DLH und die komplette Spannungsregelungselektronik enthält. Jedoch funktioniert auch jedes andere Sensormodul mit dem LSM303DLH drauf.

Technische Daten:

LSM303DLH Sensormodul von Pololu von unten
Abmessungen: 13 × 23 × 3 mm
Gewicht (ohne Pins): 0.84 g
Versorgungsspannung: 2.6 bis 5.5 V
Strom: 10 mA
Anschlussart: I2C
Accelerometer: 12-bit links geshiftet (pro Achse)
Magnetometer: 12-bit rechts geshiftet (pro Achse)
Empfindlichkeit Accelerometer: ±2, ±4, or ±8 g
Empfindlichkeit Magnetometer: ±1.3, ±1.9, ±2.5, ±4.0, ±4.7, ±5.6, or ±8.1 gauss

Anschluss:

LSM303DLH Sensormodul von Pololu von oben mit Pinbelegung

Der Sensor wird an VIN mit der Spannung versorgt (2,6-5,5V) An SCL und SDA kommen die I2C Leitungen dran. Und an GND die Masse. Die restlichen Belegungen werden nur bei Bedarf benötigt. Ich habe zum Beispiel an 3V Out eine LED angeschlossen. Mit der RN-Control von Roboternetz lässt sich das ganze sehr schön anschließen, da vom 10 Poligen I2C Kabel auch die Spannung mitgeführt wird. Ich hab den Sensor in ein kleines Gehäuse gepackt und außen am Gehäuse ein 10-poliger Wannenstecker angebracht.


Programmierung unter Bascom:

Anbei werde ich Schritt für Schritt den Code durchgehen, der zum Auslesen des Sensormoduls notwendig ist.

1. Variablen Deklaration:

Für den Sensor sind sehr viele Variablen nötig. Diese werden als erstes deklariert. Hierzu zählen auch die Adressdaten der Sensoren und deren Register (Eine genauere Beschreibung hierzu findest du im Handbuch): 

'Variablen für Beschleunigungs Sensor:
Const Acc_w_addr = &H30			‘Beschleunigungssensor Read Adresse
Const Acc_r_addr = &H31			‘Beschleunigungssensor Write Adresse

Const Ctrl_reg1_a = &H20		‘Beschleunigungssensor Einstelladresse 1
Const Ctrl_reg4_a = &H23		‘Beschleunigungssensor Einstelladresse 4

Const Out_x_l_a = &H28			‘Beschleunigungssensor Ausgabedaten
Const Out_x_h_a = &H29
Const Out_y_l_a = &H2A
Const Out_y_h_a = &H2B
Const Out_z_l_a = &H2C
Const Out_z_h_a = &H2D

'Variablen für Magnetfeld Sensor :
Const Magnet_w_addr = &H3C                                       ' Magnetfeldsensor Read Adresse                                
Const Magnet_r_addr = &H3D                                       ' Magnetfeldsensor Write Adresse

Const Cra_reg_m = &H00                                       ' Magnetfeldsensor Einstelladresse A
Const Crb_reg_m = &H01                                       ' Magnetfeldsensor Einstelladresse B
Const Mr_reg_m = &H02                                       ' Magnetfeldsensor Einstelladresse Mr Reg M

Const Out_x_h_m = &H03                                      ' Magnetfeldsensor Ausgabedaten
Const Out_x_l_m = &H04
Const Out_y_h_m = &H05
Const Out_y_l_m = &H06
Const Out_z_h_m = &H07
Const Out_z_l_m = &H08

'***********************
Dim Slaveadress As Byte                                     'Slaveadresse zum Schreiben und Lesen
Dim Subadress As Byte                                       ‘Registeradresse welche angesprochen warden soll


Dim Iax As Integer                                     'Variablen für Beschleunigungssensor
Dim Iay As Integer
Dim Iaz As Integer


Dim Xha As Byte                                     'Variablen für Beschleunigungssensor (High-Byte X Richtung)
Dim Xla As Byte                                     'Variablen für Beschleunigungssensor (Low-Byte X Richtung)
Dim Yha As Byte                                     'Variablen für Beschleunigungssensor (High-Byte Y Richtung)
Dim Yla As Byte                                     'Variablen für Beschleunigungssensor (Low-Byte Y Richtung)
Dim Zha As Byte                                     'Variablen für Beschleunigungssensor (High-Byte Z Richtung)
Dim Zla As Byte                                     'Variablen für Beschleunigungssensor (Low-Byte Z Richtung)


Dim Imx As Integer                                     'Variablen für Magnetsensor
Dim Imy As Integer
Dim Imz As Integer

Dim Xhm As Byte                                     'Variablen für Magnetsensor (High-Byte X Richtung)
Dim Xlm As Byte                                     'Variablen für Magnetsensor (Low-Byte X Richtung)
Dim Yhm As Byte                                     'Variablen für Magnetsensor (High-Byte Y Richtung)
Dim Ylm As Byte                                     'Variablen für Magnetsensor (Low-Byte Y Richtung)
Dim Zhm As Byte                                     'Variablen für Magnetsensor (High-Byte Z Richtung)
Dim Zlm As Byte                                     'Variablen für Magnetsensor (Low-Byte Z Richtung)


Dim Sgx As Single                                           'Beschleunigungsvariablen zur Berechnung
Dim Sgy As Single
Dim Sgz As Single

Dim Xh As Single
Dim Yh As Single
Dim Zh As Single

Dim Pitch As Single
Dim Roll As Single

Dim Magnetx As Single                                       'Magnetfeldvariablen zur Berechnung
Dim Magnety As Single
Dim Magnetz As Single
Dim Richtung As Single

Dim Acckorrektur(3) As Integer                                       'Korrekturwerte für Beschleunigungssensor
Dim Magkorrektur(3) As Integer                                       'Korrekturwerte für Magnetsensor

Dim Hilfsvariable As Single                                       'Hilfsvariablen die zum Berechnen benötigt werden
Dim Hilfsvariable1 As Single
Dim Hilfsvariable2 As Single
Dim Hilfsvariable3 As Single

Dim Xaccmin As Integer                                       'Variablen zum Ermitteln des Minimal Wertes des Beschleunigungssensors
Dim Xaccmax As Integer                                       'Variablen zum Ermitteln des Maximal Wertes des Beschleunigungssensors
Dim Yaccmin As Integer
Dim Yaccmax As Integer
Dim Zaccmin As Integer
Dim Zaccmax As Integer

Dim Xmagmin As Integer                                       'Variablen zum Ermitteln des Minimal Wertes des Magnetsensor
Dim Xmagmax As Integer                                       'Variablen zum Ermitteln des Maximal Wertes des Magnetsensor
Dim Ymagmin As Integer
Dim Ymagmax As Integer
Dim Zmagmin As Integer
Dim Zmagmax As Integer

2. Ports Definieren:

Hier werden nur die I2C Ports definiert. Falls Hardware I2C genutzt werden soll, muss man diesen Bereich entsprechend noch ergänzen. 

Config Scl = Portc.0                                        'I2C SCL Pin
Config Sda = Portc.1                                        'I2C SDA Pin
Config I2cdelay = 20                                        '200    'I2C Bus Geschwindigkeit
I2cinit

3. Korrekturwerte:

Da viele Einflussfaktoren auf den Sensor einwirken müssen entsprechend Korrekturwerte mit einbezogen werden, um diese zu kompensieren. Die Korrekturwerte für den Beschleunigungssensor lassen sich relativ einfach ermitteln. Weiter unten im Kapitel "Korrekturwerte ermitteln" wurde für beide Sensoren das ganze Verfahren mal beschrieben. 

'Korrekturwerte eingeben: (Achtung: Diese müssen für das jeweilige System ermittelt werden
Acckorrektur(1) = 248                                       '[xaccmin + xaccmax]/2 = xaccoff
Acckorrektur(2) = 80                                        '[yaccmin + yaccmax]/2 = yaccoff
Acckorrektur(3) = 448                                       '[zaccmin + zaccmax]/2 = zaccoff

Magkorrektur(1) = -81.5                                     '[xmagmin+xmagmax]/2  =  xmagoff
Magkorrektur(2) = 15                                      '[ymagmin+ymagmax]/2  =  ymagoff
Magkorrektur(3) = 145                                      '[zmagmin+zmagmax]/2  =  zmagoff

Xaccmin = 0
Xaccmax = 0
Yaccmin = 0
Yaccmax = 0
Zaccmin = 0
Zaccmax = 0

Xmagmin = 0
Xmagmax = 0
Ymagmin = 0
Ymagmax = 0
Zmagmin = 0
Zmagmax = 0

4. LSM303DLH Sensoreinstellungen definieren:

Bevor mit dem Sensor gearbeitet werden kann, müssen Einstellungen vorgenommen werden. Hierzu zählen zum Beispiel die Genauigkeit, updaterate sowie Energiemodus funktionen. Genaueres zu den Einstellungen werden im Handbuch ab Kapitel 8des Sensors beschrieben. 

'Funktionen Deklarieren:
Declare Sub Lsm303_init()
Declare Sub Acc_writereg(byval Breg As Byte , Byval Bval As Byte)
Declare Sub Magnet_writerega(byval Dreg As Byte , Byval Dval As Byte)
Declare Sub Magnet_writeregb(byval Ereg As Byte , Byval Eval As Byte)
Declare Sub Magnet_writeregm(byval Mreg As Byte , Byval Mval As Byte

Sub Lsm303_init()                                           'Einstellungen definieren
   Acc_writereg Ctrl_reg1_a , &H2F                          'normal power mode, 100 Hz data rate, all axes enabled    H27 = 50Hz
   Acc_writereg Ctrl_reg4_a , &H80                          'Block Data Update, LSB low, Scale +-2g
   Magnet_writerega Cra_reg_m , &H18                        'Output Rate 75Hz , Normal measurement configuration   H14 = 30Hz
   Magnet_writeregb Crb_reg_m , &H40                        'Gain setting 1,9 Gauss                                  H20=1,3Gauss
   Magnet_writeregm Mr_reg_m , &H00                         ' magnetic sensor into continuous mode
End Sub

Sub Acc_writereg(byval Breg As Byte , Byval Bval As Byte)   'Einstellungen für Beschleunigung
   I2cstart
   I2cwbyte Acc_w_addr
   I2cwbyte Breg
   I2cwbyte Bval
   I2cstop
End Sub

Sub Magnet_writerega(byval Dreg As Byte , Byval Dval As Byte)       ' 1. Einstellungen für Magnet
   I2cstart
   I2cwbyte Magnet_w_addr
   I2cwbyte Dreg
   I2cwbyte Dval
End Sub

Sub Magnet_writeregb(byval Ereg As Byte , Byval Eval As Byte)       ' 2. Einstellungen für Magnet
   I2cstart
   I2cwbyte Magnet_w_addr
   I2cwbyte Ereg
   I2cwbyte Eval
End Sub

Sub Magnet_writeregm(byval Mreg As Byte , Byval Mval As Byte)       '3. Einstellung für Magnet
   I2cstart
   I2cwbyte Magnet_w_addr
   I2cwbyte Mreg
   I2cwbyte Mval
End Sub

5. Beschleunigungssensor auslesen:

In der Funktion "Acc_readsensordata()" wird der Sensor angesprochen und die Daten vom Sensor gelesen. Da der Sensor die Werte immer für jede Achse in einem High-Byte (8-Bit) und einem Low-Byte (8-Bit) sendet müssen diese dann zu einem 16-Bit Wert mittels Shift Befehl zusammengeführt werden. Genaueres hierzu findet man auch unter dem Stichwort "Zweierkomplement". Anschließend werden die ermittelten Werte durch einen Faktor korrigiert. Nun werden Pitch- und Rollwinkel in der Funktion "Acc_calcgvalues()" berechnet und in Gradmaß ausgegeben. Hier musste ich nochmal ein Korrekturfakto einführen, da mein Sensor in der falschen Lage montiert wurde. 

'Funktionen Deklarieren:
Declare Sub Acc_readsensordata()                            
Declare Sub Acc_calcgvalues()                               

Sub Acc_readsensordata()		‘Beschleunigungssensorwerte auslesen
    Subadress = Out_x_l_a                                   '&H28

   Subadress.7 = 1			
   I2cstart
   I2cwbyte Acc_w_addr                                      '&H30         'SAD+W
   I2cwbyte Subadress
   I2crepstart
   I2cwbyte Acc_r_addr                                      'SAD+R     Acc_r_addr = &H31


   I2crbyte Xla , Ack			‘Daten kommen vom Sensor und werden in den Variablen gespeichert
   I2crbyte Xha , Ack
   I2crbyte Yla , Ack
   I2crbyte Yha , Ack
   I2crbyte Zla , Ack
   I2crbyte Zha , Nack
   I2cstop

   Iax = Xha		‘Empfangene Daten werden zusammen geführt (Low Byte und High Byte)
   Shift Iax , Left , 8
   Iax = Iax + Xla

   Iay = Yha
   Shift Iay , Left , 8
   Iay = Iay + Yla

   Iaz = Zha
   Shift Iaz , Left , 8
   Iaz = Iaz + Zla

    Iax = Iax - Acckorrektur(1)		‘Korrekturwerte werden von den Sensorwerten abgezogen
    Iay = Iay - Acckorrektur(1)
    Iaz = Iaz - Acckorrektur(1)


End Sub

Sub Acc_calcgvalues()		‘Beschleunigungssensorwerte berechnen

   Sgx = Iax
   Sgy = Iay
   Sgz = Iaz

   Pitch = Atn2(sgz , Sgx) 		‘Pitch Neigungswinkel errechnen (Bogenmaß)
   Roll = Atn2(sgz , Sgy)			‘Roll Neigungswinkel errechnen (Bogenmaß)

   Pitch = Pitch + Deg2rad(89.536552426)                         'Korrekturwert  /von mir ermittelter Wert
   Roll = Roll + Deg2rad(89.536552426)                           'Korrekturwert 

   Print "Pitch: " ; Rad2deg(pitch) ; "    " ; "Roll " ; Rad2deg(roll)   (Ausgabe von Pitch und Roll in Gradmaß)

End Sub

5. Magnetsensor auslesen:

In der Funktion "Magnet_readsensordate()" wird der Sensor angesprochen und die Daten vom Sensor gelesen. Da der Sensor die Werte immer für jede Achse in einem High-Byte (8-Bit) und einem Low-Byte (8-Bit) sendet müssen diese dann zu einem 16-Bit Wert mittels Shift Befehl zusammengeführt werden. Genaueres hierzu findet man auch unter dem Stichwort "Zweierkomplement". Anschließend werden die ermittelten Werte durch einen Faktor korrigiert. Nun werden diese in der Funktion "Magnet_calcmvalues()" mit den Beschleunigungswerten neigungskompensiert und in Gradmaß ausgegeben. Weiter unten gibts dann noch ein Kapitel zur Berechnung der Neigungskompensation. 

'Funktionen Deklarieren:
Declare Sub Magnet_readsensordata()
Declare Sub Magnet_calcmvalues()

Sub Magnet_readsensordata()		‘Magnetsensorwerte auslesen

   Subadress = Out_x_h_m                                    '&H03

   I2cstart
   I2cwbyte Magnet_w_addr                                   'SAD+W   '&H3C
   I2cwbyte Subadress
   I2crepstart
   I2cwbyte Magnet_r_addr                                   'SAD+R         Magnet_r_addr = &H3D

   I2crbyte Xhm , Ack                                       'Sensorwerte empfangen und in Xhm Variable schreiben (High Byte)
   I2crbyte Xlm , Ack                                      'Sensorwerte empfangen und in Xlm Variable schreiben (Low Byte)
   I2crbyte Yhm , Ack
   I2crbyte Ylm , Ack
   I2crbyte Zhm , Ack
   I2crbyte Zlm , Nack
   I2cstop

   Imx = Xhm
   Shift Imx , Left , 8                                     'Low Byte und High Byte zusammenführen
   Imx = Imx + Xlm

   Imy = Yhm
   Shift Imy , Left , 8
   Imy = Imy + Ylm

   Imz = Zhm
   Shift Imz , Left , 8
   Imz = Imz + Zlm

   Imx = Imx - Magkorrektur(1)                              'Korrekturwerte von ermittelten Werten abziehen
   Imy = Imy - Magkorrektur(2)
   Imz = Imz - Magkorrektur(3)
   
End Sub

Sub Magnet_calcmvalues()		‘Magnetsensorwerte berechnen

    Magnetx = Imx
    Magnety = Imy
    Magnetz = Imz
 
    Hilfsvariable1 = Magnetx * Cos(pitch)		‘Neigungskompensation einberechnen
    Hilfsvariable2 = Magnetz * Sin(pitch)
    Xh = Hilfsvariable1 + Hilfsvariable2

    Hilfsvariable1 = Magnetx * Sin(pitch)
    Hilfsvariable1 = Hilfsvariable1 * Sin(roll)
    Hilfsvariable2 = Magnety * Cos(roll)
    Hilfsvariable3 = Magnetz * Sin(roll)
    Hilfsvariable3 = Hilfsvariable3 * Cos(pitch)
    Yh = Hilfsvariable1 + Hilfsvariable2
    Yh = Yh - Hilfsvariable3
    Hilfsvariable1 = -1					'Hier musste ich mit -1 multiplizieren da der Kompass Wert sonst invertiert wurde. 
    Yh = Yh * Hilfsvariable1			        'Dies liegt am falschen Einbau des Sensors. Kann bei dir also anderst sein.

    Hilfsvariable1 = Magnetx * Cos(roll)
    Hilfsvariable1 = Hilfsvariable1 * Sin(pitch)
    Hilfsvariable2 = Magnety * Sin(roll)
    Hilfsvariable3 = Magnetz * Cos(roll)
    Hilfsvariable3 = Hilfsvariable3 * Cos(pitch)
    Zh = Hilfsvariable2 - Hilfsvariable1
    Zh = Zh + Hilfsvariable3

    Richtung = Yh / Xh                                      
    Richtung = Atn(richtung)                                  'Richtung in Bogenmaß berechnen
    Richtung = Rad2deg(richtung)                              'Richtung in Gradmaß umrechnen 

    If Xh > 0 And Yh >= 0 Then                              'Schauen in welchem Quadranten des Tangens Wertes die Richtung liegt
       Richtung = Richtung
    End If

    If Xh < 0 Then
       Richtung = Richtung + 180
    End If

    If Xh > 0 And Yh <= 0 Then
       Richtung = Richtung + 360
    End If

    If Xh = 0 And Yh < 0 Then
       Richtung = 90
    End If

    If Xh = 0 And Yh > 0 Then
       Richtung = 270
    End If

    Print "Richtung: " ; Richtung
    Print ""
End Sub

Berechnungsgrundlagen und Korrekturwerte des Sensors bestimmen:

1. Grundlagen zum Koordinatensystem:

  • Koordinatensystem für Sensorberechnung

  • Koordinatensystem für Sensorberechnung

In den beiden rechten Bildern ist zu sehen wie die Koordinatenrichtungen für den Sensor liegen müssen.

hier folgen noch weitere Einträge ....

2. Wichtigste Formeln zur Berechnung:

Formeln für Sensorberechnung

hier folgen noch weitere Einträge ....

3. Korrekturwerte für Beschleunigungssensor bestimmen:

hier folgen noch weitere Einträge ....

3. Korrekturwerte für Magnetsensor bestimmen:

hier folgen noch weitere Einträge ....


Siehe auch:

Weblinks:

Literatur:

  1. AN996 (Microchip)
  2. "Applications of Magnetoresistive Sensors in Navigation Systems" (Honeywell Inc.)
  3. Applications of Magnetic Sensors for Low Cost Compass Systems (Honeywell Inc.)
  4. AN00022 (PHILIPS)
  5. AN4248 (Freescale Semiconductor)

Datenblätter und Co:

  1. Pololu Homepage
  2. Datenblatt LSM303DLH
  3. Pololu Copass App Note
Von „https://rn-wissen.de/wiki/index.php?title=Bascom_und_Kompass_LSM303DLH&oldid=22613“

LiFePO4 Speicher Test