| 73,6 → 73,7 |
|---|
| u8 NCMAG_IsCalibrated = 0; |
| |
| u8 I2C_CompassPort = 1; |
| u8 ExtCompassOrientation = 0; |
| |
| u8 *I2C_BufferPnt; |
| u8 *I2C_ErrorPnt; |
| 89,31 → 90,6 |
|---|
| |
| u8 NCMAG_SensorType = TYPE_NONE; |
| |
| #define EEPROM_ADR_MAG_CALIBRATION_INTERN 50 // two calibrtion sets for extern and intern sensor |
| #define EEPROM_ADR_MAG_CALIBRATION_EXTERN 70 |
| |
| #define CALIBRATION_VERSION 1 |
| #define MAG_CALIBRATION_COMPATIBLE 0xA2 |
| |
| #define NCMAG_MIN_RAWVALUE -2047 |
| #define NCMAG_MAX_RAWVALUE 2047 |
| #define NCMAG_INVALID_DATA -4096 |
| |
| typedef struct |
| { |
| s16 Range; |
| s16 Offset; |
| } __attribute__((packed)) Scaling_t; |
| |
| typedef struct |
| { |
| Scaling_t MagX; |
| Scaling_t MagY; |
| Scaling_t MagZ; |
| u8 Version; |
| u8 crc; |
| } __attribute__((packed)) Calibration_t; |
| |
| Calibration_t Calibration; // calibration data in RAM |
| volatile s16vec_t AccRawVector; |
| volatile s16vec_t MagRawVector; |
| 316,11 → 292,14 |
|---|
| u8 i, crc = MAG_CALIBRATION_COMPATIBLE; |
| EEPROM_Result_t eres; |
| u8 *pBuff = (u8*)&Calibration; |
| Calibration.Version = CALIBRATION_VERSION; |
| |
| if(intern == I2C_INTERN_1) address = EEPROM_ADR_MAG_CALIBRATION_INTERN; |
| else address = EEPROM_ADR_MAG_CALIBRATION_EXTERN; |
| |
| Calibration.Version = CALIBRATION_VERSION; |
| else |
| { |
| address = EEPROM_ADR_MAG_CALIBRATION_EXTERN; |
| Calibration.Version = CALIBRATION_VERSION + ExtCompassOrientation * 16; |
| } |
| for(i = 0; i<(sizeof(Calibration)-1); i++) |
| { |
| crc += pBuff[i]; |
| 349,7 → 328,7 |
|---|
| } |
| crc = ~crc; |
| if(Calibration.crc != crc) return(0); // crc mismatch |
| if(Calibration.Version == CALIBRATION_VERSION) return(1); |
| if((Calibration.Version & 0x0f) == CALIBRATION_VERSION) return(1); |
| } |
| return(0); |
| } |
| 495,24 → 474,59 |
|---|
| { // if number of bytes are matching |
| if(RxBufferSize == sizeof(MagRawVector) ) |
| { // byte order from big to little endian |
| s16 raw; |
| s16 raw, X = 0, Y = 0, Z = 0; |
| raw = pRxBuffer[0]<<8; |
| raw+= pRxBuffer[1]; |
| if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.X = raw; |
| if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) X = raw; |
| raw = pRxBuffer[2]<<8; |
| raw+= pRxBuffer[3]; |
| if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) |
| { |
| if(NCMAG_SensorType == TYPE_LSM303DLM) MagRawVector.Z = raw; // here Z and Y are exchanged |
| else MagRawVector.Y = raw; |
| if(NCMAG_SensorType == TYPE_LSM303DLM) Z = raw; // here Z and Y are exchanged |
| else Y = raw; |
| } |
| raw = pRxBuffer[4]<<8; |
| raw+= pRxBuffer[5]; |
| if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) |
| { |
| if(NCMAG_SensorType == TYPE_LSM303DLM) MagRawVector.Y = raw; // here Z and Y are exchanged |
| else MagRawVector.Z = raw; |
| if(NCMAG_SensorType == TYPE_LSM303DLM) Y = raw; // here Z and Y are exchanged |
| else Z = raw; |
| } |
| switch(ExtCompassOrientation) |
| { |
| case 0: |
| case 1: |
| default: |
| MagRawVector.X = X; |
| MagRawVector.Y = Y; |
| MagRawVector.Z = Z; |
| break; |
| case 2: |
| MagRawVector.X = -X; |
| MagRawVector.Y = Y; |
| MagRawVector.Z = -Z; |
| break; |
| case 3: |
| MagRawVector.X = -Z; |
| MagRawVector.Y = Y; |
| MagRawVector.Z = X; |
| break; |
| case 4: |
| MagRawVector.X = Z; |
| MagRawVector.Y = Y; |
| MagRawVector.Z = -X; |
| break; |
| case 5: |
| MagRawVector.X = X; |
| MagRawVector.Y = -Z; |
| MagRawVector.Z = Y; |
| break; |
| case 6: |
| MagRawVector.X = -X; |
| MagRawVector.Y = -Z; |
| MagRawVector.Z = -Y; |
| break; |
| } |
| } |
| if(Compass_CalState || !NCMAG_IsCalibrated) |
| { // mark out data invalid |
| 533,18 → 547,32 |
|---|
| // rx data handler for acceleration raw data |
| void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize) |
| { // if number of byte are matching |
| static s32 filter_z; |
| if(RxBufferSize == sizeof(AccRawVector) ) |
| { |
| memcpy((u8*)&AccRawVector, pRxBuffer,sizeof(AccRawVector)); |
| } |
| // DebugOut.Analog[16] = AccRawVector.X; |
| // DebugOut.Analog[17] = AccRawVector.Y; |
| filter_z = (filter_z * 7 + AccRawVector.Z) / 8; |
| } |
| |
| // DebugOut.Analog[18] = filter_z; |
| // DebugOut.Analog[19] = AccRawVector.Z; |
| u8 GetExtCompassOrientation(void) |
| { |
| if(I2C_CompassPort != I2C_EXTERN_0) return(0); |
| if(abs(FromFlightCtrl.AngleNick) > 300) return(0); // MK tilted |
| if(abs(FromFlightCtrl.AngleRoll) > 300) return(0); |
| |
| if(AccRawVector.Z > 3300) return(1); // Flach - Bestückung oben - Pfeil nach vorn |
| else |
| if(AccRawVector.Z < -3300) return(2); // Flach - Bestückung unten - Pfeil nach vorn |
| else |
| if(AccRawVector.X > 3300) return(3); // Flach - Bestückung Links - Pfeil nach vorn |
| else |
| if(AccRawVector.X < -3300) return(4); // Flach - Bestückung rechts - Pfeil nach vorn |
| else |
| if(AccRawVector.Y > 3300) return(5); // Stehend - Pfeil nach oben - 'front' nach vorn |
| else |
| if(AccRawVector.Y < -3300) return(6); // Stehend - Pfeil nach unten - 'front' nach vorn |
| return(0); |
| } |
| |
| // rx data handler for reading magnetic sensor configuration |
| void NCMAG_UpdateMagConfig(u8* pRxBuffer, u8 RxBufferSize) |
| { // if number of byte are matching |
| 611,7 → 639,7 |
|---|
| { |
| u8 retval = 0; |
| // try to catch the i2c buffer within 100 ms timeout |
| if(I2C_LockBufferFunc(100)) |
| if(I2C_LockBufferFunc(50)) |
| { |
| u8 TxBytes = 0; |
| I2C_BufferPnt[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT; |
| 619,7 → 647,7 |
|---|
| TxBytes += sizeof(AccConfig); |
| if(I2C_TransmissionFunc(ACC_SLAVE_ADDRESS, TxBytes, 0, 0)) |
| { |
| if(I2C_WaitForEndOfTransmissionFunc(100)) |
| if(I2C_WaitForEndOfTransmissionFunc(50)) |
| { |
| if(*I2C_ErrorPnt == I2C_ERROR_NONE) retval = 1; |
| } |
| 709,10 → 737,10 |
|---|
| } |
| |
| //---------------------------------------------------------------- |
| void NCMAG_GetAccVector(void) |
| void NCMAG_GetAccVector(u8 timeout) |
| { |
| // try to catch the I2C buffer within 0 ms |
| if(I2C_LockBufferFunc(0)) |
| if(I2C_LockBufferFunc(timeout)) |
| { |
| u16 TxBytes = 0; |
| // set register pointer |
| 719,6 → 747,9 |
|---|
| I2C_BufferPnt[TxBytes++] = REG_ACC_X_LSB|REG_ACC_MASK_AUTOINCREMENT; |
| // initiate transmission |
| I2C_TransmissionFunc(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector)); |
| //DebugOut.Analog[16] = AccRawVector.X; |
| //DebugOut.Analog[17] = AccRawVector.Y; |
| //DebugOut.Analog[18] = AccRawVector.Z; |
| } |
| } |
| |
| 755,7 → 786,7 |
|---|
| u8 NCMAG_Init_ACCSensor(void) |
| { |
| AccConfig.ctrl_1 = ACC_CRTL1_PM_NORMAL|ACC_CRTL1_DR_50HZ|ACC_CRTL1_XEN|ACC_CRTL1_YEN|ACC_CRTL1_ZEN; |
| AccConfig.ctrl_2 = 0;//ACC_CRTL2_FILTER32; |
| AccConfig.ctrl_2 = 0; |
| AccConfig.ctrl_3 = 0x00; |
| AccConfig.ctrl_4 = ACC_CTRL4_BDU | ACC_CTRL4_FS_8G; |
| AccConfig.ctrl_5 = ACC_CTRL5_STW_OFF; |
| 785,7 → 816,7 |
|---|
| } |
| else |
| { |
| // static u8 s = 0; |
| static u8 s = 0; |
| // check for new calibration state |
| Compass_UpdateCalState(); |
| if(Compass_CalState) NCMAG_Calibrate(); |
| 799,12 → 830,16 |
|---|
| break; |
| case TYPE_LSM303DLH: |
| case TYPE_LSM303DLM: |
| NCMAG_GetMagVector(); |
| delay = 20; |
| /* if(s){ NCMAG_GetMagVector(); s = 0;} |
| else { NCMAG_GetAccVector(); s = 1;} |
| delay = 10; |
| */ |
| if(s-- || (I2C_CompassPort == I2C_INTERN_1)) NCMAG_GetMagVector(); |
| else |
| { |
| if(AccRawVector.X + AccRawVector.Y + AccRawVector.Z == 0) NCMAG_Init_ACCSensor(); |
| NCMAG_GetAccVector(0); |
| delay = 3; |
| s = 50; |
| }; |
| |
| break; |
| } |
| if(send_config == 24) TimerUpdate = SetDelay(15); // next event is the re-configuration |
| 965,7 → 1000,8 |
|---|
| retval = 0; |
| do |
| { |
| retval = NCMAG_GetIdentification(); |
| // retval = NCMAG_GetIdentification(); |
| retval = NCMAG_GetAccConfig(); // only the sensor with ACC is supported |
| if(retval) break; // break loop on success |
| // UART1_PutString("+"); |
| repeat++; |
| 980,6 → 1016,30 |
|---|
| else |
| { |
| UART1_PutString(" external sensor "); |
| NCMAG_Init_ACCSensor(); |
| |
| for(repeat = 0; repeat < 100; repeat++) |
| { |
| NCMAG_GetAccVector(10); // only the sensor with ACC is supported |
| ExtCompassOrientation = GetExtCompassOrientation(); |
| if(ExtCompassOrientation) break; |
| } |
| //DebugOut.Analog[19] = repeat; |
| |
| if(!ExtCompassOrientation) UART1_PutString(" (Orientation unknown!)"); |
| else |
| { |
| NCMag_CalibrationRead(I2C_CompassPort); |
| sprintf(msg, "with orientation: %d ",ExtCompassOrientation ); |
| UART1_PutString(msg); |
| if(ExtCompassOrientation != Calibration.Version / 16) |
| { |
| sprintf(msg, "\n\r! Warining: calibrated orientation was %d !",Calibration.Version / 16); |
| UART1_PutString(msg); |
| } |
| else UART1_PutString("ok"); |
| } |
| |
| } |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| |