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.
Inhaltsverzeichnis
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:
| 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:
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 oberen 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:
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:
- AN996 (Microchip)
- "Applications of Magnetoresistive Sensors in Navigation Systems" (Honeywell Inc.)
- Applications of Magnetic Sensors for Low Cost Compass Systems (Honeywell Inc.)
- AN00022 (PHILIPS)
- AN4248 (Freescale Semiconductor)
Datenblätter und Co:
