  INFO - FAQ - CC2-Forum - CCPro-Forum |
|
||
|
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 |
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)