WebSVN - NaviCtrl - Diff - Rev 472 and 473

 u8 NCMAG_Present = 0;
 u8 NCMAG_IsCalibrated = 0;
 u8 I2C_CompassPort = 1;
-u8 I2C_CompassPort = 1;
+u8 ExtCompassOrientation = 0;
 u8 *I2C_BufferPnt;
 #define TYPE_LSM303DLH          2
 #define TYPE_LSM303DLM          3
-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;
+u8 NCMAG_SensorType = TYPE_NONE;
 Calibration_t Calibration;              // calibration data in RAM
 volatile s16vec_t AccRawVector;
+{
         u16 address;
         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
-        if(intern == I2C_INTERN_1) address = EEPROM_ADR_MAG_CALIBRATION_INTERN;
+        {
-        else address = EEPROM_ADR_MAG_CALIBRATION_EXTERN;
+         address = EEPROM_ADR_MAG_CALIBRATION_EXTERN;
+         Calibration.Version = CALIBRATION_VERSION + ExtCompassOrientation * 16;
         Calibration.Version = CALIBRATION_VERSION;
         for(i = 0; i<(sizeof(Calibration)-1); i++)
+        {
                 crc += pBuff[i];
+        }
+                {
                         crc += pBuff[i];
+                }
                 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);
+}
 // rx data handler for magnetic sensor raw data
 void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)
 {       // 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
+                        if(NCMAG_SensorType == TYPE_LSM303DLM)  Z = raw; // here Z and Y are exchanged
-                        else                                                                    MagRawVector.Y = raw;
+                        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
+                        if(NCMAG_SensorType == TYPE_LSM303DLM)  Y = raw; // here Z and Y are exchanged
-                        else                                                                    MagRawVector.Z = raw;
+                        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
                 MagVector.X = MagRawVector.X;
+        }
+}
 // 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;
+u8 GetExtCompassOrientation(void)
+{
-//      DebugOut.Analog[17] = AccRawVector.Y;
+        if(I2C_CompassPort != I2C_EXTERN_0) return(0);
+        if(abs(FromFlightCtrl.AngleNick) > 300) return(0); // MK tilted
-        filter_z = (filter_z * 7 + AccRawVector.Z) / 8;
+        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
-//      DebugOut.Analog[18] = filter_z;
+        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
-//      DebugOut.Analog[19] = AccRawVector.Z;
+        return(0);
+}
 // rx data handler for reading magnetic sensor configuration
 void NCMAG_UpdateMagConfig(u8* pRxBuffer, u8 RxBufferSize)
 {       // if number of byte are matching
         if(RxBufferSize == sizeof(MagConfig) )
 // ----------------------------------------------------------------------------------------
 u8 NCMAG_SetAccConfig(void)
+{
         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;
                 memcpy((u8*)(&I2C_BufferPnt[TxBytes]), (u8*)&AccConfig, sizeof(AccConfig));
                 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;
+                        }
+                }
+        }
                 I2C_TransmissionFunc(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagVector, sizeof(MagVector));
+        }
+}
 //----------------------------------------------------------------
-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
                 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;
-                I2C_TransmissionFunc(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector));
+//DebugOut.Analog[18] = AccRawVector.Z;
+        }
+}
 //----------------------------------------------------------------
 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;
         return(NCMAG_SetAccConfig());
                 InitNC_MagnetSensor();
                         TimerUpdate = SetDelay(20);    // back into the old time-slot
+                }
                 else
+                {
-//                      static u8 s = 0;
+                        static u8 s = 0;
                         // check for new calibration state
                         Compass_UpdateCalState();
                         if(Compass_CalState) NCMAG_Calibrate();
                                         NCMAG_GetMagVector();
                                         delay = 20;
                                         break;
                                 case TYPE_LSM303DLH:
                                 case TYPE_LSM303DLM:
-                                        NCMAG_GetMagVector();
                                         delay = 20;
-/*                                      if(s){ NCMAG_GetMagVector(); s = 0;}
+                                        if(s-- || (I2C_CompassPort == I2C_INTERN_1)) NCMAG_GetMagVector();
+                                        else
+                                         {
+                                          if(AccRawVector.X + AccRawVector.Y + AccRawVector.Z == 0) NCMAG_Init_ACCSensor();
-                                        else { NCMAG_GetAccVector(); s = 1;}
+                                          NCMAG_GetAccVector(0);
-                                        delay = 10;
+                                          delay = 3;
+                                          s = 50;
+                                         };
-*/
                                         break;
+                        }
                         if(send_config == 24) TimerUpdate = SetDelay(15);    // next event is the re-configuration
                         else TimerUpdate = SetDelay(delay);    // every 20 ms are 50 Hz
+                }
         // get id bytes
         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++;
          NCMAG_SelectI2CBus(I2C_CompassPort);
+        }
         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");
+         }
+        }
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         NCMAG_Present = 0;

Subversion Repositories NaviCtrl

(root)/tags/V2.06a/ncmag.c @ 577 - Rev 472 → 473