Das Open-Control-Projekt - Die Alternative zur C-Control-I


Das Forum zur C-Control-1
Welche C-Control-Varianten existieren?
Übersicht - Suchen - Neueste 50 Beiträge - Neuer Beitrag - Login - Registrieren
INFO - FAQ - CC2-Forum - CCPro-Forum 

 9-Achsen Sensor LSM303 mit I2C ansteuern Kategorie: Programmierung Basic (von Ralf Rosche - 1.01.2022 17:12)
Ralf Rosche nutzt:  CC1-Unit V1.1, CC1-M-Unit V1.1
Hallo zusammen,
ich habe unten Code angef├╝gt zum Auslesen eines LSM303 Sensors.
Viele andere Compass oder Accelerometer Sensoren funktionieren nach dem gleichen Schema.
Die Breakout Boards sind teilweise unter 5 Euro zu haben.
Auf jeden Fall muss man die Datenbl├Ątter genau studieren.
Ich habe die I2C Assembler Routinen von Dietmar Harlos eingebunden. Diese vermeiden ein H├Ąngenbleiben, wenn der Sensor nicht antwortet.
Man kann solche Sensoren auch gut als Bewegungssensoren verwenden. Auch auf die mathematische Berechung der Lage im Raum kann man verzichten, wenn es nur um Differenzen geht.
Der Sensor liefert Werte im Bereich von +/-32768 f├╝r alle Achsen, Beschleunigungs und Magnetwerte.
Liegt der Sensor gerde im Raum und sind damit Pitch und Roll = 0, dann kann man recht einfach ├╝ber die atan2(magy/magx) Funktion (siehe hier im Forum) die Winkeldifferenz zum Bezugspunt 0 Grad Norden berechnen und somit ein Kompassmodul realisieren.
Weil die St├Ąrken der C-Control nicht in der Mathematik liegen, sind mit der 16Bit Integer Arithmetik Klimmz├╝ge zu machen. Da ist die OpenControl wohl schon weiter :-)
Gruss Ralf




' Ansteuerung des I2C 6-Achsen Sensors LSM303d von STMicroelectronic
' Datasheet: https://www.st.com/en/mems-and-sensors/lsm303d.html
' Applictaion Note : https://www.pololu.com/file/0J434/LSM303DLH-compass-app-note.pdf
' Es gibt verscheidene Breakout Boards, von Seeed, Grove etc.
' Auf richtige Spannungsversorgung und Pullups auf der CLK und Datenleitung achten
' Es gibt 4 verschiedene Varianten des LSM303, sie unterscheiden sich teilweise ganz
' erheblich bei den Registern und der Initialisierung sowie den Adressen.
' Ich beziehe mich hier auf den LSM303D
' Am C-Control Board h├Ąngt er an Pin 1,2
' Ich empfehle den I2C Treiber von Dietmar Harlos, weil dieser robust ist und nicht zum
' H├Ąngenbleiben durch Timeouts f├╝hrt.
' syscode "i2cbasic.s19" muss einmalig eingebunden werden.
' Eine Besonderheit gilt f├╝r den LSM303, er erwartet eine RESTART Sequenz, andere Sensoren kommen ohne klar
' der LSM303 nicht. Die Restart Sequenz ist nichts Anderes als ein schneller I2C Stop gefolgt von einem I2C Start
' wenn man es genau nimmt, sollte die Sequenz bei steigender CLK Flanke gefeuret wreden, er funktioniert aber auch so.
' Der Sensor gibt die Beschleunigungsrohdaten f├╝r die x,y,z Achse aus, daraus  lassen sich Pitch und Roll einfach berechnen
' Pitch ist die Neigung um die Querachse, Roll die Neigung um die L├Ąngsachse. Sind beide Werte 0, also liegt der Sensor gerade im Raum
' l├Ąsst sich daraus recht einfach mit den Magnetsensordaten der x und y Achse recht einfach der Winkel (Kompass) in der Ebvene beerchnen
' das erfolgt mit atan2(y/x) Sid Roll und oder Pitch ungleich null, wird es kompliziert, das ├╝berfordert die kleine C-Control,
' auch wenn accz keine Rolle spielt. Siehe Aplliction Notes zud em Sensor.
' Der LSM303d verf├╝gt ├╝ber veile Register, ├╝ber die man die Senisitivit├Ąt und das Verhalten beeinflussen kann. Der Sensor sit
' ├╝brigens vom Werk aus kalibrirt, sodass man die Prozedur nicht mehr durchf├╝hren muss.
' Bei der Kalibrierung wird der Sensor in eine definierte Lage gebracht und die daraus Offset Werte berechnet, die dauerhaft
' im EEPROM des Sensor gespeichert werden.
' Einige Register m├╝ssen konfiguriert werden, sonst leiferte der Sensor keine Werte, beim Sartup befindet er sich immer im Powerdowm Mode
' Register &h20,&h21,&h24,&h25 m├╝ssen nach Dtaenblatt konfiguriert werden

