Rev 329 | Rev 338 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 329 | Rev 330 | ||
|---|---|---|---|
| Line 286... | Line 286... | ||
| 286 | } |
286 | } |
| Line 287... | Line 287... | ||
| 287 | 287 | ||
| 288 | 288 | ||
| - | 289 | void NCMAG_Calibrate(void) |
|
| 289 | void NCMAG_Calibrate(void) |
290 | { |
| 290 | { |
291 | u8 msg[64]; |
| 291 | static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0; |
292 | static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0; |
| Line 292... | Line 293... | ||
| 292 | static s16 X = 0, Y = 0, Z = 0; |
293 | static s16 X = 0, Y = 0, Z = 0; |
| Line 341... | Line 342... | ||
| 341 | Calibration.MagZ.Offset = (Zmin + Zmax) / 2; |
342 | Calibration.MagZ.Offset = (Zmin + Zmax) / 2; |
| 342 | if((Calibration.MagX.Range > MIN_CALIBRATION) && (Calibration.MagY.Range > MIN_CALIBRATION) && (Calibration.MagZ.Range > MIN_CALIBRATION)) |
343 | if((Calibration.MagX.Range > MIN_CALIBRATION) && (Calibration.MagY.Range > MIN_CALIBRATION) && (Calibration.MagZ.Range > MIN_CALIBRATION)) |
| 343 | { |
344 | { |
| 344 | NCMAG_IsCalibrated = NCMag_CalibrationWrite(); |
345 | NCMAG_IsCalibrated = NCMag_CalibrationWrite(); |
| 345 | BeepTime = 2500; |
346 | BeepTime = 2500; |
| 346 | UART1_PutString("\r\n Calibration okay"); |
347 | UART1_PutString("\r\n Calibration okay\n\r"); |
| 347 | } |
348 | } |
| 348 | else |
349 | else |
| 349 | { |
350 | { |
| - | 351 | UART1_PutString("\r\n Calibration FAILED - Values too low"); |
|
| - | 352 | if(Calibration.MagX.Range < MIN_CALIBRATION) UART1_PutString("X! "); |
|
| - | 353 | if(Calibration.MagY.Range < MIN_CALIBRATION) UART1_PutString("y! "); |
|
| - | 354 | if(Calibration.MagZ.Range < MIN_CALIBRATION) UART1_PutString("Z! "); |
|
| - | 355 | UART1_PutString("\r\n"); |
|
| 350 | // restore old calibration data from eeprom |
356 | // restore old calibration data from eeprom |
| 351 | NCMAG_IsCalibrated = NCMag_CalibrationRead(); |
357 | NCMAG_IsCalibrated = NCMag_CalibrationRead(); |
| 352 | UART1_PutString("\r\n Calibration FAILED - Values too low: "); |
- | |
| 353 | if(Calibration.MagX.Range < MIN_CALIBRATION) UART1_PutString("X! "); |
- | |
| 354 | if(Calibration.MagY.Range < MIN_CALIBRATION) UART1_PutString("Y! "); |
- | |
| 355 | if(Calibration.MagZ.Range < MIN_CALIBRATION) UART1_PutString("Z! "); |
- | |
| 356 | } |
358 | } |
| - | 359 | sprintf(msg, "X: (%i - %i = %i)\r\n",Xmax,Xmin,Xmax - Xmin); |
|
| - | 360 | UART1_PutString(msg); |
|
| - | 361 | sprintf(msg, "Y: (%i - %i = %i)\r\n",Ymax,Ymin,Ymax - Ymin); |
|
| - | 362 | UART1_PutString(msg); |
|
| - | 363 | sprintf(msg, "Z: (%i - %i = %i)\r\n",Zmax,Zmin,Zmax - Zmin); |
|
| - | 364 | UART1_PutString(msg); |
|
| 357 | } |
365 | } |
| 358 | break; |
366 | break; |
| Line 359... | Line 367... | ||
| 359 | 367 | ||
| 360 | default: |
368 | default: |
| Line 391... | Line 399... | ||
| 391 | raw = pRxBuffer[0]<<8; |
399 | raw = pRxBuffer[0]<<8; |
| 392 | raw+= pRxBuffer[1]; |
400 | raw+= pRxBuffer[1]; |
| 393 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.X = raw; |
401 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.X = raw; |
| 394 | raw = pRxBuffer[2]<<8; |
402 | raw = pRxBuffer[2]<<8; |
| 395 | raw+= pRxBuffer[3]; |
403 | raw+= pRxBuffer[3]; |
| 396 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.Y = raw; |
404 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) |
| - | 405 | { |
|
| - | 406 | if(NCMAG_Identification2.Sub == 0x3c) MagRawVector.Z = raw; // here Z and Y are exchanged |
|
| - | 407 | else MagRawVector.Y = raw; |
|
| - | 408 | } |
|
| 397 | raw = pRxBuffer[4]<<8; |
409 | raw = pRxBuffer[4]<<8; |
| 398 | raw+= pRxBuffer[5]; |
410 | raw+= pRxBuffer[5]; |
| 399 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.Z = raw; |
411 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) |
| - | 412 | { |
|
| - | 413 | if(NCMAG_Identification2.Sub == 0x3c) MagRawVector.Y = raw; // here Z and Y are exchanged |
|
| - | 414 | else MagRawVector.Z = raw; |
|
| - | 415 | } |
|
| - | 416 | ||
| 400 | } |
417 | } |
| 401 | if(Compass_CalState || !NCMAG_IsCalibrated) |
418 | if(Compass_CalState || !NCMAG_IsCalibrated) |
| 402 | { // mark out data invalid |
419 | { // mark out data invalid |
| 403 | MagVector.X = MagRawVector.X; |
420 | MagVector.X = MagRawVector.X; |
| 404 | MagVector.Y = MagRawVector.Y; |
421 | MagVector.Y = MagRawVector.Y; |
| Line 575... | Line 592... | ||
| 575 | void NCMAG_GetMagVector(void) |
592 | void NCMAG_GetMagVector(void) |
| 576 | { |
593 | { |
| 577 | // try to catch the I2C buffer within 0 ms |
594 | // try to catch the I2C buffer within 0 ms |
| 578 | if(I2C_LockBuffer(0)) |
595 | if(I2C_LockBuffer(0)) |
| 579 | { |
596 | { |
| - | 597 | // s16 tmp; |
|
| 580 | u16 TxBytes = 0; |
598 | u16 TxBytes = 0; |
| 581 | // set register pointer |
599 | // set register pointer |
| 582 | I2C_Buffer[TxBytes++] = REG_MAG_DATAX_MSB; |
600 | I2C_Buffer[TxBytes++] = REG_MAG_DATAX_MSB; |
| 583 | // initiate transmission |
601 | // initiate transmission |
| 584 | I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagVector, sizeof(MagVector)); |
602 | I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagVector, sizeof(MagVector)); |
| Line 597... | Line 615... | ||
| 597 | // initiate transmission |
615 | // initiate transmission |
| 598 | I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector)); |
616 | I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector)); |
| 599 | } |
617 | } |
| 600 | } |
618 | } |
| Line -... | Line 619... | ||
| - | 619 | ||
| - | 620 | //---------------------------------------------------------------- |
|
| - | 621 | void InitNC_MagnetSensor(void) |
|
| - | 622 | { |
|
| - | 623 | s16 xscale, yscale, zscale; |
|
| - | 624 | u8 crb_gain, cra_rate; |
|
| - | 625 | u8 i = 0, retval = 1; |
|
| - | 626 | ||
| - | 627 | switch(NCMAG_MagType) |
|
| - | 628 | { |
|
| - | 629 | case MAG_TYPE_HMC5843: |
|
| - | 630 | crb_gain = HMC5843_CRB_GAIN_10GA; |
|
| - | 631 | cra_rate = HMC5843_CRA_RATE_50HZ; |
|
| - | 632 | xscale = HMC5843_TEST_XSCALE; |
|
| - | 633 | yscale = HMC5843_TEST_YSCALE; |
|
| - | 634 | zscale = HMC5843_TEST_ZSCALE; |
|
| - | 635 | break; |
|
| - | 636 | ||
| - | 637 | case MAG_TYPE_LSM303DLH: |
|
| - | 638 | crb_gain = LSM303DLH_CRB_GAIN_13GA; |
|
| - | 639 | cra_rate = LSM303DLH_CRA_RATE_75HZ; |
|
| - | 640 | xscale = LSM303DLH_TEST_XSCALE; |
|
| - | 641 | yscale = LSM303DLH_TEST_YSCALE; |
|
| - | 642 | zscale = LSM303DLH_TEST_ZSCALE; |
|
| - | 643 | break; |
|
| - | 644 | ||
| - | 645 | default: |
|
| - | 646 | return(0); |
|
| - | 647 | } |
|
| - | 648 | ||
| - | 649 | MagConfig.cra = cra_rate|CRA_MODE_NORMAL; |
|
| - | 650 | MagConfig.crb = crb_gain; |
|
| - | 651 | MagConfig.mode = MODE_CONTINUOUS; |
|
| - | 652 | NCMAG_SetMagConfig(); |
|
| - | 653 | } |
|
| - | 654 | ||
| 601 | 655 | ||
| 602 | // -------------------------------------------------------- |
656 | // -------------------------------------------------------- |
| 603 | void NCMAG_Update(void) |
657 | void NCMAG_Update(void) |
| 604 | { |
658 | { |
| 605 | static u32 TimerUpdate = 0; |
659 | static u32 TimerUpdate = 0; |
| Line 615... | Line 669... | ||
| 615 | { |
669 | { |
| 616 | if(Compass_Heading != -1) send_config = 0; // no re-configuration if value is valid |
670 | if(Compass_Heading != -1) send_config = 0; // no re-configuration if value is valid |
| 617 | if(++send_config == 25) // 500ms |
671 | if(++send_config == 25) // 500ms |
| 618 | { |
672 | { |
| 619 | send_config = 0; |
673 | send_config = 0; |
| 620 | MagConfig.mode = MODE_CONTINUOUS; |
- | |
| 621 | // activate positive bias field |
- | |
| 622 | NCMAG_SetMagConfig(); |
674 | InitNC_MagnetSensor(); |
| 623 | TimerUpdate = SetDelay(15); // back into the old time-slot |
675 | TimerUpdate = SetDelay(15); // back into the old time-slot |
| 624 | } |
676 | } |
| 625 | else |
677 | else |
| 626 | { |
678 | { |
| 627 | // check for new calibration state |
679 | // check for new calibration state |
| Line 632... | Line 684... | ||
| 632 | else TimerUpdate = SetDelay(20); // every 20 ms are 50 Hz |
684 | else TimerUpdate = SetDelay(20); // every 20 ms are 50 Hz |
| 633 | } |
685 | } |
| 634 | } |
686 | } |
| 635 | } |
687 | } |
| Line -... | Line 688... | ||
| - | 688 | ||
| 636 | 689 | ||
| 637 | // -------------------------------------------------------- |
690 | // -------------------------------------------------------- |
| 638 | u8 NCMAG_SelfTest(void) |
691 | u8 NCMAG_SelfTest(void) |
| 639 | { |
692 | { |
| 640 | u8 msg[64]; |
693 | u8 msg[64]; |
| Line 815... | Line 868... | ||
| 815 | UART1_PutString("\r\n Selftest failed!!!!!!!!!!!!!!!!!!!!\r\n"); |
868 | UART1_PutString("\r\n Selftest failed!!!!!!!!!!!!!!!!!!!!\r\n"); |
| 816 | LED_RED_ON; |
869 | LED_RED_ON; |
| 817 | NCMAG_IsCalibrated = 0; |
870 | NCMAG_IsCalibrated = 0; |
| 818 | } else UART1_PutString("\r\n Selftest ok"); |
871 | } else UART1_PutString("\r\n Selftest ok"); |
| 819 | } |
872 | } |
| 820 | - | ||
| - | 873 | else InitNC_MagnetSensor(); |
|
| 821 | } |
874 | } |
| 822 | else |
875 | else |
| 823 | { |
876 | { |
| 824 | UART1_PutString("\n\r Not compatible!"); |
877 | UART1_PutString("\n\r Not compatible!"); |
| 825 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE; |
878 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE; |