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; |