option ccbas

' Definitionen der I2C-Einsprungadressen
define I2C_START        &h101
define I2C_STOP &h10e
define I2C_READ &h12f
define I2C_WRITE        &h119
define I2C_GETACK       &h156
define I2C_SENDACK      &h173
define I2C_SENDNACK     &h17C

' Definitionen der ├ťbergabe-Variable an die Assembler Routine
define BASIC_IO,BaseAdr byte
' Definitionen der Programvariablen
define accx, accy, accz,magx,magy,magz   word
DEFINE value,rad,i,quadrant,roll,pitch   word
define tmp byte

#main
 init_lsm303d ' Sensor initialisieren, siehe Datenblatt und Anmerkungen im Code
#loop
  get_lsmData
  ' Accelerator Werte ausgegeben
  print "accx:"; accx
  print "accy:"; accy
  print "accz:"; accz
  ' Magnetsensor Werte ausgegeben
  print "magx:"; magx
  print "magy:"; magy
  print "magz:"; magz
  ' Pitch berechnen
  pitch = asin(accx/16*-1)/10
  print "pitch:"; pitch
  ' Roll berechnen
  value =  accy/16
  accz =  accy/16 * 1000 /cos(pitch)
  accz =  accy/16
  magz =  1000 /cos(pitch)
  value = accz * magz
  roll = asin(value)
  print "roll:"; roll
  pause 50
  goto loop
end

procedure init_lsm303d
  BaseAdr = &H3c   ' I2C basisadresse des LSM303D
  sys I2C_START
  BASIC_IO = BaseAdr
  sys I2C_WRITE
  sys I2C_GETACK
  BASIC_IO = &H20   ' Konfigurationregister 1
  sys I2C_WRITE
  sys I2C_GETACK
  BASIC_IO = &H57   ' 50hz, alle Achsen enabled, continious update
  sys I2C_WRITE
  sys I2C_GETACK
  sys I2C_STOP
  sys I2C_START
  BASIC_IO = BaseAdr
  sys I2C_WRITE
  sys I2C_GETACK
  BASIC_IO = &H21  ' Konfigurationregister 2
  sys I2C_WRITE
  sys I2C_GETACK
  BASIC_IO = &HC0   '  50Hz Filter Bandwith, Fullscale +/- 2g, selftest disabled, 3 wire Interface
  sys I2C_WRITE
  sys I2C_GETACK
  sys I2C_STOP
  sys I2C_START
  BASIC_IO = BaseAdr
  sys I2C_WRITE
  sys I2C_GETACK
  BASIC_IO = &H24  ' Konfigurationregister 3
  sys I2C_WRITE
  sys I2C_GETACK
  BASIC_IO = &H10   ' 50hz Magentic datarate, low resosution, temperature sensor disabled
  sys I2C_WRITE
  sys I2C_GETACK
  sys I2C_STOP
  sys I2C_START
  BASIC_IO = BaseAdr
  sys I2C_WRITE
  sys I2C_GETACK
  BASIC_IO = &H25   ' Konfigurationregister 4
  sys I2C_WRITE
  sys I2C_GETACK
  BASIC_IO = &H00    ' magnetic resolution +/- 2 gauss
  sys I2C_WRITE
  sys I2C_GETACK
  sys I2C_STOP
  sys I2C_START
  BASIC_IO = BaseAdr
  sys I2C_WRITE
  sys I2C_GETACK
  BASIC_IO = &H26   ' Konfigurationregister 5
  sys I2C_WRITE
  sys I2C_GETACK
  BASIC_IO = &H00      ' normal Mode, continious conversion
  sys I2C_WRITE
  sys I2C_GETACK
  sys I2C_STOP
  pause 100
return

