Rev 613 | Rev 704 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 613 | Rev 686 | ||
|---|---|---|---|
| Line 70... | Line 70... | ||
| 70 | 70 | ||
| 71 | u8 NCMAG_Present = 0; |
71 | u8 NCMAG_Present = 0; |
| 72 | u8 NCMAG_IsCalibrated = 0; |
72 | u8 NCMAG_IsCalibrated = 0; |
| 73 | u8 NCMAG_SensorType = TYPE_NONE; |
73 | u8 NCMAG_SensorType = TYPE_NONE; |
| - | 74 | u8 NCMAG_Orientation = 0; // 0 means unknown! |
|
| Line 74... | Line 75... | ||
| 74 | u8 NCMAG_Orientation = 0; // 0 means unknown! |
75 | u8 NCMAG_Compass_use_Orientation = 0; |
| 75 | 76 | ||
| Line 76... | Line 77... | ||
| 76 | #define CALIBRATION_VERSION 1 |
77 | #define CALIBRATION_VERSION 1 |
| Line 422... | Line 423... | ||
| 422 | speak = 1; |
423 | speak = 1; |
| 423 | CompassValueErrorCount = 0; |
424 | CompassValueErrorCount = 0; |
| 424 | if(Compass_CalState != OldCalState) // only once per state |
425 | if(Compass_CalState != OldCalState) // only once per state |
| 425 | { |
426 | { |
| 426 | UART1_PutString("\r\nStarting compass calibration"); |
427 | UART1_PutString("\r\nStarting compass calibration"); |
| 427 | if(Compass_I2CPort == NCMAG_PORT_EXTERN) |
428 | if(NCMAG_Compass_use_Orientation) |
| 428 | { |
429 | { |
| 429 | if(!NCMAG_Orientation) NCMAG_Orientation = NCMAG_GetOrientationFromAcc(); |
430 | if(!NCMAG_Orientation) NCMAG_Orientation = NCMAG_GetOrientationFromAcc(); |
| 430 | UART1_PutString(" - External sensor "); |
431 | UART1_PutString(" - External sensor "); |
| 431 | sprintf(msg, "with orientation: %d ", NCMAG_Orientation); |
432 | sprintf(msg, "with orientation: %d ", NCMAG_Orientation); |
| 432 | UART1_PutString(msg); |
433 | UART1_PutString(msg); |
| Line 668... | Line 669... | ||
| 668 | //---------------------------------------------------------------------- |
669 | //---------------------------------------------------------------------- |
| Line 669... | Line 670... | ||
| 669 | 670 | ||
| 670 | u8 NCMAG_GetOrientationFromAcc(void) |
671 | u8 NCMAG_GetOrientationFromAcc(void) |
| 671 | { |
672 | { |
| 672 | // only if external compass connected |
673 | // only if external compass connected |
| 673 | if(Compass_I2CPort != NCMAG_PORT_EXTERN) return(0); |
674 | if(!NCMAG_Compass_use_Orientation) return(0); |
| 674 | // MK must not be tilted |
675 | // MK must not be tilted |
| 675 | if((abs(FromFlightCtrl.AngleNick) > 300) || (abs(FromFlightCtrl.AngleRoll) > 300)) |
676 | if((abs(FromFlightCtrl.AngleNick) > 300) || (abs(FromFlightCtrl.AngleRoll) > 300)) |
| 676 | { |
677 | { |
| 677 | // UART1_PutString("\r\nTilted"); |
678 | // UART1_PutString("\r\nTilted"); |
| Line 984... | Line 985... | ||
| 984 | break; |
985 | break; |
| 985 | case TYPE_LSM303DLH: |
986 | case TYPE_LSM303DLH: |
| 986 | case TYPE_LSM303DLM: |
987 | case TYPE_LSM303DLM: |
| 987 | case TYPE_LSM303D: |
988 | case TYPE_LSM303D: |
| 988 | delay = 20; // next cycle after 20 ms |
989 | delay = 20; // next cycle after 20 ms |
| 989 | if(s-- || (Compass_I2CPort == NCMAG_PORT_INTERN)) |
990 | if(s-- || (NCMAG_Compass_use_Orientation == 0)) |
| 990 | { |
991 | { |
| 991 | NCMAG_GetMagVector(5); |
992 | NCMAG_GetMagVector(5); |
| 992 | } |
993 | } |
| 993 | else // having an external compass, read every 50th cycle the ACC vec |
994 | else // having an external compass, read every 50th cycle the ACC vec |
| 994 | { // try to initialize if no data are there |
995 | { // try to initialize if no data are there |
| Line 1130... | Line 1131... | ||
| 1130 | } |
1131 | } |
| Line 1131... | Line 1132... | ||
| 1131 | 1132 | ||
| 1132 | 1133 | ||
| 1133 | void NCMAG_CheckOrientation(void) |
1134 | void NCMAG_CheckOrientation(void) |
| 1134 | { // only for external sensor |
1135 | { // only for external sensor |
| 1135 | if(Compass_I2CPort == NCMAG_PORT_EXTERN) |
1136 | if(NCMAG_Compass_use_Orientation) |
| 1136 | { |
1137 | { |
| 1137 | NCMAG_Orientation = NCMAG_GetOrientationFromAcc(); |
1138 | NCMAG_Orientation = NCMAG_GetOrientationFromAcc(); |
| 1138 | if(NCMAG_Orientation != (Calibration.Version>>4)) NCMAG_IsCalibrated = 0; |
1139 | if(NCMAG_Orientation != (Calibration.Version>>4)) NCMAG_IsCalibrated = 0; |
| Line 1287... | Line 1288... | ||
| 1287 | if(!NCMAG_IsCalibrated) UART1_PutString("\r\n Not calibrated!"); |
1288 | if(!NCMAG_IsCalibrated) UART1_PutString("\r\n Not calibrated!"); |
| 1288 | } |
1289 | } |
| 1289 | else UART1_PutString("\r\n EEPROM data not available!!!!!!!!!!!!!!!"); |
1290 | else UART1_PutString("\r\n EEPROM data not available!!!!!!!!!!!!!!!"); |
| Line 1290... | Line 1291... | ||
| 1290 | 1291 | ||
| 1291 | // in case of an external sensor, try to get the orientation by acc readings |
1292 | // in case of an external sensor, try to get the orientation by acc readings |
| 1292 | if(Compass_I2CPort == NCMAG_PORT_EXTERN) |
1293 | if(NCMAG_Compass_use_Orientation) |
| 1293 | { |
1294 | { |
| 1294 | // try to get orientation by acc sensor values |
1295 | // try to get orientation by acc sensor values |
| 1295 | for(repeat = 0; repeat < 100; repeat++) |
1296 | for(repeat = 0; repeat < 100; repeat++) |
| 1296 | { |
1297 | { |