Rev 475 | Rev 482 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 475 | Rev 480 | ||
---|---|---|---|
Line 368... | Line 368... | ||
368 | if(!ExtCompassOrientation) ExtCompassOrientation = GetExtCompassOrientation(); |
368 | if(!ExtCompassOrientation) ExtCompassOrientation = GetExtCompassOrientation(); |
369 | UART1_PutString(" - External sensor "); |
369 | UART1_PutString(" - External sensor "); |
370 | sprintf(msg, "with orientation: %d ",ExtCompassOrientation); |
370 | sprintf(msg, "with orientation: %d ",ExtCompassOrientation); |
371 | UART1_PutString(msg); |
371 | UART1_PutString(msg); |
372 | } |
372 | } |
373 | else UART1_PutString(" - Internal sensor"); |
373 | else UART1_PutString(" - Internal sensor "); |
374 | } |
374 | } |
375 | break; |
375 | break; |
376 | case 2: // 2nd step of calibration |
376 | case 2: // 2nd step of calibration |
377 | // find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane |
377 | // find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane |
378 | if(X < Xmin) { Xmin = X; BeepTime = 20;} |
378 | if(X < Xmin) { Xmin = X; BeepTime = 20;} |
Line 750... | Line 750... | ||
750 | 750 | ||
751 | // ---------------------------------------------------------------------------------------- |
751 | // ---------------------------------------------------------------------------------------- |
752 | void NCMAG_GetMagVector(void) |
752 | void NCMAG_GetMagVector(void) |
753 | { |
753 | { |
754 | // try to catch the I2C buffer within 0 ms |
754 | // try to catch the I2C buffer within 0 ms |
755 | if(I2C_LockBufferFunc(0)) |
755 | if(I2C_LockBufferFunc(5)) |
756 | { |
756 | { |
757 | u16 TxBytes = 0; |
757 | u16 TxBytes = 0; |
758 | // set register pointer |
758 | // set register pointer |
759 | I2C_BufferPnt[TxBytes++] = REG_MAG_DATAX_MSB; |
759 | I2C_BufferPnt[TxBytes++] = REG_MAG_DATAX_MSB; |
Line 817... | Line 817... | ||
817 | AccConfig.ctrl_4 = ACC_CTRL4_BDU | ACC_CTRL4_FS_8G; |
817 | AccConfig.ctrl_4 = ACC_CTRL4_BDU | ACC_CTRL4_FS_8G; |
818 | AccConfig.ctrl_5 = ACC_CTRL5_STW_OFF; |
818 | AccConfig.ctrl_5 = ACC_CTRL5_STW_OFF; |
819 | return(NCMAG_SetAccConfig()); |
819 | return(NCMAG_SetAccConfig()); |
820 | } |
820 | } |
821 | // -------------------------------------------------------- |
821 | // -------------------------------------------------------- |
822 | void NCMAG_Update(void) |
822 | void NCMAG_Update(u8 init) |
823 | { |
823 | { |
824 | static u32 TimerUpdate = 0; |
824 | static u32 TimerUpdate = 0; |
825 | static s8 send_config = 0; |
825 | static s8 send_config = 0; |
826 | u32 delay = 20; |
826 | u32 delay = 20; |
- | 827 | if(init) TimerUpdate = SetDelay(10); |
|
- | 828 | ||
827 | // todo State Handling for both busses !! |
829 | // todo State Handling for both busses !! |
828 | if((I2C1_State == I2C_STATE_OFF) || (I2C_CompassPort == 0 && I2C0_State == I2C_STATE_OFF) || !NCMAG_Present ) |
830 | if((I2C1_State == I2C_STATE_OFF) || (I2C_CompassPort == 0 && I2C0_State == I2C_STATE_OFF)/* || !NCMAG_Present*/ ) |
829 | { |
831 | { |
830 | Compass_Heading = -1; |
832 | Compass_Heading = -1; |
831 | DebugOut.Analog[14]++; // count I2C error |
833 | DebugOut.Analog[14]++; // count I2C error |
- | 834 | TimerUpdate = SetDelay(10); |
|
832 | return; |
835 | return; |
833 | } |
836 | } |
834 | if(CheckDelay(TimerUpdate)) |
837 | if(CheckDelay(TimerUpdate))// && I2C0_State == I2C_STATE_IDLE && I2C1_State == I2C_STATE_IDLE) |
835 | { |
838 | { |
836 | if(Compass_Heading != -1) send_config = 0; // no re-configuration if value is valid |
839 | if(Compass_Heading != -1) send_config = 0; // no re-configuration if value is valid |
837 | if(++send_config == 25) // 500ms |
840 | if(++send_config == 25) // 500ms |
838 | { |
841 | { |
839 | send_config = -25; // next try after 1 second |
842 | send_config = -25; // next try after 1 second |
Line 855... | Line 858... | ||
855 | delay = 20; |
858 | delay = 20; |
856 | break; |
859 | break; |
857 | case TYPE_LSM303DLH: |
860 | case TYPE_LSM303DLH: |
858 | case TYPE_LSM303DLM: |
861 | case TYPE_LSM303DLM: |
859 | delay = 20; |
862 | delay = 20; |
- | 863 | //delay = 2; |
|
860 | if(s-- || (I2C_CompassPort == I2C_INTERN_1)) NCMAG_GetMagVector(); |
864 | if(s-- || (I2C_CompassPort == I2C_INTERN_1)) NCMAG_GetMagVector(); |
861 | else |
865 | else |
862 | { |
866 | { |
863 | if(AccRawVector.X + AccRawVector.Y + AccRawVector.Z == 0) NCMAG_Init_ACCSensor(); |
867 | if(AccRawVector.X + AccRawVector.Y + AccRawVector.Z == 0) NCMAG_Init_ACCSensor(); |
864 | NCMAG_GetAccVector(0); |
868 | NCMAG_GetAccVector(5); |
865 | delay = 10; |
869 | delay = 10; |
866 | s = 40; // about 0,8 sec |
870 | s = 40; // about 0,8 sec |
867 | }; |
871 | }; |
868 | if(!s) delay = 10; // ACC-Reading in the next step after 10ms |
872 | if(!s) delay = 10; // ACC-Reading in the next step after 10ms |
- | 873 | //if(!s) delay = 2; // ACC-Reading in the next step after 10ms |
|
869 | break; |
874 | break; |
870 | } |
875 | } |
871 | if(send_config == 24) TimerUpdate = SetDelay(15); // next event is the re-configuration |
876 | if(send_config == 24) TimerUpdate = SetDelay(15); // next event is the re-configuration |
872 | else TimerUpdate = SetDelay(delay); // every 20 ms are 50 Hz |
877 | else TimerUpdate = SetDelay(delay); // every 20 ms are 50 Hz |
873 | } |
878 | } |
Line 1012... | Line 1017... | ||
1012 | //---------------------------------------------------------------- |
1017 | //---------------------------------------------------------------- |
1013 | u8 NCMAG_Init(void) |
1018 | u8 NCMAG_Init(void) |
1014 | { |
1019 | { |
1015 | u8 msg[64]; |
1020 | u8 msg[64]; |
1016 | u8 retval = 0; |
1021 | u8 retval = 0; |
1017 | u8 repeat; |
1022 | u8 repeat = 0; |
Line 1018... | Line 1023... | ||
1018 | 1023 | ||
1019 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1024 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1020 | // Search external sensor |
1025 | // Search external sensor |
1021 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1026 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1022 | I2C_CompassPort = I2C_EXTERN_0; |
1027 | I2C_CompassPort = I2C_EXTERN_0; |
Line 1023... | Line 1028... | ||
1023 | NCMAG_SelectI2CBus(I2C_CompassPort); |
1028 | NCMAG_SelectI2CBus(I2C_CompassPort); |
1024 | 1029 | ||
1025 | // get id bytes |
- | |
- | 1030 | // get id bytes |
|
1026 | retval = 0; |
1031 | retval = 0; |
1027 | do |
1032 | for(repeat = 0; repeat < 5; repeat++) |
1028 | { |
1033 | { |
1029 | // retval = NCMAG_GetIdentification(); |
1034 | // retval = NCMAG_GetIdentification(); |
1030 | retval = NCMAG_GetAccConfig(); // only the sensor with ACC is supported |
1035 | retval = NCMAG_GetAccConfig(); // only the sensor with ACC is supported |
1031 | if(retval) break; // break loop on success |
1036 | if(retval) break; // break loop on success |
1032 | // UART1_PutString("+"); |
1037 | UART1_PutString("_"); |
1033 | repeat++; |
- | |
1034 | } while(repeat < 5); |
1038 | } |
1035 | 1039 | //retval = 1; |
|
1036 | if(!retval) |
1040 | if(!retval) |
1037 | { |
1041 | { |
1038 | UART1_PutString(" internal sensor"); |
1042 | UART1_PutString(" internal sensor"); |
Line 1062... | Line 1066... | ||
1062 | if(ExtCompassOrientation != Calibration.Version / 16) |
1066 | if(ExtCompassOrientation != Calibration.Version / 16) |
1063 | { |
1067 | { |
1064 | sprintf(msg, "\n\r! Warning: calibrated orientation was %d !",Calibration.Version / 16); |
1068 | sprintf(msg, "\n\r! Warning: calibrated orientation was %d !",Calibration.Version / 16); |
1065 | UART1_PutString(msg); |
1069 | UART1_PutString(msg); |
1066 | } |
1070 | } |
1067 | else UART1_PutString("ok"); |
1071 | else UART1_PutString("ok "); |
1068 | } |
1072 | } |
Line 1069... | Line 1073... | ||
1069 | 1073 | ||
1070 | } |
1074 | } |
Line 1071... | Line 1075... | ||
1071 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1075 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1072 | 1076 | ||
1073 | NCMAG_Present = 0; |
1077 | NCMAG_Present = 0; |
1074 | NCMAG_SensorType = TYPE_HMC5843; // assuming having an HMC5843 |
1078 | NCMAG_SensorType = TYPE_HMC5843; // assuming having an HMC5843 |
1075 | // polling for LSM302DLH/DLM option by ACC address ack |
- | |
1076 | repeat = 0; |
1079 | // polling for LSM302DLH/DLM option by ACC address ack |
1077 | do |
1080 | for(repeat = 0; repeat < 3; repeat++) |
1078 | { |
1081 | { |
1079 | retval = NCMAG_GetAccConfig(); |
- | |
1080 | if(retval) break; // break loop on success |
1082 | retval = NCMAG_GetAccConfig(); |
1081 | // UART1_PutString("*"); |
- | |
1082 | repeat++; |
1083 | if(retval) break; // break loop on success |
1083 | } while(repeat < 3); |
1084 | } |
1084 | if(retval) |
1085 | if(retval) |
1085 | { |
1086 | { |
Line 1086... | Line 1087... | ||
1086 | // initialize ACC sensor |
1087 | // initialize ACC sensor |
1087 | NCMAG_Init_ACCSensor(); |
1088 | NCMAG_Init_ACCSensor(); |
1088 | 1089 | ||
1089 | NCMAG_SensorType = TYPE_LSM303DLH; |
- | |
1090 | // polling of sub identification |
1090 | NCMAG_SensorType = TYPE_LSM303DLH; |
1091 | repeat = 0; |
1091 | // polling of sub identification |
1092 | do |
1092 | for(repeat = 0; repeat < 12; repeat++) |
1093 | { |
- | |
1094 | retval = NCMAG_GetIdentification_Sub(); |
1093 | { |
1095 | if(retval) break; // break loop on success |
- | |
1096 | // UART1_PutString("."); |
1094 | retval = NCMAG_GetIdentification_Sub(); |
1097 | repeat++; |
1095 | if(retval) break; // break loop on success |
1098 | }while(repeat < 12); |
1096 | } |
1099 | if(retval) |
1097 | if(retval) |
1100 | { |
1098 | { |
1101 | if(NCMAG_Identification2.Sub == MAG_IDF_LSM303DLM) NCMAG_SensorType = TYPE_LSM303DLM; |
1099 | if(NCMAG_Identification2.Sub == MAG_IDF_LSM303DLM) NCMAG_SensorType = TYPE_LSM303DLM; |
1102 | } |
1100 | } |
1103 | } |
- | |
1104 | // get id bytes |
1101 | } |
1105 | retval = 0; |
1102 | // get id bytes |
1106 | do |
1103 | for(repeat = 0; repeat < 3; repeat++) |
1107 | { |
- | |
1108 | retval = NCMAG_GetIdentification(); |
1104 | { |
1109 | if(retval) break; // break loop on success |
- | |
Line 1110... | Line 1105... | ||
1110 | // UART1_PutString("#"); |
1105 | retval = NCMAG_GetIdentification(); |
1111 | repeat++; |
1106 | if(retval) break; // break loop on success |
1112 | } while(repeat < 12); |
1107 | } |
1113 | 1108 |