procedure get_lsmData
  accx = 0
  accy = 0
  accz = 0
  magx = 0
  magy = 0
  magz = 0
  sys I2C_START
  BASIC_IO = &H3c
  sys I2C_WRITE
  sys I2C_GETACK
  BASIC_IO = &H28 or 128 ' 6 Acc. Werte lesen, Register wird  inkrementiert Basisadresse ist &H28
  sys I2C_WRITE
  sys I2C_GETACK
  sys I2C_STOP
  sys I2C_START
  BASIC_IO = &H3d
  sys I2C_WRITE
  sys I2C_GETACK
  BASIC_IO = 0
  sys I2C_READ
  tmp =  BASIC_IO
  sys I2C_SENDACK
  sys I2C_READ
  accx =   (accx or  BASIC_IO)  shl 8   or tmp
  sys I2C_SENDACK
  sys I2C_READ
  tmp =  BASIC_IO
  sys I2C_SENDACK
  sys I2C_READ
  accy =  (accy or  BASIC_IO)  shl 8   or tmp
  sys I2C_SENDACK
  sys I2C_READ
  tmp =  BASIC_IO
  sys I2C_SENDACK
  sys I2C_READ
  accz =  (accz or  BASIC_IO)  shl 8   or tmp
  sys I2C_SENDNACK
  sys I2C_STOP
  sys I2C_START
  BASIC_IO = &H3C
  sys I2C_WRITE
  sys I2C_GETACK
  BASIC_IO = &H08 or 128   ' 6 Mag. Werte lesen, Register wird inkrementiert   Basisadresse ist &H08
  sys I2C_WRITE
  sys I2C_GETACK
  sys I2C_STOP
  sys I2C_START
  BASIC_IO = &H3d
  sys I2C_WRITE
  sys I2C_GETACK
  BASIC_IO = 0
  sys I2C_READ
  tmp =  BASIC_IO
  sys I2C_SENDACK
  sys I2C_READ
  magx =  (magx or  BASIC_IO)  shl 8   or tmp
  sys I2C_SENDACK
  sys I2C_READ
  tmp =  BASIC_IO
  sys I2C_SENDACK
  sys I2C_READ
  magy =  (magy or  BASIC_IO)  shl 8   or tmp
  sys I2C_SENDACK
  sys I2C_READ
  tmp =  BASIC_IO
  sys I2C_SENDACK
  sys I2C_READ
  magz =  (magz or  BASIC_IO)  shl 8   or tmp
  sys I2C_SENDNACK
  sys I2C_STOP
return



FUNCTION asin(value)
  if value < 0 then
     value = value * -1
     quadrant = -1
  else
      quadrant = 1
  end if
  looktab asinus1000, value, value
RETURN value  * quadrant

FUNCTION cos(rad)
if rad >157 and rad <=314 then
   rad = 157 - (rad - 157)
   quadrant = -1
 else if rad >314 and rad <=471 then
      rad = rad - 314
      quadrant = -1
 else if rad >471 and rad <=628 then
      rad = 157 - (rad - 471)
      quadrant = 1
 else
     quadrant = 1
 end if
     rad = 157-rad
 looktab sinus, rad, value
RETURN value  * quadrant

FUNCTION sin(rad)
  if rad >157 and rad <=314 then
    rad = 157 - (rad - 157)
    quadrant = 1
  else if rad >314 and rad <=471 then
      rad = rad - 314
      quadrant = -1
  else if rad >471 and rad <=628 then
      rad = 157 - (rad - 471)
      quadrant = -1
  else
     quadrant = 1
  end if
  looktab sinus, rad, value
RETURN value  * quadrant

end

TABlE sinus
 0 9 19 29 39 49 59 69 79 89 99 109 119 129 139
 149 159 169 179 188 198 208 218 227 237 247 257 266 276
 285 295 305 314 324 333 342 352 361 370 380 389 398 407
 416 425 434 443 452 461 470 479 488 496 505 514 522 531
 539 548 556 564 572 581 589 597 605 613 620 628 636 644
 651 659 666 674 681 688 696 703 710 717 724 731 737 744
 751 757 764 770 777 783 789 795 801 807 813 819 824 830
 836 841 846 852 857 862 867 872 877 881 886 891 895 900
 904 908 912 916 920 924 928 932 935 939 942 945 948 952
 955 958 960 963 966 968 971 973 975 977 979 981 983 985
 987 988 990 991 992 993 994 995 996 997 998 998 999 999
 999 999 1000
end table

