Rev 472 | Rev 474 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 472 | Rev 473 | ||
---|---|---|---|
Line 71... | Line 71... | ||
71 | 71 | ||
72 | u8 NCMAG_Present = 0; |
72 | u8 NCMAG_Present = 0; |
Line 73... | Line 73... | ||
73 | u8 NCMAG_IsCalibrated = 0; |
73 | u8 NCMAG_IsCalibrated = 0; |
- | 74 | ||
Line 74... | Line 75... | ||
74 | 75 | u8 I2C_CompassPort = 1; |
|
75 | u8 I2C_CompassPort = 1; |
76 | u8 ExtCompassOrientation = 0; |
76 | 77 | ||
77 | u8 *I2C_BufferPnt; |
78 | u8 *I2C_BufferPnt; |
Line 87... | Line 88... | ||
87 | #define TYPE_LSM303DLH 2 |
88 | #define TYPE_LSM303DLH 2 |
88 | #define TYPE_LSM303DLM 3 |
89 | #define TYPE_LSM303DLM 3 |
Line 89... | Line 90... | ||
89 | 90 | ||
Line 90... | Line -... | ||
90 | u8 NCMAG_SensorType = TYPE_NONE; |
- | |
91 | - | ||
92 | #define EEPROM_ADR_MAG_CALIBRATION_INTERN 50 // two calibrtion sets for extern and intern sensor |
- | |
93 | #define EEPROM_ADR_MAG_CALIBRATION_EXTERN 70 |
- | |
94 | - | ||
95 | #define CALIBRATION_VERSION 1 |
- | |
96 | #define MAG_CALIBRATION_COMPATIBLE 0xA2 |
- | |
97 | - | ||
98 | #define NCMAG_MIN_RAWVALUE -2047 |
- | |
99 | #define NCMAG_MAX_RAWVALUE 2047 |
- | |
100 | #define NCMAG_INVALID_DATA -4096 |
- | |
101 | - | ||
102 | typedef struct |
- | |
103 | { |
- | |
104 | s16 Range; |
- | |
105 | s16 Offset; |
- | |
106 | } __attribute__((packed)) Scaling_t; |
- | |
107 | - | ||
108 | typedef struct |
- | |
109 | { |
- | |
110 | Scaling_t MagX; |
- | |
111 | Scaling_t MagY; |
- | |
112 | Scaling_t MagZ; |
- | |
113 | u8 Version; |
- | |
114 | u8 crc; |
- | |
115 | } __attribute__((packed)) Calibration_t; |
91 | u8 NCMAG_SensorType = TYPE_NONE; |
116 | 92 | ||
117 | Calibration_t Calibration; // calibration data in RAM |
93 | Calibration_t Calibration; // calibration data in RAM |
Line 118... | Line 94... | ||
118 | volatile s16vec_t AccRawVector; |
94 | volatile s16vec_t AccRawVector; |
Line 314... | Line 290... | ||
314 | { |
290 | { |
315 | u16 address; |
291 | u16 address; |
316 | u8 i, crc = MAG_CALIBRATION_COMPATIBLE; |
292 | u8 i, crc = MAG_CALIBRATION_COMPATIBLE; |
317 | EEPROM_Result_t eres; |
293 | EEPROM_Result_t eres; |
318 | u8 *pBuff = (u8*)&Calibration; |
294 | u8 *pBuff = (u8*)&Calibration; |
- | 295 | Calibration.Version = CALIBRATION_VERSION; |
|
Line 319... | Line 296... | ||
319 | 296 | ||
- | 297 | if(intern == I2C_INTERN_1) address = EEPROM_ADR_MAG_CALIBRATION_INTERN; |
|
- | 298 | else |
|
320 | if(intern == I2C_INTERN_1) address = EEPROM_ADR_MAG_CALIBRATION_INTERN; |
299 | { |
321 | else address = EEPROM_ADR_MAG_CALIBRATION_EXTERN; |
- | |
322 | 300 | address = EEPROM_ADR_MAG_CALIBRATION_EXTERN; |
|
- | 301 | Calibration.Version = CALIBRATION_VERSION + ExtCompassOrientation * 16; |
|
323 | Calibration.Version = CALIBRATION_VERSION; |
302 | } |
324 | for(i = 0; i<(sizeof(Calibration)-1); i++) |
303 | for(i = 0; i<(sizeof(Calibration)-1); i++) |
325 | { |
304 | { |
326 | crc += pBuff[i]; |
305 | crc += pBuff[i]; |
327 | } |
306 | } |
Line 347... | Line 326... | ||
347 | { |
326 | { |
348 | crc += pBuff[i]; |
327 | crc += pBuff[i]; |
349 | } |
328 | } |
350 | crc = ~crc; |
329 | crc = ~crc; |
351 | if(Calibration.crc != crc) return(0); // crc mismatch |
330 | if(Calibration.crc != crc) return(0); // crc mismatch |
352 | if(Calibration.Version == CALIBRATION_VERSION) return(1); |
331 | if((Calibration.Version & 0x0f) == CALIBRATION_VERSION) return(1); |
353 | } |
332 | } |
354 | return(0); |
333 | return(0); |
355 | } |
334 | } |
Line 493... | Line 472... | ||
493 | // rx data handler for magnetic sensor raw data |
472 | // rx data handler for magnetic sensor raw data |
494 | void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize) |
473 | void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize) |
495 | { // if number of bytes are matching |
474 | { // if number of bytes are matching |
496 | if(RxBufferSize == sizeof(MagRawVector) ) |
475 | if(RxBufferSize == sizeof(MagRawVector) ) |
497 | { // byte order from big to little endian |
476 | { // byte order from big to little endian |
498 | s16 raw; |
477 | s16 raw, X = 0, Y = 0, Z = 0; |
499 | raw = pRxBuffer[0]<<8; |
478 | raw = pRxBuffer[0]<<8; |
500 | raw+= pRxBuffer[1]; |
479 | raw+= pRxBuffer[1]; |
501 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.X = raw; |
480 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) X = raw; |
502 | raw = pRxBuffer[2]<<8; |
481 | raw = pRxBuffer[2]<<8; |
503 | raw+= pRxBuffer[3]; |
482 | raw+= pRxBuffer[3]; |
504 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) |
483 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) |
505 | { |
484 | { |
506 | if(NCMAG_SensorType == TYPE_LSM303DLM) MagRawVector.Z = raw; // here Z and Y are exchanged |
485 | if(NCMAG_SensorType == TYPE_LSM303DLM) Z = raw; // here Z and Y are exchanged |
507 | else MagRawVector.Y = raw; |
486 | else Y = raw; |
508 | } |
487 | } |
509 | raw = pRxBuffer[4]<<8; |
488 | raw = pRxBuffer[4]<<8; |
510 | raw+= pRxBuffer[5]; |
489 | raw+= pRxBuffer[5]; |
511 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) |
490 | if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) |
512 | { |
491 | { |
513 | if(NCMAG_SensorType == TYPE_LSM303DLM) MagRawVector.Y = raw; // here Z and Y are exchanged |
492 | if(NCMAG_SensorType == TYPE_LSM303DLM) Y = raw; // here Z and Y are exchanged |
514 | else MagRawVector.Z = raw; |
493 | else Z = raw; |
- | 494 | } |
|
- | 495 | switch(ExtCompassOrientation) |
|
- | 496 | { |
|
- | 497 | case 0: |
|
- | 498 | case 1: |
|
- | 499 | default: |
|
- | 500 | MagRawVector.X = X; |
|
- | 501 | MagRawVector.Y = Y; |
|
- | 502 | MagRawVector.Z = Z; |
|
- | 503 | break; |
|
- | 504 | case 2: |
|
- | 505 | MagRawVector.X = -X; |
|
- | 506 | MagRawVector.Y = Y; |
|
- | 507 | MagRawVector.Z = -Z; |
|
- | 508 | break; |
|
- | 509 | case 3: |
|
- | 510 | MagRawVector.X = -Z; |
|
- | 511 | MagRawVector.Y = Y; |
|
- | 512 | MagRawVector.Z = X; |
|
- | 513 | break; |
|
- | 514 | case 4: |
|
- | 515 | MagRawVector.X = Z; |
|
- | 516 | MagRawVector.Y = Y; |
|
- | 517 | MagRawVector.Z = -X; |
|
- | 518 | break; |
|
- | 519 | case 5: |
|
- | 520 | MagRawVector.X = X; |
|
- | 521 | MagRawVector.Y = -Z; |
|
- | 522 | MagRawVector.Z = Y; |
|
- | 523 | break; |
|
- | 524 | case 6: |
|
- | 525 | MagRawVector.X = -X; |
|
- | 526 | MagRawVector.Y = -Z; |
|
- | 527 | MagRawVector.Z = -Y; |
|
- | 528 | break; |
|
515 | } |
529 | } |
516 | } |
530 | } |
517 | if(Compass_CalState || !NCMAG_IsCalibrated) |
531 | if(Compass_CalState || !NCMAG_IsCalibrated) |
518 | { // mark out data invalid |
532 | { // mark out data invalid |
519 | MagVector.X = MagRawVector.X; |
533 | MagVector.X = MagRawVector.X; |
Line 531... | Line 545... | ||
531 | } |
545 | } |
532 | } |
546 | } |
533 | // rx data handler for acceleration raw data |
547 | // rx data handler for acceleration raw data |
534 | void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize) |
548 | void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize) |
535 | { // if number of byte are matching |
549 | { // if number of byte are matching |
536 | static s32 filter_z; |
- | |
537 | if(RxBufferSize == sizeof(AccRawVector) ) |
550 | if(RxBufferSize == sizeof(AccRawVector) ) |
538 | { |
551 | { |
539 | memcpy((u8*)&AccRawVector, pRxBuffer,sizeof(AccRawVector)); |
552 | memcpy((u8*)&AccRawVector, pRxBuffer,sizeof(AccRawVector)); |
540 | } |
553 | } |
- | 554 | } |
|
- | 555 | ||
541 | // DebugOut.Analog[16] = AccRawVector.X; |
556 | u8 GetExtCompassOrientation(void) |
- | 557 | { |
|
542 | // DebugOut.Analog[17] = AccRawVector.Y; |
558 | if(I2C_CompassPort != I2C_EXTERN_0) return(0); |
- | 559 | if(abs(FromFlightCtrl.AngleNick) > 300) return(0); // MK tilted |
|
543 | filter_z = (filter_z * 7 + AccRawVector.Z) / 8; |
560 | if(abs(FromFlightCtrl.AngleRoll) > 300) return(0); |
Line 544... | Line 561... | ||
544 | 561 | ||
- | 562 | if(AccRawVector.Z > 3300) return(1); // Flach - Bestückung oben - Pfeil nach vorn |
|
- | 563 | else |
|
- | 564 | if(AccRawVector.Z < -3300) return(2); // Flach - Bestückung unten - Pfeil nach vorn |
|
- | 565 | else |
|
- | 566 | if(AccRawVector.X > 3300) return(3); // Flach - Bestückung Links - Pfeil nach vorn |
|
545 | // DebugOut.Analog[18] = filter_z; |
567 | else |
- | 568 | if(AccRawVector.X < -3300) return(4); // Flach - Bestückung rechts - Pfeil nach vorn |
|
- | 569 | else |
|
- | 570 | if(AccRawVector.Y > 3300) return(5); // Stehend - Pfeil nach oben - 'front' nach vorn |
|
- | 571 | else |
|
- | 572 | if(AccRawVector.Y < -3300) return(6); // Stehend - Pfeil nach unten - 'front' nach vorn |
|
546 | // DebugOut.Analog[19] = AccRawVector.Z; |
573 | return(0); |
- | 574 | } |
|
547 | } |
575 | |
548 | // rx data handler for reading magnetic sensor configuration |
576 | // rx data handler for reading magnetic sensor configuration |
549 | void NCMAG_UpdateMagConfig(u8* pRxBuffer, u8 RxBufferSize) |
577 | void NCMAG_UpdateMagConfig(u8* pRxBuffer, u8 RxBufferSize) |
550 | { // if number of byte are matching |
578 | { // if number of byte are matching |
551 | if(RxBufferSize == sizeof(MagConfig) ) |
579 | if(RxBufferSize == sizeof(MagConfig) ) |
Line 609... | Line 637... | ||
609 | // ---------------------------------------------------------------------------------------- |
637 | // ---------------------------------------------------------------------------------------- |
610 | u8 NCMAG_SetAccConfig(void) |
638 | u8 NCMAG_SetAccConfig(void) |
611 | { |
639 | { |
612 | u8 retval = 0; |
640 | u8 retval = 0; |
613 | // try to catch the i2c buffer within 100 ms timeout |
641 | // try to catch the i2c buffer within 100 ms timeout |
614 | if(I2C_LockBufferFunc(100)) |
642 | if(I2C_LockBufferFunc(50)) |
615 | { |
643 | { |
616 | u8 TxBytes = 0; |
644 | u8 TxBytes = 0; |
617 | I2C_BufferPnt[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT; |
645 | I2C_BufferPnt[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT; |
618 | memcpy((u8*)(&I2C_BufferPnt[TxBytes]), (u8*)&AccConfig, sizeof(AccConfig)); |
646 | memcpy((u8*)(&I2C_BufferPnt[TxBytes]), (u8*)&AccConfig, sizeof(AccConfig)); |
619 | TxBytes += sizeof(AccConfig); |
647 | TxBytes += sizeof(AccConfig); |
620 | if(I2C_TransmissionFunc(ACC_SLAVE_ADDRESS, TxBytes, 0, 0)) |
648 | if(I2C_TransmissionFunc(ACC_SLAVE_ADDRESS, TxBytes, 0, 0)) |
621 | { |
649 | { |
622 | if(I2C_WaitForEndOfTransmissionFunc(100)) |
650 | if(I2C_WaitForEndOfTransmissionFunc(50)) |
623 | { |
651 | { |
624 | if(*I2C_ErrorPnt == I2C_ERROR_NONE) retval = 1; |
652 | if(*I2C_ErrorPnt == I2C_ERROR_NONE) retval = 1; |
625 | } |
653 | } |
626 | } |
654 | } |
627 | } |
655 | } |
Line 707... | Line 735... | ||
707 | I2C_TransmissionFunc(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagVector, sizeof(MagVector)); |
735 | I2C_TransmissionFunc(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagVector, sizeof(MagVector)); |
708 | } |
736 | } |
709 | } |
737 | } |
Line 710... | Line 738... | ||
710 | 738 | ||
711 | //---------------------------------------------------------------- |
739 | //---------------------------------------------------------------- |
712 | void NCMAG_GetAccVector(void) |
740 | void NCMAG_GetAccVector(u8 timeout) |
713 | { |
741 | { |
714 | // try to catch the I2C buffer within 0 ms |
742 | // try to catch the I2C buffer within 0 ms |
715 | if(I2C_LockBufferFunc(0)) |
743 | if(I2C_LockBufferFunc(timeout)) |
716 | { |
744 | { |
717 | u16 TxBytes = 0; |
745 | u16 TxBytes = 0; |
718 | // set register pointer |
746 | // set register pointer |
719 | I2C_BufferPnt[TxBytes++] = REG_ACC_X_LSB|REG_ACC_MASK_AUTOINCREMENT; |
747 | I2C_BufferPnt[TxBytes++] = REG_ACC_X_LSB|REG_ACC_MASK_AUTOINCREMENT; |
720 | // initiate transmission |
748 | // initiate transmission |
- | 749 | I2C_TransmissionFunc(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector)); |
|
- | 750 | //DebugOut.Analog[16] = AccRawVector.X; |
|
- | 751 | //DebugOut.Analog[17] = AccRawVector.Y; |
|
721 | I2C_TransmissionFunc(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector)); |
752 | //DebugOut.Analog[18] = AccRawVector.Z; |
722 | } |
753 | } |
Line 723... | Line 754... | ||
723 | } |
754 | } |
724 | 755 | ||
Line 753... | Line 784... | ||
753 | 784 | ||
754 | //---------------------------------------------------------------- |
785 | //---------------------------------------------------------------- |
755 | u8 NCMAG_Init_ACCSensor(void) |
786 | u8 NCMAG_Init_ACCSensor(void) |
756 | { |
787 | { |
757 | AccConfig.ctrl_1 = ACC_CRTL1_PM_NORMAL|ACC_CRTL1_DR_50HZ|ACC_CRTL1_XEN|ACC_CRTL1_YEN|ACC_CRTL1_ZEN; |
788 | AccConfig.ctrl_1 = ACC_CRTL1_PM_NORMAL|ACC_CRTL1_DR_50HZ|ACC_CRTL1_XEN|ACC_CRTL1_YEN|ACC_CRTL1_ZEN; |
758 | AccConfig.ctrl_2 = 0;//ACC_CRTL2_FILTER32; |
789 | AccConfig.ctrl_2 = 0; |
759 | AccConfig.ctrl_3 = 0x00; |
790 | AccConfig.ctrl_3 = 0x00; |
760 | AccConfig.ctrl_4 = ACC_CTRL4_BDU | ACC_CTRL4_FS_8G; |
791 | AccConfig.ctrl_4 = ACC_CTRL4_BDU | ACC_CTRL4_FS_8G; |
761 | AccConfig.ctrl_5 = ACC_CTRL5_STW_OFF; |
792 | AccConfig.ctrl_5 = ACC_CTRL5_STW_OFF; |
762 | return(NCMAG_SetAccConfig()); |
793 | return(NCMAG_SetAccConfig()); |
Line 783... | Line 814... | ||
783 | InitNC_MagnetSensor(); |
814 | InitNC_MagnetSensor(); |
784 | TimerUpdate = SetDelay(20); // back into the old time-slot |
815 | TimerUpdate = SetDelay(20); // back into the old time-slot |
785 | } |
816 | } |
786 | else |
817 | else |
787 | { |
818 | { |
788 | // static u8 s = 0; |
819 | static u8 s = 0; |
789 | // check for new calibration state |
820 | // check for new calibration state |
790 | Compass_UpdateCalState(); |
821 | Compass_UpdateCalState(); |
791 | if(Compass_CalState) NCMAG_Calibrate(); |
822 | if(Compass_CalState) NCMAG_Calibrate(); |
Line 792... | Line 823... | ||
792 | 823 | ||
Line 797... | Line 828... | ||
797 | NCMAG_GetMagVector(); |
828 | NCMAG_GetMagVector(); |
798 | delay = 20; |
829 | delay = 20; |
799 | break; |
830 | break; |
800 | case TYPE_LSM303DLH: |
831 | case TYPE_LSM303DLH: |
801 | case TYPE_LSM303DLM: |
832 | case TYPE_LSM303DLM: |
802 | NCMAG_GetMagVector(); |
- | |
803 | delay = 20; |
833 | delay = 20; |
804 | /* if(s){ NCMAG_GetMagVector(); s = 0;} |
834 | if(s-- || (I2C_CompassPort == I2C_INTERN_1)) NCMAG_GetMagVector(); |
- | 835 | else |
|
- | 836 | { |
|
- | 837 | if(AccRawVector.X + AccRawVector.Y + AccRawVector.Z == 0) NCMAG_Init_ACCSensor(); |
|
805 | else { NCMAG_GetAccVector(); s = 1;} |
838 | NCMAG_GetAccVector(0); |
806 | delay = 10; |
839 | delay = 3; |
- | 840 | s = 50; |
|
- | 841 | }; |
|
807 | */ |
842 | |
808 | break; |
843 | break; |
809 | } |
844 | } |
810 | if(send_config == 24) TimerUpdate = SetDelay(15); // next event is the re-configuration |
845 | if(send_config == 24) TimerUpdate = SetDelay(15); // next event is the re-configuration |
811 | else TimerUpdate = SetDelay(delay); // every 20 ms are 50 Hz |
846 | else TimerUpdate = SetDelay(delay); // every 20 ms are 50 Hz |
812 | } |
847 | } |
Line 963... | Line 998... | ||
963 | 998 | ||
964 | // get id bytes |
999 | // get id bytes |
965 | retval = 0; |
1000 | retval = 0; |
966 | do |
1001 | do |
967 | { |
1002 | { |
- | 1003 | // retval = NCMAG_GetIdentification(); |
|
968 | retval = NCMAG_GetIdentification(); |
1004 | retval = NCMAG_GetAccConfig(); // only the sensor with ACC is supported |
969 | if(retval) break; // break loop on success |
1005 | if(retval) break; // break loop on success |
970 | // UART1_PutString("+"); |
1006 | // UART1_PutString("+"); |
971 | repeat++; |
1007 | repeat++; |
Line 978... | Line 1014... | ||
978 | NCMAG_SelectI2CBus(I2C_CompassPort); |
1014 | NCMAG_SelectI2CBus(I2C_CompassPort); |
979 | } |
1015 | } |
980 | else |
1016 | else |
981 | { |
1017 | { |
982 | UART1_PutString(" external sensor "); |
1018 | UART1_PutString(" external sensor "); |
- | 1019 | NCMAG_Init_ACCSensor(); |
|
- | 1020 | ||
- | 1021 | for(repeat = 0; repeat < 100; repeat++) |
|
- | 1022 | { |
|
- | 1023 | NCMAG_GetAccVector(10); // only the sensor with ACC is supported |
|
- | 1024 | ExtCompassOrientation = GetExtCompassOrientation(); |
|
- | 1025 | if(ExtCompassOrientation) break; |
|
- | 1026 | } |
|
- | 1027 | //DebugOut.Analog[19] = repeat; |
|
- | 1028 | ||
- | 1029 | if(!ExtCompassOrientation) UART1_PutString(" (Orientation unknown!)"); |
|
- | 1030 | else |
|
- | 1031 | { |
|
- | 1032 | NCMag_CalibrationRead(I2C_CompassPort); |
|
- | 1033 | sprintf(msg, "with orientation: %d ",ExtCompassOrientation ); |
|
- | 1034 | UART1_PutString(msg); |
|
- | 1035 | if(ExtCompassOrientation != Calibration.Version / 16) |
|
- | 1036 | { |
|
- | 1037 | sprintf(msg, "\n\r! Warining: calibrated orientation was %d !",Calibration.Version / 16); |
|
- | 1038 | UART1_PutString(msg); |
|
- | 1039 | } |
|
- | 1040 | else UART1_PutString("ok"); |
|
- | 1041 | } |
|
- | 1042 | ||
983 | } |
1043 | } |
984 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1044 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 985... | Line 1045... | ||
985 | 1045 | ||
986 | NCMAG_Present = 0; |
1046 | NCMAG_Present = 0; |