LSM303DLH 3D Compass and accelerometer Umrechnungsporblem
Hallo
ich habe mich seit heute früh an den LSM303DLH Magnetfeld- und Beschleunigungssensor rangesetzt.
Ich bin soweit gekommen, dass ich alle Werte für Beschleunigung und Magnetfeld via I2C Bus aus dem Sensor bekomme (diese verändern sich auch entsprechend der Bewegung).
Jetzt hab ich allerdings noch nen kleines Verständnisproblem bei der Umrechnung in brauchbare Werte (Grad und Beschleunigung).
In der Dokumentation steht:
Code:
MR_Data[0] = ReadCurrentAddress(); //read OUT_X_H_M (MSB)
MR_Data[1] = ReadCurrentAddress(); //read OUT_X_L_M (LSB)
MR_Data[2] = ReadCurrentAddress(); //read OUT_Y_H_M (MSB)
MR_Data[3] = ReadCurrentAddress(); //read OUT_Y_L_M (LSB)
MR_Data[4] = ReadCurrentAddress();//read OUT_Z_H_M (MSB)
MR_Data[5] = ReadCurrentAddress(); //read OUT_Z_L_M (LSB)
Mx = (int) (MR_Data[0] << 8) + MR_Data[1];
My = (int) (MR_Data[2] << 8) + MR_Data[3];
Mz = (int) (MR_Data[4] << 8) + MR_Data[5];
Aber wie übertrage ich dies auf Bascom befehle. Also die letzten 3 Zeilen. Was bedeutet dieses << und wieso wird da dann die andere MR_Data[] hinzuaddiert.
Ich hab jetzt hier noch nen anderes Beispiel zur Programmierung vom Beschleunigungssensor gefunden, aber ich kann hier keinen Zusammenhang zwischen dem im Datenblatt stehenden Beispiel und der Version von diesem Threat erkennen.
Auch wenn die Werte aus dem Threat sehr plausibel erscheinen.
Ich bekomm folgende Daten aus dem Sensor: (Beispiel Beschleunigung in x Richtung, Sensor liegt dabei irgendwie auf dem Tisch)
Messung |
OUT_X_L_A |
OUT_X_H_A |
Umgerechnet in Beschleunigung aus anderem Threat |
1 |
01100000 |
00011001 |
0.40 g |
2 |
10000000 |
00011001 |
0.40 g |
3 |
01100000 |
00011001 |
0.40 g |
Ich versteh auch nicht wie sich das X_L_A und X_H_A zusammensetz. Beides ist ja wohl für den X Wert zuständig.
Ich hoff mir kann da jemand helfen.
Hier mal mein Code, falls ihr den benötigt:
Code:
'Microcontroller Einstellungen Definieren.
$lib "lcd_i2c.lib"
$regfile = "m32def.dat"
$framesize = 60 '32
$swstack = 60 ' 32
$hwstack = 60 '32
$crystal = 16000000
$baud = 9600
'Funktionen für Sensor:
Declare Sub Lsm303_init()
Declare Sub Acc_writereg(byval Breg As Byte , Byval Bval As Byte)
Declare Sub Acc_readsensordata() '-> iAx, iAy, iAz
Declare Sub Acc_calcgvalues() '-> sGx, sGy, sGz
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)
Declare Sub Magnet_readsensordata()
Declare Sub Magnet_calcmvalues()
'Variablen für Beschleunigungs Sensor:
Const Acc_w_addr = &H30
Const Acc_r_addr = &H31
Const Ctrl_reg1_a = &H20
Const Ctrl_reg4_a = &H23
Const Out_x_l_a = &H28
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
Const Magnet_r_addr = &H3D
Const Cra_reg_m = &H00
Const Crb_reg_m = &H01
Const Mr_reg_m = &H02
Const Out_x_h_m = &H03 ' Magnetfeld
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
Const Res_2g = 0.0009765625 'Auflösung bei +- 2g
Dim Abaccdata(6) As Byte
Dim Abmagdata(6) As Byte
Dim Iax As Integer At Abaccdata(1) Overlay
Dim Iay As Integer At Abaccdata(3) Overlay
Dim Iaz As Integer At Abaccdata(5) Overlay
Dim Imx As Integer At Abmagdata(1) Overlay
Dim Imy As Integer At Abmagdata(3) Overlay
Dim Imz As Integer At Abmagdata(5) Overlay
Dim Sgx As Single 'Beschleunigungsvariablen
Dim Sgy As Single
Dim Sgz As Single
Dim Magnetx As Single 'Magnetfeldvariablen
Dim Magnety As Single
Dim Magnetz As Single
'****************************I2C##******************
Config Scl = Portc.0 'I2C SCL Pin
Config Sda = Portc.1 'I2C SDA Pin
Config I2cdelay = 20 '200 'I2C Bus Geschwindigkeit
I2cinit
Print "LSM303DLH Beschleunigungssensor und Magnometer"
Lsm303_init
Wait 2
Acc_readsensordata
Acc_calcgvalues
Magnet_readsensordata
Magnet_calcmvalues
Print "Beschleunigung: X: " ; Fusing(sgx , "#.##") ; " Y: " ; Fusing(sgy , "#.##") ; " Z: " ; Fusing(sgz , "#.##") 'Aus anderem Threat übernommen
Print "Magnetfeld: X: " ; Fusing(magnetx , "#.##") ; " Y: " ; Fusing(magnety , "#.##") ; " Z: " ; Fusing(magnetz , "#.##") 'Aus anderem Threat übernommen
Waitms 500
Loop
End
'LSM303DLH Sensor auslesen (Funktionen)
Sub Lsm303_init() 'Einstellungen definieren
Acc_writereg Ctrl_reg1_a , &H27 'normal power mode, 50 Hz data rate, all axes enabled
Magnet_writerega Cra_reg_m , &H14 'Output Rate 30Hz , Normal measurement configuration
Magnet_writeregb Crb_reg_m , &H20 'Gain setting 1,3 Gauss
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
Sub Acc_readsensordata()
I2cstart
I2cwbyte Acc_w_addr
I2cwbyte &HA8
I2cstart
I2cwbyte Acc_r_addr
I2crbyte Abaccdata(1) , Ack
I2crbyte Abaccdata(2) , Ack
I2crbyte Abaccdata(3) , Ack
I2crbyte Abaccdata(4) , Ack
I2crbyte Abaccdata(5) , Ack
I2crbyte Abaccdata(6) , Nack
I2cstop
Print "xxxx " ; Bin(abaccdata(1)) ; " xx " ; Bin(abaccdata(2))
Shift Iax , Right , 4 , Signed 'Aus anderem Threat übernommen
Shift Iay , Right , 4 , Signed 'Aus anderem Threat übernommen
Shift Iaz , Right , 4 , Signed 'Aus anderem Threat übernommen
End Sub
Sub Acc_calcgvalues()
Sgx = Iax * Res_2g 'Aus anderem Threat übernommen
Sgy = Iay * Res_2g 'Aus anderem Threat übernommen
Sgz = Iaz * Res_2g 'Aus anderem Threat übernommen
End Sub
Sub Magnet_readsensordata()
Dim Temp As Byte
I2cstart
I2cwbyte Magnet_w_addr
I2cwbyte Out_x_h_m
I2cstart
I2cwbyte Magnet_r_addr
I2crbyte Abmagdata(1) , Ack
I2crbyte Abmagdata(2) , Ack
I2crbyte Abmagdata(3) , Ack
I2crbyte Abmagdata(4) , Ack
I2crbyte Abmagdata(5) , Ack
I2crbyte Abmagdata(6) , Nack
I2cstop
End Sub
Sub Magnet_calcmvalues()
Magnetx = Imx
Magnety = Imy
Magnetz = Imz
End Sub
Liste der Anhänge anzeigen (Anzahl: 1)
Hallo
ich will das Thema nochmal aufgreifen. hab jetzt nach langer Zeit wieder zeit für mein Projekt gefunden.
Leider komm ich nicht so richtig vorran. Hab schon das Handbuch gewälzt aber komm einfach net weiter, daher hoffe ich, dass ihr mir helfen könnt.
Und zwar bekomm ich keine sinnvollen Werte aus dem Sensor raus. Beschleunigung geht, aber nur wenn ich das nicht direkt nach Handbuch mache.
Magnetkompass bekomm ich gar nicht hin....
hier erstmal mein Code (direkt nach Handbuch):
Code:
'LSM303DLH Sensor auslesen (Funktionen)
'Einstellungen:
Sub Lsm303_init() 'Einstellungen definieren
Acc_writereg Ctrl_reg1_a , &H27 'normal power mode, 50 Hz data rate, all axes enabled
Magnet_writerega Cra_reg_m , &H14 'Output Rate 30Hz , Normal measurement configuration
Magnet_writeregb Crb_reg_m , &H20 'Gain setting 1,3 Gauss
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
Code:
'Auslesen der Daten und Auswertung:
Sub Acc_readsensordata() 'Beschleunigungssensor auslesen
I2cstart 'ST
I2cwbyte Acc_w_addr 'SAD + W
I2cwbyte &HA8 'SUB = OUT_X_L_A, SUB(7) = 1
I2cstart 'SR
I2cwbyte Acc_r_addr 'SAD + R
I2crbyte Abaccdata(1) , Ack
I2crbyte Abaccdata(2) , Ack
I2crbyte Abaccdata(3) , Ack
I2crbyte Abaccdata(4) , Ack
I2crbyte Abaccdata(5) , Ack
I2crbyte Abaccdata(6) , Nack
I2cstop
Shift Iax , Right , 4 , Signed
Shift Iay , Right , 4 , Signed
Shift Iaz , Right , 4 , Signed
Sgx = Iax
Sgy = Iay
Sgz = Iaz
Iax = Abaccdata(1)
Shift Iax , Left , 8 , Signed
Iax = Iax + Abaccdata(2)
Iay = Abaccdata(3)
Shift Iay , Left , 8 , Signed
Iay = Iay + Abaccdata(4)
Iaz = Abaccdata(5)
Shift Iaz , Left , 8 , Signed
Imz = Iaz + Abaccdata(6)
End Sub
Sub Acc_calcgvalues() 'ausgelesene Beschleunigungswerte Auswerten
'Sgx = Sgx * Res_2g
'Sgy = Sgy * Res_2g
'Sgz = Sgz * Res_2g
Hilfsvariable1 = Acckorrektur(1) * Iax 'Norminalisierung mit Korrekturwerten X Richtung (Formel 3 Siehe Bild)
Hilfsvariable2 = Acckorrektur(2) * Iay
Hilfsvariable3 = Acckorrektur(3) * Iaz
Hilfsvariable = Hilfsvariable1 + Hilfsvariable2
Hilfsvariable = Hilfsvariable + Hilfsvariable3
Sgx = Hilfsvariable + Acckorrektur(4)
Hilfsvariable1 = Acckorrektur(5) * Iax 'Norminalisierung mit Korrekturwerten Y Richtung
Hilfsvariable2 = Acckorrektur(6) * Iay
Hilfsvariable3 = Acckorrektur(7) * Iaz
Hilfsvariable = Hilfsvariable1 + Hilfsvariable2
Hilfsvariable = Hilfsvariable + Hilfsvariable3
Sgy = Hilfsvariable + Acckorrektur(8)
Hilfsvariable1 = Acckorrektur(9) * Iax 'Norminalisierung mit Korrekturwerten Z Richtung
Hilfsvariable2 = Acckorrektur(10) * Iay
Hilfsvariable3 = Acckorrektur(11) * Iaz
Hilfsvariable = Hilfsvariable1 + Hilfsvariable2
Hilfsvariable = Hilfsvariable + Hilfsvariable3
Sgz = Hilfsvariable + Acckorrektur(12)
Pitch = -asin(sgx) '(Formel 10 Siehe Bild)
Hilfsvariable = Cos(pitch)
Hilfsvariable = Sgy / Hilfsvariable
Roll = Asin(hilfsvariable)
End Sub
Sub Magnet_readsensordata() 'Magnetsensordaten auslesen
Dim Temp As Byte
I2cstart
I2cwbyte Magnet_w_addr
I2cwbyte Out_x_h_m
I2cstart
I2cwbyte Magnet_r_addr
'I2cwbyte Magnet_w_addr
'I2cwbyte Mr_reg_m
I2crbyte Abmagdata(1) , Ack
I2crbyte Abmagdata(2) , Ack
I2crbyte Abmagdata(3) , Ack
I2crbyte Abmagdata(4) , Ack
I2crbyte Abmagdata(5) , Ack
I2crbyte Abmagdata(6) , Nack
I2cstop
Imx = Abaccdata(1)
Shift Imx , Left , 8 , Signed
Imx = Imx + Abaccdata(2)
Imy = Abaccdata(3)
Shift Imy , Left , 8 , Signed
Imy = Imy + Abaccdata(4)
Imz = Abaccdata(5)
Shift Imz , Left , 8 , Signed
Imz = Imz + Abaccdata(6)
End Sub
Sub Magnet_calcmvalues() 'ausgelesene Magnetsensorwerte Auswerten
Dim Xh As Single
Dim Yh As Single
Dim Zh As Single
Imx = Imx - Magkorrektur(4)
Imy = Imy - Magkorrektur(8)
Imz = Imz - Magkorrektur(12)
Hilfsvariable1 = Magkorrektur(1) * Imx 'Norminalisierung mit Korrekturwerten X Richtung (Formel 4 Siehe Bild)
Hilfsvariable2 = Magkorrektur(2) * Imy
Hilfsvariable3 = Magkorrektur(3) * Imz
Hilfsvariable = Hilfsvariable1 + Hilfsvariable2
Magnetx = Hilfsvariable + Hilfsvariable3
Hilfsvariable1 = Magkorrektur(5) * Imx 'Norminalisierung mit Korrekturwerten Y Richtung
Hilfsvariable2 = Magkorrektur(6) * Imy
Hilfsvariable3 = Magkorrektur(7) * Imz
Hilfsvariable = Hilfsvariable1 + Hilfsvariable2
Magnety = Hilfsvariable + Hilfsvariable3
Hilfsvariable1 = Magkorrektur(8) * Imx 'Norminalisierung mit Korrekturwerten Z Richtung
Hilfsvariable2 = Magkorrektur(10) * Imy
Hilfsvariable3 = Magkorrektur(11) * Imz
Hilfsvariable = Hilfsvariable1 + Hilfsvariable2
Magnetz = Hilfsvariable + Hilfsvariable3
Hilfsvariable1 = Magnetx * Cos(pitch) 'X-Wert ausrechnen (Formel 12 Siehe Bild)
Hilfsvariable2 = Magnetz * Sin(pitch)
Xh = Hilfsvariable1 + Hilfsvariable2
Hilfsvariable1 = Magnetx * Sin(pitch) 'Y-Wert ausrechnen
Hilfsvariable1 = Hilfsvariable1 * Sin(roll)
Hilfsvariable2 = Magnety * Cos(roll)
Hilfsvariable3 = Magnetz * Sin(roll)
Hilfsvariable3 = Hilfsvariable3 * Cos(pitch)
Yh = Hilfsvariable1 + Hilfsvariable2
Yh = Yh - Hilfsvariable3
Hilfsvariable1 = Magnetx * Cos(roll) 'Z-Wert ausrechnen
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 in Grad bestimmen (Formel 13 Siehe Bild)
Richtung = Atn(richtung)
Richtung = Rad2deg(richtung)
If Xh > 0 And Yh >= 0 Then 'schauen in welchem Quadranten der Wert liegt (wegen Tangens) (Formel 13 Siehe Bild)
Richtung = Richtung
End If
If Xh < 0 Then
Richtung = 180 + Richtung
End If
If Xh > 0 And Yh <= 0 Then
Richtung = 360 + Richtung
End If
If Xh = 0 And Yh < 0 Then
Richtung = 90 + Richtung
End If
If Xh = 0 And Yh > 0 Then
Richtung = 270 + Richtung
End If
End Sub
Mein Problem:
Es ändern sich die Werte ständig obwohl der Sensor einfach nur fest montiert ist und sich nicht bewegt. Und ich weis nicht warum.
Wenn ich den Sensor um z.B. 90 Grad drehe dann bleiben die Werte immer noch so in dem Bereich. Am Winkel und Pitch ändert sich auch so gut wie nichts...
Hoffe da kann mir jemand weiterhelfen. Wenn das läuft würd ich dann auch mal nen Tutorial hierzu ins Wiki stellen wollen.
Anbei ein paar Messdaten. (erste zwei Messungen stehender Sensor, andere Messungen Sensor in Bewegung)
Messung 1:
Zitat:
Beschleunigung: X: -4626.00 Y: -4626.00 Z: -4626.00
Magnetfeld: X: -4119.00 Y: -4119.00 Z: -4119.00
Pitch: 1.570796366 Roll: 1.570796366
Richtung: 224.999618528
Messung 2:
Zitat:
Beschleunigung: X: -5654.00 Y: -5654.00 Z: -5654.00
Magnetfeld: X: -5147.00 Y: -5147.00 Z: -5147.00
Pitch: 1.570796366 Roll: 1.570796366
Richtung: 224.999618528
Messung 3:
Zitat:
Beschleunigung: X: -5654.00 Y: -5654.00 Z: -5654.00
Magnetfeld: X: -5147.00 Y: -5147.00 Z: -5147.00
Pitch: 1.570796366 Roll: 1.570796366
Richtung: 224.999618528
Messung 4:
Zitat:
Beschleunigung: X: -5397.00 Y: -5397.00 Z: -5397.00
Magnetfeld: X: -4890.00 Y: -4890.00 Z: -4890.00
Pitch: 1.570796366 Roll: 1.570796366
Richtung: 225.00036621
Messung 5:
Zitat:
Beschleunigung: X: -6424.00 Y: -6424.00 Z: -6424.00
Magnetfeld: X: -5662.00 Y: -5662.00 Z: -5662.00
Pitch: 1.570796366 Roll: 1.570796366
Richtung: 224.999618528
Messung 6:
Zitat:
Beschleunigung: X: -5145.00 Y: -5145.00 Z: -5145.00
Magnetfeld: X: -6169.00 Y: -6169.00 Z: -6169.00
Pitch: 1.570796366 Roll: 1.570796366
Richtung: 224.999618528
Messung 7:
Zitat:
Beschleunigung: X: -5145.00 Y: -5145.00 Z: -5145.00
Magnetfeld: X: -6169.00 Y: -6169.00 Z: -6169.00
Pitch: 1.570796366 Roll: 1.570796366
Richtung: 224.999618528 Grad
Das Steht in der Bedienungsanleitung: (Siehe Bild)
Liste der Anhänge anzeigen (Anzahl: 1)
Also hab das mal gemacht.
dabei ist mir aufgefallen dass die Magnetwerte sich immer nur um den Faktor 256 ändern .
Anhang 25383
Also um die Multiplikation des H_Bytes.
das bedeutet demnach, dass entweder der Magnetsensor defekt ist, was ich nicht hoffe. Oder dass hier beim auslesen ich irgendwas falsch mache.
hier nochmal der Code vom auslesen des Magnetsensor:
Code:
'Variablen für Magnetfeld Sensor :
Const Magnet_w_addr = &H3C '3C 1E
Const Magnet_r_addr = &H3D
Const Cra_reg_m = &H00
Const Crb_reg_m = &H01
Const Mr_reg_m = &H02
Const Out_x_h_m = &H03 ' Magnetfeld
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 Abmagdata(6) As Byte
Dim Imx As Integer At Abmagdata(1) Overlay
Dim Imy As Integer At Abmagdata(3) Overlay
Dim Imz As Integer At Abmagdata(5) Overlay
.......
Sub Magnet_readsensordata()
Dim Temp As Byte
I2cstart
I2cwbyte Magnet_w_addr
I2cwbyte Out_x_h_m
I2cstart
I2cwbyte Magnet_r_addr
'I2cwbyte Magnet_w_addr
'I2cwbyte Mr_reg_m
I2crbyte Abmagdata(1) , Ack
I2crbyte Abmagdata(2) , Ack
I2crbyte Abmagdata(3) , Ack
I2crbyte Abmagdata(4) , Ack
I2crbyte Abmagdata(5) , Ack
I2crbyte Abmagdata(6) , Nack
I2cstop
Imx = Abmagdata(1)
Shift Imx , Left , 8 , Signed
Imx = Imx + Abmagdata(2)
Imy = Abmagdata(3)
Shift Imy , Left , 8 , Signed
Imy = Imy + Abmagdata(4)
Imz = Abmagdata(5)
Shift Imz , Left , 8 , Signed
Imz = Imz + Abmagdata(6)
End Sub