TABLE asinus1000
  0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22
  23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42
  43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62
  63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82
  83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101
  102 103 104 105 106 107 108 109 110 111 112 113 114 115 116
  117 118 119 120 121 122 123 124 125 126 127 128 129 130 131
  132 133 134 135 136 137 138 139 140 141 142 143 145 146 147
  148 149 150 151 152 153 154 155 156 157 158 159 160 161 162
  163 164 165 166 167 168 169 170 171 172 173 174 175 176 177
  178 179 180 181 182 183 184 185 186 187 188 189 190 191 192
  193 194 195 196 197 198 199 200 201 202 203 204 205 206 207
  209 210 211 212 213 214 215 216 217 218 219 220 221 222 223
  224 225 226 227 228 229 230 231 232 233 234 235 236 237 238
  239 240 241 242 243 244 245 246 248 249 250 251 252 253 254
  255 256 257 258 259 260 261 262 263 264 265 266 267 268 269
  270 271 272 273 274 275 277 278 279 280 281 282 283 284 285
  286 287 288 289 290 291 292 293 294 295 296 297 298 299 301
  302 303 304 305 306 307 308 309 310 311 312 313 314 315 316
  317 318 319 320 322 323 324 325 326 327 328 329 330 331 332
  333 334 335 336 337 338 339 341 342 343 344 345 346 347 348
  349 350 351 352 353 354 355 357 358 359 360 361 362 363 364
  365 366 367 368 369 370 371 373 374 375 376 377 378 379 380
  381 382 383 384 385 387 388 389 390 391 392 393 394 395 396
  397 398 400 401 402 403 404 405 406 407 408 409 410 412 413
  414 415 416 417 418 419 420 421 422 424 425 426 427 428 429
  430 431 432 433 435 436 437 438 439 440 441 442 443 444 446
  447 448 449 450 451 452 453 454 456 457 458 459 460 461 462
  463 465 466 467 468 469 470 471 472 473 475 476 477 478 479
  480 481 483 484 485 486 487 488 489 490 492 493 494 495 496
  497 498 500 501 502 503 504 505 506 508 509 510 511 512 513
  514 516 517 518 519 520 521 522 524 525 526 527 528 529 531
  532 533 534 535 536 538 539 540 541 542 543 545 546 547 548
  549 550 552 553 554 555 556 557 559 560 561 562 563 565 566
  567 568 569 570 572 573 574 575 576 578 579 580 581 582 584
  585 586 587 588 590 591 592 593 594 596 597 598 599 600 602
  603 604 605 607 608 609 610 611 613 614 615 616 618 619 620
  621 622 624 625 626 627 629 630 631 632 634 635 636 637 639
  640 641 642 644 645 646 647 649 650 651 652 654 655 656 657
  659 660 661 662 664 665 666 667 669 670 671 673 674 675 676
  678 679 680 682 683 684 685 687 688 689 691 692 693 694 696
  697 698 700 701 702 704 705 706 708 709 710 712 713 714 716
  717 718 719 721 722 723 725 726 727 729 730 732 733 734 736
  737 738 740 741 742 744 745 746 748 749 750 752 753 755 756
  757 759 760 761 763 764 766 767 768 770 771 773 774 775 777
  778 780 781 782 784 785 787 788 789 791 792 794 795 797 798
  799 801 802 804 805 807 808 810 811 812 814 815 817 818 820
  821 823 824 826 827 829 830 832 833 835 836 838 839 841 842
  844 845 847 848 850 851 853 854 856 857 859 860 862 863 865
  866 868 869 871 873 874 876 877 879 880 882 884 885 887 888
  890 891 893 895 896 898 899 901 903 904 906 908 909 911 912
  914 916 917 919 921 922 924 926 927 929 931 932 934 936 937
  939 941 942 944 946 948 949 951 953 954 956 958 960 961 963
  965 967 968 970 972 974 976 977 979 981 983 985 986 988 990
  992 994 995 997 999 1001 1003 1005 1007 1008 1010 1012 1014
  1016 1018 1020 1022 1024 1026 1027 1029 1031 1033 1035 1037
  1039 1041 1043 1045 1047 1049 1051 1053 1055 1057 1059 1061
  1063 1065 1068 1070 1072 1074 1076 1078 1080 1082 1084 1086
  1089 1091 1093 1095 1097 1100 1102 1104 1106 1108 1111 1113
  1115 1117 1120 1122 1124 1127 1129 1131 1134 1136 1138 1141
  1143 1146 1148 1151 1153 1156 1158 1160 1163 1166 1168 1171
  1173 1176 1178 1181 1184 1186 1189 1192 1194 1197 1200 1203
  1205 1208 1211 1214 1217 1220 1223 1226 1229 1232 1235 1238
  1241 1244 1247 1250 1253 1256 1260 1263 1266 1270 1273 1276
  1280 1283 1287 1291 1294 1298 1302 1305 1309 1313 1317 1321
  1325 1329 1334 1338 1342 1347 1351 1356 1361 1365 1370 1376
  1381 1386 1392 1397 1403 1409 1416 1422 1429 1437 1444 1452
  1461 1471 1481 1493 1508 1526 1571
end Table

'syscode "i2cbasic.s19"

 Antwort schreiben

Bisherige Antworten:

Re: 9-Achsen Sensor LSM303 mit I2C ansteuern (von Ralf Rosche - 7.01.2022 9:17)
    Re: 9-Achsen Sensor LSM303 mit I2C ansteuern (von das |_ Team - 7.01.2022 15:19)