Rev 583 | Rev 598 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 583 | Rev 590 | ||
|---|---|---|---|
| Line 117... | Line 117... | ||
| 117 | #define REG_MAG_IDA 0x0A |
117 | #define REG_MAG_IDA 0x0A |
| 118 | #define REG_MAG_IDB 0x0B |
118 | #define REG_MAG_IDB 0x0B |
| 119 | #define REG_MAG_IDC 0x0C |
119 | #define REG_MAG_IDC 0x0C |
| 120 | #define REG_MAG_IDF 0x0F // WHO_AM_I _M = 0x03c when LSM303DLM is connected |
120 | #define REG_MAG_IDF 0x0F // WHO_AM_I _M = 0x03c when LSM303DLM is connected |
| Line -... | Line 121... | ||
| - | 121 | ||
| - | 122 | #define ADR_LSM303D_MAG 0x08 |
|
| - | 123 | #define REG_303D_CTRL0 0x1F // Fifo + Interrupts |
|
| - | 124 | #define REG_303D_CTRL1 0x20 // ACC Update Speed |
|
| - | 125 | #define REG_303D_CTRL2 0x21 // ACC Gain + Filter |
|
| - | 126 | #define REG_303D_CTRL3 0x22 // Fifo + Interrupts |
|
| - | 127 | #define REG_303D_CTRL4 0x23 // Fifo + Interrupts |
|
| - | 128 | #define REG_303D_CTRL5 0x24 // Temperature Enable + Magn.Resolution + Magn.Rate |
|
| - | 129 | #define REG_303D_CTRL6 0x25 // Magn. Gain |
|
| 121 | 130 | #define REG_303D_CTRL7 0x26 // Magn. PowerDown + Filter |
|
| 122 | // bit mask for configuration mode |
131 | // bit mask for configuration mode |
| 123 | #define CRA_MODE_MASK 0x03 |
132 | #define CRA_MODE_MASK 0x03 |
| 124 | #define CRA_MODE_NORMAL 0x00 //default |
133 | #define CRA_MODE_NORMAL 0x00 //default |
| 125 | #define CRA_MODE_POSBIAS 0x01 |
134 | #define CRA_MODE_POSBIAS 0x01 |
| Line 142... | Line 151... | ||
| 142 | // ids |
151 | // ids |
| 143 | #define MAG_IDA 0x48 |
152 | #define MAG_IDA 0x48 |
| 144 | #define MAG_IDB 0x34 |
153 | #define MAG_IDB 0x34 |
| 145 | #define MAG_IDC 0x33 |
154 | #define MAG_IDC 0x33 |
| 146 | #define MAG_IDF_LSM303DLM 0x3C |
155 | #define MAG_IDF_LSM303DLM 0x3C |
| - | 156 | #define MAG_IDF_LSM303D 0x49 |
|
| - | 157 | #define MAG_IDF_LSM303DLH 0x00 |
|
| Line 147... | Line 158... | ||
| 147 | 158 | ||
| 148 | // the special HMC5843 interface |
159 | // the special HMC5843 interface |
| 149 | // bit mask for rate |
160 | // bit mask for rate |
| 150 | #define HMC5843_CRA_RATE_0_5HZ 0x00 |
161 | #define HMC5843_CRA_RATE_0_5HZ 0x00 |
| 151 | #define HMC5843_CRA_RATE_1HZ 0x04 |
162 | #define HMC5843_CRA_RATE_1HZ 0x04 |
| 152 | #define HMC5843_CRA_RATE_2HZ 0x08 |
163 | #define HMC5843_CRA_RATE_2HZ 0x08 |
| 153 | #define HMC5843_CRA_RATE_5HZ 0x0C |
164 | #define HMC5843_CRA_RATE_5HZ 0x0C |
| 154 | #define HMC5843_CRA_RATE_10HZ 0x10 //default |
165 | #define HMC5843_CRA_RATE_10HZ 0x10 //default |
| 155 | #define HMC5843_CRA_RATE_20HZ 0x14 |
166 | #define HMC5843_CRA_RATE_20HZ 0x14 |
| 156 | #define HMC5843_CRA_RATE_50HZ 0x18 |
167 | #define HMC5843_CRA_RATE_50HZ 0x18 // <-- we use this |
| 157 | // bit mask for gain |
168 | // bit mask for gain |
| 158 | #define HMC5843_CRB_GAIN_07GA 0x00 |
169 | #define HMC5843_CRB_GAIN_07GA 0x00 |
| 159 | #define HMC5843_CRB_GAIN_10GA 0x20 //default |
170 | #define HMC5843_CRB_GAIN_10GA 0x20 //default |
| 160 | #define HMC5843_CRB_GAIN_15GA 0x40 // <--- we use this |
171 | #define HMC5843_CRB_GAIN_15GA 0x40 // <--- we use this |
| Line 176... | Line 187... | ||
| 176 | #define LSM303DLH_CRA_RATE_1_5HZ 0x04 |
187 | #define LSM303DLH_CRA_RATE_1_5HZ 0x04 |
| 177 | #define LSM303DLH_CRA_RATE_3_0HZ 0x08 |
188 | #define LSM303DLH_CRA_RATE_3_0HZ 0x08 |
| 178 | #define LSM303DLH_CRA_RATE_7_5HZ 0x0C |
189 | #define LSM303DLH_CRA_RATE_7_5HZ 0x0C |
| 179 | #define LSM303DLH_CRA_RATE_15HZ 0x10 //default |
190 | #define LSM303DLH_CRA_RATE_15HZ 0x10 //default |
| 180 | #define LSM303DLH_CRA_RATE_30HZ 0x14 |
191 | #define LSM303DLH_CRA_RATE_30HZ 0x14 |
| - | 192 | #define LSM303DLH_CRA_RATE_75HZ 0x18 // <-- we use this |
|
| - | 193 | ||
| - | 194 | // LSM303D CTRL1 |
|
| - | 195 | #define LSM303D_ACC_RATE_0HZ 0x0F |
|
| - | 196 | #define LSM303D_ACC_RATE_3HZ 0x1F |
|
| - | 197 | #define LSM303D_ACC_RATE_6HZ 0x2F |
|
| - | 198 | #define LSM303D_ACC_RATE_12HZ 0x3F |
|
| 181 | #define LSM303DLH_CRA_RATE_75HZ 0x18 |
199 | #define LSM303D_ACC_RATE_25HZ 0x4F |
| - | 200 | #define LSM303D_ACC_RATE_50HZ 0x5F |
|
| - | 201 | #define LSM303D_ACC_RATE_100HZ 0x6F |
|
| - | 202 | #define LSM303D_ACC_RATE_200HZ 0x7F |
|
| - | 203 | #define LSM303D_ACC_RATE_400HZ 0x8F |
|
| - | 204 | #define LSM303D_ACC_RATE_800HZ 0x9F |
|
| - | 205 | #define LSM303D_ACC_RATE_1600HZ 0xAF |
|
| - | 206 | ||
| - | 207 | // LSM303D CTRL1 |
|
| - | 208 | #define LSM303D_ACC_SCALE_2g 0x00 |
|
| - | 209 | #define LSM303D_ACC_SCALE_4g 0x08 |
|
| - | 210 | #define LSM303D_ACC_SCALE_6g 0x10 |
|
| - | 211 | #define LSM303D_ACC_SCALE_8g 0x18 |
|
| - | 212 | #define LSM303D_ACC_SCALE_16g 0x20 |
|
| - | 213 | ||
| - | 214 | // LSM303D CTRL5 |
|
| - | 215 | #define LSM303D_ODR_RATE_3HZ 0x00 |
|
| - | 216 | #define LSM303D_ODR_RATE_6HZ 0x04 |
|
| - | 217 | #define LSM303D_ODR_RATE_12HZ 0x08 |
|
| - | 218 | #define LSM303D_ODR_RATE_25HZ 0x0C |
|
| - | 219 | #define LSM303D_ODR_RATE_50HZ 0x10 |
|
| - | 220 | #define LSM303D_ODR_RATE_100HZ 0x14 // attention: ACC Rate must be >50Hz to use this |
|
| - | 221 | #define LSM303D_HIGH_RESULUTION 0x60 |
|
| - | 222 | #define LSM303D_LOW_RESULUTION 0x00 |
|
| - | 223 | #define LSM303D_TEMP_ENABLE 0x80 |
|
| - | 224 | ||
| - | 225 | // LSM303D CTRL6 |
|
| - | 226 | #define LSM303D_MAG_SCALE_2GA 0x00 |
|
| - | 227 | #define LSM303D_MAG_SCALE_4GA 0x20 |
|
| - | 228 | #define LSM303D_MAG_SCALE_8GA 0x40 // <-- we use this (Achtung: der LSM303D hat eine höhere Auflösung) |
|
| - | 229 | #define LSM303D_MAG_SCALE_12GA 0x60 |
|
| Line 182... | Line 230... | ||
| 182 | 230 | ||
| 183 | // bit mask for gain |
231 | // bit mask for gain |
| 184 | #define LSM303DLH_CRB_GAIN_XXGA 0x00 |
232 | #define LSM303DLH_CRB_GAIN_XXGA 0x00 |
| 185 | #define LSM303DLH_CRB_GAIN_13GA 0x20 //default |
233 | #define LSM303DLH_CRB_GAIN_13GA 0x20 //default |
| Line 222... | Line 270... | ||
| 222 | 270 | ||
| 223 | // the i2c ACC interface |
271 | // the i2c ACC interface |
| Line 224... | Line 272... | ||
| 224 | #define ACC_SLAVE_ADDRESS 0x30 // i2c slave for acc. sensor registers |
272 | #define ACC_SLAVE_ADDRESS 0x30 // i2c slave for acc. sensor registers |
| 225 | 273 | ||
| Line 226... | Line 274... | ||
| 226 | // multiple byte read/write mask |
274 | // multiple byte read/write mask |
| 227 | #define REG_ACC_MASK_AUTOINCREMENT 0x80 |
275 | #define REG_MASK_AUTOINCREMENT 0x80 |
| 228 | 276 | ||
| 229 | // register mapping |
277 | // register mapping |
| Line 425... | Line 473... | ||
| 425 | MinCalibration = HMC5843_CALIBRATION_RANGE; |
473 | MinCalibration = HMC5843_CALIBRATION_RANGE; |
| 426 | break; |
474 | break; |
| Line 427... | Line 475... | ||
| 427 | 475 | ||
| 428 | case TYPE_LSM303DLH: |
476 | case TYPE_LSM303DLH: |
| - | 477 | case TYPE_LSM303DLM: |
|
| - | 478 | case TYPE_LSM303D: |
|
| 429 | case TYPE_LSM303DLM: |
479 | default: |
| 430 | UART1_PutString("\r\nFinished: LSM303 calibration\n\r"); |
480 | UART1_PutString("\r\nFinished: LSM303 calibration\n\r"); |
| 431 | MinCalibration = LSM303_CALIBRATION_RANGE; |
481 | MinCalibration = LSM303_CALIBRATION_RANGE; |
| 432 | break; |
482 | break; |
| 433 | } |
483 | } |
| Line 509... | Line 559... | ||
| 509 | } |
559 | } |
| 510 | } |
560 | } |
| Line 511... | Line 561... | ||
| 511 | 561 | ||
| 512 | // rx data handler for magnetic sensor raw data |
562 | // rx data handler for magnetic sensor raw data |
| - | 563 | void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize) |
|
| - | 564 | { |
|
| 513 | void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize) |
565 | u8 endian = 1; |
| 514 | { // if number of bytes are matching |
566 | if(NCMAG_SensorType == TYPE_LSM303D) endian = 0; // here the low and high-bytes are mixed |
| 515 | if(RxBufferSize == sizeof(MagRawVector) ) |
567 | if(RxBufferSize == sizeof(MagRawVector) ) // if number of bytes are matching |
| 516 | { // byte order from big to little endian |
568 | { // byte order from big to little endian |
| 517 | s16 raw, X = 0, Y = 0, Z = 0; |
569 | s16 raw, X = 0, Y = 0, Z = 0; |
| 518 | raw = pRxBuffer[0]<<8; |
- | |
| 519 | raw+= pRxBuffer[1]; |
570 | if(endian) {raw = pRxBuffer[0]<<8; raw+= pRxBuffer[1];} else {raw = (pRxBuffer[1] << 8) + pRxBuffer[0]; raw >>= 3;} |
| 520 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) X = raw; |
571 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) X = raw; |
| 521 | else if(CompassValueErrorCount < 35) CompassValueErrorCount++; // invalid data |
- | |
| 522 | 572 | else if(CompassValueErrorCount < 35) CompassValueErrorCount++; // invalid data |
|
| 523 | raw = pRxBuffer[2]<<8; |
- | |
| 524 | raw+= pRxBuffer[3]; |
573 | if(endian) {raw = pRxBuffer[2]<<8; raw+= pRxBuffer[3];} else {raw = (pRxBuffer[3] << 8) + pRxBuffer[2]; raw >>= 3;} |
| 525 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) |
574 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) |
| 526 | { |
575 | { |
| 527 | if(NCMAG_SensorType == TYPE_LSM303DLM) Z = raw; // here Z and Y are exchanged |
576 | if(NCMAG_SensorType == TYPE_LSM303DLM) Z = raw; // here Z and Y are exchanged |
| 528 | else Y = raw; |
577 | else Y = raw; |
| 529 | } |
578 | } |
| 530 | else if(CompassValueErrorCount < 35) CompassValueErrorCount++; // invalid data |
- | |
| 531 | 579 | else if(CompassValueErrorCount < 35) CompassValueErrorCount++; // invalid data |
|
| 532 | raw = pRxBuffer[4]<<8; |
- | |
| 533 | raw+= pRxBuffer[5]; |
580 | if(endian) {raw = pRxBuffer[4]<<8; raw+= pRxBuffer[5];} else {raw = (pRxBuffer[5] << 8) + pRxBuffer[4]; raw >>= 3;} |
| 534 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) |
581 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) |
| 535 | { |
582 | { |
| 536 | if(NCMAG_SensorType == TYPE_LSM303DLM) Y = raw; // here Z and Y are exchanged |
583 | if(NCMAG_SensorType == TYPE_LSM303DLM) Y = raw; // here Z and Y are exchanged |
| 537 | else Z = raw; |
584 | else Z = raw; |
| 538 | } |
585 | } |
| 539 | else if(CompassValueErrorCount < 35) CompassValueErrorCount++; // invalid data |
- | |
| 540 | 586 | else if(CompassValueErrorCount < 35) CompassValueErrorCount++; // invalid data |
|
| 541 | // correct compass orientation |
587 | // correct compass orientation |
| 542 | switch(NCMAG_Orientation) |
588 | switch(NCMAG_Orientation) |
| 543 | { |
589 | { |
| 544 | case 0: |
590 | case 0: |
| Line 652... | Line 698... | ||
| 652 | u8 retval = 0; |
698 | u8 retval = 0; |
| Line 653... | Line 699... | ||
| 653 | 699 | ||
| 654 | // try to catch the i2c buffer within 100 ms timeout |
700 | // try to catch the i2c buffer within 100 ms timeout |
| 655 | if(I2CBus_LockBuffer(Compass_I2CPort, 100)) |
701 | if(I2CBus_LockBuffer(Compass_I2CPort, 100)) |
| - | 702 | { |
|
| - | 703 | if(NCMAG_SensorType == TYPE_LSM303D) |
|
| - | 704 | { |
|
| - | 705 | u8 TxBytes = 0; |
|
| - | 706 | u8 CfgData[] = { REG_303D_CTRL0 | REG_MASK_AUTOINCREMENT, |
|
| - | 707 | 0, // Ctrl 0 -> Fifo |
|
| - | 708 | LSM303D_ACC_RATE_6HZ, // Ctrl 1 -> ACC Update Speed |
|
| - | 709 | LSM303D_ACC_SCALE_8g, // Ctrl 2 -> ACC Gain + Filter (0 = 773Hz) |
|
| - | 710 | 0, // Ctrl 3 -> Interrupts |
|
| - | 711 | 0, // Ctrl 4 -> Interrupts |
|
| - | 712 | LSM303D_ODR_RATE_50HZ | LSM303D_TEMP_ENABLE | LSM303D_LOW_RESULUTION,// Ctrl 5 -> Temperature Enable + Magn.Resolution + Magn.Rate |
|
| - | 713 | LSM303D_MAG_SCALE_8GA, // Ctrl 6 -> Magn. Scale |
|
| - | 714 | 0x80 // Ctrl 7 -> Magn. PowerDown + Filter |
|
| - | 715 | }; |
|
| - | 716 | ||
| - | 717 | TxBytes = sizeof(CfgData); |
|
| - | 718 | DebugOut.Analog[16]++; |
|
| - | 719 | if(I2CBus_Transmission(Compass_I2CPort, MAG_SLAVE_ADDRESS, CfgData, TxBytes, 0, 0)) |
|
| - | 720 | { |
|
| - | 721 | if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 100)) |
|
| - | 722 | { |
|
| - | 723 | if(I2CBus(Compass_I2CPort)->Error == I2C_ERROR_NONE) retval = 1; |
|
| - | 724 | } |
|
| - | 725 | } |
|
| - | 726 | } |
|
| - | 727 | else |
|
| 656 | { |
728 | { |
| 657 | u8 TxBytes = 0; |
729 | u8 TxBytes = 0; |
| Line 658... | Line 730... | ||
| 658 | u8 TxData[sizeof(MagConfig) + 3]; |
730 | u8 TxData[sizeof(MagConfig) + 3]; |
| 659 | 731 | ||
| Line 665... | Line 737... | ||
| 665 | if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 100)) |
737 | if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 100)) |
| 666 | { |
738 | { |
| 667 | if(I2CBus(Compass_I2CPort)->Error == I2C_ERROR_NONE) retval = 1; |
739 | if(I2CBus(Compass_I2CPort)->Error == I2C_ERROR_NONE) retval = 1; |
| 668 | } |
740 | } |
| 669 | } |
741 | } |
| - | 742 | } |
|
| 670 | } |
743 | } |
| 671 | return(retval); |
744 | return(retval); |
| 672 | } |
745 | } |
| Line 673... | Line 746... | ||
| 673 | 746 | ||
| - | 747 | // ---------------------------------------------------------------------------------------- |
|
| 674 | // ---------------------------------------------------------------------------------------- |
748 | /* |
| 675 | u8 NCMAG_GetMagConfig(void) |
749 | u8 NCMAG_GetMagConfig(void) |
| 676 | { |
750 | { |
| 677 | u8 retval = 0; |
751 | u8 retval = 0; |
| 678 | // try to catch the i2c buffer within 100 ms timeout |
752 | // try to catch the i2c buffer within 100 ms timeout |
| Line 689... | Line 763... | ||
| 689 | } |
763 | } |
| 690 | } |
764 | } |
| 691 | } |
765 | } |
| 692 | return(retval); |
766 | return(retval); |
| 693 | } |
767 | } |
| 694 | 768 | */ |
|
| 695 | // ---------------------------------------------------------------------------------------- |
769 | // ---------------------------------------------------------------------------------------- |
| 696 | u8 NCMAG_SetAccConfig(void) |
770 | u8 NCMAG_SetAccConfig(void) |
| 697 | { |
771 | { |
| 698 | u8 retval = 0; |
772 | u8 retval = 0; |
| 699 | // try to catch the i2c buffer within 50 ms timeout |
773 | // try to catch the i2c buffer within 50 ms timeout |
| 700 | if(I2CBus_LockBuffer(Compass_I2CPort, 50)) |
774 | if(I2CBus_LockBuffer(Compass_I2CPort, 50)) |
| 701 | { |
775 | { |
| 702 | u8 TxBytes = 0; |
776 | u8 TxBytes = 0; |
| 703 | u8 TxData[sizeof(AccConfig) + 3]; |
777 | u8 TxData[sizeof(AccConfig) + 3]; |
| 704 | TxData[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT; |
778 | TxData[TxBytes++] = REG_ACC_CTRL1|REG_MASK_AUTOINCREMENT; |
| 705 | memcpy(&TxData[TxBytes], (u8*)&AccConfig, sizeof(AccConfig)); |
779 | memcpy(&TxData[TxBytes], (u8*)&AccConfig, sizeof(AccConfig)); |
| 706 | TxBytes += sizeof(AccConfig); |
780 | TxBytes += sizeof(AccConfig); |
| 707 | if(I2CBus_Transmission(Compass_I2CPort, ACC_SLAVE_ADDRESS, TxData, TxBytes, 0, 0)) |
781 | if(I2CBus_Transmission(Compass_I2CPort, ACC_SLAVE_ADDRESS, TxData, TxBytes, 0, 0)) |
| 708 | { |
782 | { |
| 709 | if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 50)) |
783 | if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 50)) |
| Line 722... | Line 796... | ||
| 722 | // try to catch the i2c buffer within 100 ms timeout |
796 | // try to catch the i2c buffer within 100 ms timeout |
| 723 | if(I2CBus_LockBuffer(Compass_I2CPort, 100)) |
797 | if(I2CBus_LockBuffer(Compass_I2CPort, 100)) |
| 724 | { |
798 | { |
| 725 | u8 TxBytes = 0; |
799 | u8 TxBytes = 0; |
| 726 | u8 TxData[3]; |
800 | u8 TxData[3]; |
| 727 | TxData[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT; |
801 | TxData[TxBytes++] = REG_ACC_CTRL1|REG_MASK_AUTOINCREMENT; |
| 728 | if(I2CBus_Transmission(Compass_I2CPort, ACC_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateAccConfig, sizeof(AccConfig))) |
802 | if(I2CBus_Transmission(Compass_I2CPort, ACC_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateAccConfig, sizeof(AccConfig))) |
| 729 | { |
803 | { |
| 730 | if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 100)) |
804 | if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 100)) |
| 731 | { |
805 | { |
| 732 | if(I2CBus(Compass_I2CPort)->Error == I2C_ERROR_NONE) retval = 1; |
806 | if(I2CBus(Compass_I2CPort)->Error == I2C_ERROR_NONE) retval = 1; |
| Line 738... | Line 812... | ||
| 738 | 812 | ||
| 739 | // ---------------------------------------------------------------------------------------- |
813 | // ---------------------------------------------------------------------------------------- |
| 740 | u8 NCMAG_GetIdentification(void) |
814 | u8 NCMAG_GetIdentification(void) |
| 741 | { |
815 | { |
| - | 816 | u8 retval = 0; |
|
| 742 | u8 retval = 0; |
817 | if(NCMAG_SensorType == TYPE_LSM303D) return(1); |
| 743 | // try to catch the i2c buffer within 100 ms timeout |
818 | // try to catch the i2c buffer within 100 ms timeout |
| 744 | if(I2CBus_LockBuffer(Compass_I2CPort, 100)) |
819 | if(I2CBus_LockBuffer(Compass_I2CPort, 100)) |
| 745 | { |
820 | { |
| 746 | u8 TxBytes = 0; |
821 | u8 TxBytes = 0; |
| Line 791... | Line 866... | ||
| 791 | if(I2CBus_LockBuffer(Compass_I2CPort, timeout)) |
866 | if(I2CBus_LockBuffer(Compass_I2CPort, timeout)) |
| 792 | { |
867 | { |
| 793 | u8 TxBytes = 0; |
868 | u8 TxBytes = 0; |
| 794 | u8 TxData[3]; |
869 | u8 TxData[3]; |
| 795 | // set register pointer |
870 | // set register pointer |
| - | 871 | if(NCMAG_SensorType == TYPE_LSM303D) TxData[TxBytes++] = ADR_LSM303D_MAG | REG_MASK_AUTOINCREMENT; |
|
| 796 | TxData[TxBytes++] = REG_MAG_DATAX_MSB; |
872 | else TxData[TxBytes++] = REG_MAG_DATAX_MSB; |
| 797 | // initiate transmission |
873 | // initiate transmission |
| 798 | I2CBus_Transmission(Compass_I2CPort, MAG_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateMagVector, sizeof(MagVector)); |
874 | I2CBus_Transmission(Compass_I2CPort, MAG_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateMagVector, sizeof(MagVector)); |
| 799 | } |
875 | } |
| 800 | } |
876 | } |
| Line 806... | Line 882... | ||
| 806 | if(I2CBus_LockBuffer(Compass_I2CPort, timeout)) |
882 | if(I2CBus_LockBuffer(Compass_I2CPort, timeout)) |
| 807 | { |
883 | { |
| 808 | u8 TxBytes = 0; |
884 | u8 TxBytes = 0; |
| 809 | u8 TxData[3]; |
885 | u8 TxData[3]; |
| 810 | // set register pointer |
886 | // set register pointer |
| - | 887 | if(NCMAG_SensorType == TYPE_LSM303D) |
|
| - | 888 | { |
|
| 811 | TxData[TxBytes++] = REG_ACC_X_LSB|REG_ACC_MASK_AUTOINCREMENT; |
889 | TxData[TxBytes++] = REG_ACC_X_LSB | REG_MASK_AUTOINCREMENT; |
| - | 890 | I2CBus_Transmission(Compass_I2CPort, MAG_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector)); |
|
| - | 891 | } |
|
| - | 892 | else |
|
| - | 893 | { |
|
| - | 894 | TxData[TxBytes++] = REG_ACC_X_LSB | REG_MASK_AUTOINCREMENT; |
|
| 812 | // initiate transmission |
895 | // initiate transmission |
| 813 | I2CBus_Transmission(Compass_I2CPort, ACC_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector)); |
896 | I2CBus_Transmission(Compass_I2CPort, ACC_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector)); |
| - | 897 | } |
|
| 814 | } |
898 | } |
| 815 | } |
899 | } |
| Line 816... | Line 900... | ||
| 816 | 900 | ||
| 817 | //---------------------------------------------------------------- |
901 | //---------------------------------------------------------------- |
| 818 | u8 NCMAG_ConfigureSensor(void) |
902 | u8 NCMAG_ConfigureSensor(void) |
| 819 | { |
- | |
| 820 | u8 crb_gain, cra_rate; |
903 | { |
| 821 | switch(NCMAG_SensorType) |
904 | switch(NCMAG_SensorType) |
| 822 | { |
905 | { |
| - | 906 | case TYPE_HMC5843: |
|
| 823 | case TYPE_HMC5843: |
907 | MagConfig.cra = HMC5843_CRA_RATE_50HZ|CRA_MODE_NORMAL; |
| 824 | crb_gain = HMC5843_CRB_GAIN_15GA; |
908 | MagConfig.crb = HMC5843_CRB_GAIN_15GA; |
| 825 | cra_rate = HMC5843_CRA_RATE_50HZ; |
909 | MagConfig.mode = MODE_CONTINUOUS; |
| Line 826... | Line 910... | ||
| 826 | break; |
910 | break; |
| 827 | 911 | ||
| 828 | case TYPE_LSM303DLH: |
912 | case TYPE_LSM303DLH: |
| 829 | case TYPE_LSM303DLM: |
913 | case TYPE_LSM303DLM: |
| 830 | // crb_gain = LSM303DLH_CRB_GAIN_19GA; |
914 | // MagConfig.crb = LSM303DLH_CRB_GAIN_19GA; |
| - | 915 | MagConfig.crb = LSM303DLH_CRB_GAIN_40GA; // seit 2.03 -> 2.2014 |
|
| - | 916 | MagConfig.cra = LSM303DLH_CRA_RATE_75HZ|CRA_MODE_NORMAL;; |
|
| - | 917 | MagConfig.mode = MODE_CONTINUOUS; |
|
| - | 918 | break; |
|
| 831 | crb_gain = LSM303DLH_CRB_GAIN_40GA; // seit 2.03 -> 2.2014 |
919 | case TYPE_LSM303D: |
| 832 | cra_rate = LSM303DLH_CRA_RATE_75HZ; |
- | |
| 833 | break; |
920 | // -> see defined data in NCMAG_SetMagConfig(); |
| 834 | 921 | break; |
|
| 835 | default: |
922 | default: |
| 836 | return(0); |
- | |
| 837 | } |
- | |
| 838 | - | ||
| 839 | MagConfig.cra = cra_rate|CRA_MODE_NORMAL; |
- | |
| 840 | MagConfig.crb = crb_gain; |
923 | return(0); |
| 841 | MagConfig.mode = MODE_CONTINUOUS; |
924 | } |
| Line 842... | Line 925... | ||
| 842 | return(NCMAG_SetMagConfig()); |
925 | return(NCMAG_SetMagConfig()); |
| 843 | } |
926 | } |
| 844 | 927 | ||
| - | 928 | ||
| - | 929 | //---------------------------------------------------------------- |
|
| - | 930 | u8 NCMAG_Init_ACCSensor(void) |
|
| - | 931 | { |
|
| - | 932 | if(NCMAG_SensorType == TYPE_LSM303D) |
|
| - | 933 | { |
|
| 845 | 934 | return(NCMAG_SetMagConfig()); |
|
| 846 | //---------------------------------------------------------------- |
935 | } |
| 847 | u8 NCMAG_Init_ACCSensor(void) |
936 | else |
| 848 | { |
937 | { |
| 849 | AccConfig.ctrl_1 = ACC_CRTL1_PM_NORMAL|ACC_CRTL1_DR_50HZ|ACC_CRTL1_XEN|ACC_CRTL1_YEN|ACC_CRTL1_ZEN; |
938 | AccConfig.ctrl_1 = ACC_CRTL1_PM_NORMAL|ACC_CRTL1_DR_50HZ|ACC_CRTL1_XEN|ACC_CRTL1_YEN|ACC_CRTL1_ZEN; |
| 850 | AccConfig.ctrl_2 = 0; |
939 | AccConfig.ctrl_2 = 0; |
| - | 940 | AccConfig.ctrl_3 = 0x00; |
|
| 851 | AccConfig.ctrl_3 = 0x00; |
941 | AccConfig.ctrl_4 = ACC_CTRL4_BDU | ACC_CTRL4_FS_8G; |
| 852 | AccConfig.ctrl_4 = ACC_CTRL4_BDU | ACC_CTRL4_FS_8G; |
942 | AccConfig.ctrl_5 = ACC_CTRL5_STW_OFF; |
| 853 | AccConfig.ctrl_5 = ACC_CTRL5_STW_OFF; |
943 | return(NCMAG_SetAccConfig()); |
| 854 | return(NCMAG_SetAccConfig()); |
944 | } |
| 855 | } |
945 | } |
| Line 892... | Line 982... | ||
| 892 | delay = 20; // next cycle after 20 ms |
982 | delay = 20; // next cycle after 20 ms |
| 893 | NCMAG_GetMagVector(5); |
983 | NCMAG_GetMagVector(5); |
| 894 | break; |
984 | break; |
| 895 | case TYPE_LSM303DLH: |
985 | case TYPE_LSM303DLH: |
| 896 | case TYPE_LSM303DLM: |
986 | case TYPE_LSM303DLM: |
| - | 987 | case TYPE_LSM303D: |
|
| 897 | delay = 20; // next cycle after 20 ms |
988 | delay = 20; // next cycle after 20 ms |
| 898 | if(s-- || (Compass_I2CPort == NCMAG_PORT_INTERN)) |
989 | if(s-- || (Compass_I2CPort == NCMAG_PORT_INTERN)) |
| 899 | { |
990 | { |
| 900 | NCMAG_GetMagVector(5); |
991 | NCMAG_GetMagVector(5); |
| 901 | } |
992 | } |
| Line 904... | Line 995... | ||
| 904 | if((AccRawVector.X + AccRawVector.Y + AccRawVector.Z) == 0) NCMAG_Init_ACCSensor(); |
995 | if((AccRawVector.X + AccRawVector.Y + AccRawVector.Z) == 0) NCMAG_Init_ACCSensor(); |
| 905 | // get new data |
996 | // get new data |
| 906 | NCMAG_GetAccVector(5); |
997 | NCMAG_GetAccVector(5); |
| 907 | delay = 10; // next cycle after 10 ms |
998 | delay = 10; // next cycle after 10 ms |
| 908 | s = 40; //reset downconter about 0,8 sec |
999 | s = 40; //reset downconter about 0,8 sec |
| - | 1000 | /* |
|
| - | 1001 | DebugOut.Analog[17] = AccVector.X; |
|
| - | 1002 | DebugOut.Analog[18] = AccVector.Y; |
|
| - | 1003 | DebugOut.Analog[19] = AccVector.Z; |
|
| - | 1004 | s = 2; |
|
| - | 1005 | */ |
|
| 909 | } |
1006 | } |
| 910 | break; |
1007 | break; |
| 911 | } |
1008 | } |
| 912 | if(send_config == 24) TimerUpdate = SetDelay(15); // next event is the re-configuration |
1009 | if(send_config == 24) TimerUpdate = SetDelay(15); // next event is the re-configuration |
| 913 | else TimerUpdate = SetDelay(delay); // every 20 ms are 50 Hz |
1010 | else TimerUpdate = SetDelay(delay); // every 20 ms are 50 Hz |
| Line 953... | Line 1050... | ||
| 953 | case TYPE_LSM303DLM: |
1050 | case TYPE_LSM303DLM: |
| 954 | // does not support self test feature |
1051 | // does not support self test feature |
| 955 | done = 1; |
1052 | done = 1; |
| 956 | return(1); // always return success |
1053 | return(1); // always return success |
| 957 | break; |
1054 | break; |
| - | 1055 | case TYPE_LSM303D: |
|
| - | 1056 | done = 1; |
|
| - | 1057 | return(1); // always return success |
|
| 958 | 1058 | break; |
|
| 959 | default: |
1059 | default: |
| 960 | return(0); |
1060 | return(0); |
| 961 | } |
1061 | } |
| Line 962... | Line 1062... | ||
| 962 | 1062 | ||
| Line 1066... | Line 1166... | ||
| 1066 | 1166 | ||
| 1067 | //-------------------------------------------- |
1167 | //-------------------------------------------- |
| 1068 | // search external sensor first |
1168 | // search external sensor first |
| 1069 | //-------------------------------------------- |
1169 | //-------------------------------------------- |
| 1070 | Compass_I2CPort = NCMAG_PORT_EXTERN; |
- | |
| 1071 | // get id bytes |
1170 | Compass_I2CPort = NCMAG_PORT_EXTERN; |
| - | 1171 | retval = 0; |
|
| 1072 | retval = 0; |
1172 | // get id bytes |
| 1073 | for(repeat = 0; repeat < 5; repeat++) |
1173 | for(repeat = 0; repeat < 5; repeat++) |
| 1074 | { |
1174 | { |
| - | 1175 | if(NCMAG_GetIdentification_Sub()) break; |
|
| - | 1176 | UART1_PutString("="); |
|
| - | 1177 | } |
|
| - | 1178 | //sprintf(msg, "(WhoAmI=%02x)", NCMAG_Identification2.Sub); |
|
| - | 1179 | //UART1_PutString(msg); |
|
| - | 1180 | if(NCMAG_Identification2.Sub == MAG_IDF_LSM303D) |
|
| - | 1181 | { |
|
| - | 1182 | NCMAG_SensorType = TYPE_LSM303D; |
|
| - | 1183 | NCMAG_ConfigureSensor(); |
|
| - | 1184 | retval = 1; |
|
| - | 1185 | } |
|
| - | 1186 | else |
|
| - | 1187 | { // External Sensor not LSM303D |
|
| - | 1188 | for(repeat = 0; repeat < 3; repeat++) // does it have ACC? |
|
| 1075 | //retval = NCMAG_GetIdentification(); |
1189 | { |
| 1076 | retval = NCMAG_GetAccConfig(); // only the external sensor with ACC is supported |
1190 | retval = NCMAG_GetAccConfig(); // only the external sensor with ACC is supported |
| 1077 | if(retval) break; // break loop on success |
1191 | if(retval) break; // break loop on success |
| 1078 | UART1_PutString("_"); |
1192 | UART1_PutString("_"); |
| 1079 | } |
1193 | } |
| Line 1088... | Line 1202... | ||
| 1088 | { |
1202 | { |
| 1089 | UART1_PutString(" external sensor"); |
1203 | UART1_PutString(" external sensor"); |
| 1090 | Compass_I2CPort = NCMAG_PORT_EXTERN; |
1204 | Compass_I2CPort = NCMAG_PORT_EXTERN; |
| 1091 | } |
1205 | } |
| 1092 | //------------------------------------------- |
1206 | //------------------------------------------- |
| 1093 | - | ||
| 1094 | NCMAG_Present = 0; |
1207 | NCMAG_Present = 0; |
| 1095 | NCMAG_SensorType = TYPE_HMC5843; // assuming having an HMC5843 |
1208 | NCMAG_SensorType = TYPE_HMC5843; // assuming having an HMC5843 |
| 1096 | // polling for LSM302DLH/DLM option by ACC address ack |
- | |
| 1097 | repeat = 0; |
1209 | |
| 1098 | for(repeat = 0; repeat < 3; repeat++) |
1210 | for(repeat = 0; repeat < 3; repeat++) |
| 1099 | { |
1211 | { |
| - | 1212 | if(NCMAG_GetIdentification_Sub()) break; |
|
| - | 1213 | UART1_PutString("="); |
|
| - | 1214 | } |
|
| - | 1215 | if(NCMAG_Identification2.Sub == MAG_IDF_LSM303D) // internal Sensor the LSM303D? |
|
| - | 1216 | { |
|
| - | 1217 | NCMAG_SensorType = TYPE_LSM303D; |
|
| - | 1218 | NCMAG_ConfigureSensor(); |
|
| - | 1219 | retval = 1; |
|
| - | 1220 | } |
|
| - | 1221 | else |
|
| - | 1222 | for(repeat = 0; repeat < 3; repeat++) // polling for LSM303DLH/DLM option by ACC address ack |
|
| - | 1223 | { |
|
| 1100 | retval = NCMAG_GetAccConfig(); |
1224 | retval = NCMAG_GetAccConfig(); |
| 1101 | if(retval) break; // break loop on success |
1225 | if(retval) break; // break loop on success |
| 1102 | } |
1226 | } |
| 1103 | if(retval) |
1227 | if(retval) // it is a sensor with ACC |
| 1104 | { |
1228 | { |
| 1105 | // initialize ACC sensor |
- | |
| 1106 | NCMAG_Init_ACCSensor(); |
1229 | switch(NCMAG_Identification2.Sub) |
| 1107 | 1230 | { |
|
| 1108 | NCMAG_SensorType = TYPE_LSM303DLH; |
1231 | case MAG_IDF_LSM303DLM: NCMAG_SensorType = TYPE_LSM303DLM; |
| 1109 | // polling of sub identification |
1232 | NCMAG_Init_ACCSensor(); |
| 1110 | repeat = 0; |
1233 | break; |
| 1111 | for(repeat = 0; repeat < 12; repeat++) |
1234 | case MAG_IDF_LSM303DLH: NCMAG_SensorType = TYPE_LSM303DLH; |
| 1112 | { |
- | |
| 1113 | retval = NCMAG_GetIdentification_Sub(); |
1235 | NCMAG_Init_ACCSensor(); |
| 1114 | if(retval) break; // break loop on success |
- | |
| 1115 | } |
- | |
| 1116 | if(retval) |
1236 | break; |
| 1117 | { |
- | |
| 1118 | if(NCMAG_Identification2.Sub == MAG_IDF_LSM303DLM) NCMAG_SensorType = TYPE_LSM303DLM; |
1237 | case MAG_IDF_LSM303D: NCMAG_SensorType = TYPE_LSM303D; |
| - | 1238 | break; |
|
| 1119 | } |
1239 | } |
| 1120 | } |
1240 | } |
| 1121 | // get id bytes |
1241 | // get id bytes |
| 1122 | retval = 0; |
1242 | retval = 0; |
| 1123 | for(repeat = 0; repeat < 3; repeat++) |
1243 | for(repeat = 0; repeat < 3; repeat++) |
| 1124 | { |
1244 | { |
| 1125 | retval = NCMAG_GetIdentification(); |
1245 | retval = NCMAG_GetIdentification(); |
| 1126 | if(retval) break; // break loop on success |
1246 | if(retval) break; // break loop on success |
| 1127 | } |
1247 | } |
| 1128 | 1248 | } |
|
| 1129 | // if we got an answer to id request |
1249 | // if we got an answer to id request |
| 1130 | if(retval) |
1250 | if(retval) |
| 1131 | { |
1251 | { |
| 1132 | u8 n1[] = "\n\r HMC5843"; |
1252 | u8 n1[] = "\n\r HMC5843"; |
| 1133 | u8 n2[] = "\n\r LSM303DLH"; |
1253 | u8 n2[] = "\n\r LSM303DLH"; |
| 1134 | u8 n3[] = "\n\r LSM303DLM"; |
1254 | u8 n3[] = "\n\r LSM303DLM"; |
| - | 1255 | u8 n4[] = "\n\r LSM303D"; |
|
| - | 1256 | u8 n5[] = "\n\r ??"; |
|
| 1135 | u8* pn = n1; |
1257 | u8* pn = n5; |
| Line 1136... | Line 1258... | ||
| 1136 | 1258 | ||
| 1137 | switch(NCMAG_SensorType) |
1259 | switch(NCMAG_SensorType) |
| 1138 | { |
1260 | { |
| 1139 | case TYPE_HMC5843: |
1261 | case TYPE_HMC5843: |
| Line 1143... | Line 1265... | ||
| 1143 | pn = n2; |
1265 | pn = n2; |
| 1144 | break; |
1266 | break; |
| 1145 | case TYPE_LSM303DLM: |
1267 | case TYPE_LSM303DLM: |
| 1146 | pn = n3; |
1268 | pn = n3; |
| 1147 | break; |
1269 | break; |
| - | 1270 | case TYPE_LSM303D: |
|
| - | 1271 | pn = n4; |
|
| - | 1272 | break; |
|
| 1148 | } |
1273 | } |
| Line 1149... | Line 1274... | ||
| 1149 | 1274 | ||
| 1150 | sprintf(msg, " %s ID 0x%02x/%02x/%02x-%02x", pn, NCMAG_Identification.A, NCMAG_Identification.B, NCMAG_Identification.C,NCMAG_Identification2.Sub); |
1275 | sprintf(msg, " %s ID 0x%02x/%02x/%02x-%02x", pn, NCMAG_Identification.A, NCMAG_Identification.B, NCMAG_Identification.C,NCMAG_Identification2.Sub); |
| 1151 | UART1_PutString(msg); |
1276 | UART1_PutString(msg); |
| 1152 | if ( (NCMAG_Identification.A == MAG_IDA) |
1277 | if (( (NCMAG_Identification.A == MAG_IDA) |
| 1153 | && (NCMAG_Identification.B == MAG_IDB) |
1278 | && (NCMAG_Identification.B == MAG_IDB) |
| - | 1279 | && (NCMAG_Identification.C == MAG_IDC)) |
|
| 1154 | && (NCMAG_Identification.C == MAG_IDC)) |
1280 | || NCMAG_SensorType == TYPE_LSM303D) |
| 1155 | { |
1281 | { |
| Line 1156... | Line 1282... | ||
| 1156 | NCMAG_Present = 1; |
1282 | NCMAG_Present = 1; |
| 1157 | 1283 | ||