WebSVN - NaviCtrl - Diff - Rev 360 and 394

 #include "main.h"
 u8 NCMAG_Present = 0;
+u8 NCMAG_IsCalibrated = 0;
-u8 NCMAG_IsCalibrated = 0;
+// supported magnetic sensor types
-#define MAG_TYPE_NONE           0
+#define TYPE_NONE                       0
+#define TYPE_HMC5843            1
+#define TYPE_LSM303DLH          2
-#define MAG_TYPE_HMC5843        1
+#define TYPE_LSM303DLM          3
-#define MAG_TYPE_LSM303DLH      2
 u8 NCMAG_MagType = MAG_TYPE_NONE;
+u8 NCMAG_SensorType = TYPE_NONE;
-#define CALIBRATION_VERSION                     1
+#define EEPROM_ADR_MAG_CALIBRATION              50
-#define EEPROM_ADR_MAG_CALIBRATION              50
+#define CALIBRATION_VERSION                     1
-#define MAG_CALIBRATION_COMPATIBEL              0xA2
+#define MAG_CALIBRATION_COMPATIBLE              0xA2
 #define REG_MAG_STATUS          0x09
 #define REG_MAG_IDA                     0x0A
 #define REG_MAG_IDB                     0x0B
 #define REG_MAG_IDC                     0x0C
-#define REG_MAG_IDF                     0x0F
+#define REG_MAG_IDF                     0x0F  // WHO_AM_I _M = 0x03c when LSM303DLM is connected
 // bit mask for configuration mode
 #define CRA_MODE_MASK           0x03
 // ids
 #define MAG_IDA         0x48
 #define MAG_IDB         0x34
+#define MAG_IDC         0x33
-#define MAG_IDC         0x33
+#define MAG_IDF_LSM303DLM       0x3C
 // the special HMC5843 interface
 // bit mask for rate
 #define HMC5843_CRB_GAIN_65GA           0xE0
 // self test value
 #define HMC5843_TEST_XSCALE             555
 #define HMC5843_TEST_YSCALE             555
 #define HMC5843_TEST_ZSCALE             555
-// clibration range
+// calibration range
 #define HMC5843_CALIBRATION_RANGE   600
 // the special LSM302DLH interface
 // bit mask for rate
 #define LSM303DLH_CRB_GAIN_25GA         0x60
 #define LSM303DLH_CRB_GAIN_40GA         0x80
 #define LSM303DLH_CRB_GAIN_47GA         0xA0
 #define LSM303DLH_CRB_GAIN_56GA         0xC0
 #define LSM303DLH_CRB_GAIN_81GA         0xE0
+typedef struct
+{
+        u8 A;
+        u8 B;
+        u8 C;
+} __attribute__((packed)) Identification_t;
+volatile Identification_t NCMAG_Identification;
+typedef struct
+{
+        u8 Sub;
+} __attribute__((packed)) Identification2_t;
+volatile Identification2_t NCMAG_Identification2;
+typedef struct
+{
+        u8 cra;
+        u8 crb;
+        u8 mode;
+} __attribute__((packed)) MagConfig_t;
+volatile MagConfig_t MagConfig;
 // self test value
 #define LSM303DLH_TEST_XSCALE   495
 #define LSM303DLH_TEST_YSCALE   495
 #define LSM303DLH_TEST_ZSCALE   470
 // clibration range
 #define LSM303_CALIBRATION_RANGE   550
 // the i2c ACC interface
+#define ACC_SLAVE_ADDRESS               0x30    // i2c slave for acc. sensor registers
+// multiple byte read/write mask
+#define REG_ACC_MASK_AUTOINCREMENT 0x80
 #define ACC_SLAVE_ADDRESS               0x30    // i2c slave for acc. sensor registers
 // register mapping
 #define REG_ACC_CTRL1                   0x20
 #define REG_ACC_CTRL2                   0x21
 #define REG_ACC_CTRL3                   0x22
 #define REG_ACC_Y_LSB                   0x2A
 #define REG_ACC_Y_MSB                   0x2B
 #define REG_ACC_Z_LSB                   0x2C
 #define REG_ACC_Z_MSB                   0x2D
+#define ACC_CRTL1_PM_DOWN               0x00
+#define ACC_CRTL1_PM_NORMAL             0x20
+#define ACC_CRTL1_PM_LOW_0_5HZ  0x40
+#define ACC_CRTL1_PM_LOW_1HZ    0x60
+#define ACC_CRTL1_PM_LOW_2HZ    0x80
+#define ACC_CRTL1_PM_LOW_5HZ    0xA0
+#define ACC_CRTL1_PM_LOW_10HZ   0xC0
+// Output data rate in normal power mode
+#define ACC_CRTL1_DR_50HZ               0x00
+#define ACC_CRTL1_DR_100HZ              0x08
+#define ACC_CRTL1_DR_400HZ              0x10
+#define ACC_CRTL1_DR_1000HZ             0x18
+// axis anable flags
+#define ACC_CRTL1_XEN                   0x01
+#define ACC_CRTL1_YEN                   0x02
+#define ACC_CRTL1_ZEN                   0x04
+#define ACC_CTRL4_BDU                   0x80 // Block data update, (0: continuos update; 1: output registers not updated between MSB and LSB reading)
+#define ACC_CTRL4_BLE                   0x40 // Big/little endian, (0: data LSB @ lower address; 1: data MSB @ lower address)
+#define ACC_CTRL4_FS_2G                 0x00
+#define ACC_CTRL4_FS_4G                 0x10
+#define ACC_CTRL4_FS_8G                 0x30
+#define ACC_CTRL4_STSIGN_PLUS   0x00
+#define ACC_CTRL4_STSIGN_MINUS  0x08
-typedef struct
-{
-        u8 A;
-        u8 B;
-        u8 C;
-} __attribute__((packed)) Identification_t;
-volatile Identification_t NCMAG_Identification;
-typedef struct
-{
-        u8 Sub;
-} __attribute__((packed)) Identification2_t;
-volatile Identification2_t NCMAG_Identification2;
-typedef struct
-{
-        u8 cra;
-        u8 crb;
+#define ACC_CTRL4_ST_ENABLE             0x02
-        u8 mode;
-} __attribute__((packed)) MagConfig_t;
 #define ACC_CTRL5_STW_ON                0x03
-volatile MagConfig_t MagConfig;
+#define ACC_CTRL5_STW_OFF               0x00
 typedef struct
 volatile AccConfig_t AccConfig;
 u8 NCMag_CalibrationWrite(void)
+{
-        u8 i, crc = MAG_CALIBRATION_COMPATIBEL;
+        u8 i, crc = MAG_CALIBRATION_COMPATIBLE;
         EEPROM_Result_t eres;
         u8 *pBuff = (u8*)&Calibration;
         return(i);
+}
 u8 NCMag_CalibrationRead(void)
 {
-        u8 i, crc = MAG_CALIBRATION_COMPATIBEL;
+        u8 i, crc = MAG_CALIBRATION_COMPATIBLE;
         u8 *pBuff = (u8*)&Calibration;
         if(EEPROM_SUCCESS == EEPROM_ReadBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration)))
+{
         u8 msg[64];
         static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
         static s16 X = 0, Y = 0, Z = 0;
         static u8 OldCalState = 0;
-        s16 MinCaclibration = 450;
+        s16 MinCalibration = 450;
         X = (4*X + MagRawVector.X + 3)/5;
         Y = (4*Y + MagRawVector.Y + 3)/5;
                 case 5:
                         // Save values
                         if(Compass_CalState != OldCalState) // avoid continously writing of eeprom!
-                        {
-//                              #define MIN_CALIBRATION    256
+                        {
-                                if(NCMAG_MagType == MAG_TYPE_HMC5843)
+                                switch(NCMAG_SensorType)
+                                {
-                                 {
+                                        case TYPE_HMC5843:
-                                  UART1_PutString("\r\nHMC5843 calibration\n\r");
+                                                UART1_PutString("\r\nHMC5843 calibration\n\r");
-                                  MinCaclibration = HMC5843_CALIBRATION_RANGE;
+                                                MinCalibration = HMC5843_CALIBRATION_RANGE;
+                                                break;
-                                 }
-                                if(NCMAG_MagType == MAG_TYPE_LSM303DLH)
+                                        case TYPE_LSM303DLH:
-                                 {
+                                        case TYPE_LSM303DLM:
-                                  UART1_PutString("\r\n\r\nLSM303 calibration\n\r");
+                                                UART1_PutString("\r\n\r\nLSM303 calibration\n\r");
+                                                MinCalibration = LSM303_CALIBRATION_RANGE;
-                                  MinCaclibration =LSM303_CALIBRATION_RANGE;
+                                        break;
+                                }
                                 if(EarthMagneticStrengthTheoretic)
                                  {
-                                  MinCaclibration = (MinCaclibration * EarthMagneticStrengthTheoretic) / 50;
+                                  MinCalibration = (MinCalibration * EarthMagneticStrengthTheoretic) / 50;
                                   sprintf(msg, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic);
                                   UART1_PutString(msg);
+                                 }
                                 Calibration.MagX.Offset = (Xmin + Xmax) / 2;
                                 Calibration.MagY.Range = Ymax - Ymin;
                                 Calibration.MagY.Offset = (Ymin + Ymax) / 2;
                                 Calibration.MagZ.Range = Zmax - Zmin;
                                 Calibration.MagZ.Offset = (Zmin + Zmax) / 2;
-                                if((Calibration.MagX.Range > MinCaclibration) && (Calibration.MagY.Range > MinCaclibration) && (Calibration.MagZ.Range > MinCaclibration))
+                                if((Calibration.MagX.Range > MinCalibration) && (Calibration.MagY.Range > MinCalibration) && (Calibration.MagZ.Range > MinCalibration))
+                                {
                                         NCMAG_IsCalibrated = NCMag_CalibrationWrite();
                                         BeepTime = 2500;
                                         UART1_PutString("\r\n-> Calibration okay <-\n\r");
+                                }
                                 else
+                                {
                                         UART1_PutString("\r\nCalibration FAILED - Values too low: ");
-                                    if(Calibration.MagX.Range < MinCaclibration) UART1_PutString("X! ");
+                                    if(Calibration.MagX.Range < MinCalibration) UART1_PutString("X! ");
-                                    if(Calibration.MagY.Range < MinCaclibration) UART1_PutString("Y! ");
+                                    if(Calibration.MagY.Range < MinCalibration) UART1_PutString("Y! ");
-                                    if(Calibration.MagZ.Range < MinCaclibration) UART1_PutString("Z! ");
+                                    if(Calibration.MagZ.Range < MinCalibration) UART1_PutString("Z! ");
                                         UART1_PutString("\r\n");
                                         // restore old calibration data from eeprom
                                         NCMAG_IsCalibrated = NCMag_CalibrationRead();
                                         UART1_PutString(msg);
                                         sprintf(msg, "Y: (%i - %i = %i)\r\n",Ymax,Ymin,Ymax - Ymin);
                                         UART1_PutString(msg);
                                         sprintf(msg, "Z: (%i - %i = %i)\r\n",Zmax,Zmin,Zmax - Zmin);
                                         UART1_PutString(msg);
-                                        sprintf(msg, "(Minimum ampilitude is: %i)\r\n",MinCaclibration);
+                                        sprintf(msg, "(Minimum ampilitude is: %i)\r\n",MinCalibration);
                                         UART1_PutString(msg);
+                        }
                         break;
                 if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.X = raw;
                 raw = pRxBuffer[2]<<8;
                 raw+= pRxBuffer[3];
             if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE)
+                {
-                  if(NCMAG_Identification2.Sub == 0x3c) MagRawVector.Z = raw; // here Z and Y are exchanged
+                        if(NCMAG_SensorType == TYPE_LSM303DLM)  MagRawVector.Z = raw; // here Z and Y are exchanged
-                  else MagRawVector.Y = raw;
+                        else                                                                    MagRawVector.Y = raw;
+                }
                 raw = pRxBuffer[4]<<8;
                 raw+= pRxBuffer[5];
                 if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE)
+                {
-                  if(NCMAG_Identification2.Sub == 0x3c) MagRawVector.Y = raw; // here Z and Y are exchanged
+                        if(NCMAG_SensorType == TYPE_LSM303DLM)  MagRawVector.Y = raw; // here Z and Y are exchanged
-                  else MagRawVector.Z = raw;
+                        else                                                                    MagRawVector.Z = raw;
+                }
-//MagRawVector.X += 2 * ((s32) FC.Poti[7] - 128);
+        }
         if(Compass_CalState || !NCMAG_IsCalibrated)
         {       // mark out data invalid
                 MagVector.X = MagRawVector.X;
                 MagVector.Y = MagRawVector.Y;
 {       // if number of byte are matching
         if(RxBufferSize == sizeof(AccRawVector) )
+        {
                 memcpy((u8*)&AccRawVector, pRxBuffer,sizeof(AccRawVector));
+        }
+        DebugOut.Analog[16] = AccRawVector.X;
+        DebugOut.Analog[17] = AccRawVector.Y;
+        DebugOut.Analog[18] = AccRawVector.Z;
+}
 // 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 retval = 0;
         // try to catch the i2c buffer within 100 ms timeout
         if(I2C_LockBuffer(100))
+        {
                 u8 TxBytes = 0;
-                I2C_Buffer[TxBytes++] = REG_ACC_CTRL1;
+                I2C_Buffer[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT;
                 memcpy((u8*)(&I2C_Buffer[TxBytes]), (u8*)&AccConfig, sizeof(AccConfig));
                 TxBytes += sizeof(AccConfig);
                 if(I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, 0, 0))
+                {
                         if(I2C_WaitForEndOfTransmission(100))
         u8 retval = 0;
         // try to catch the i2c buffer within 100 ms timeout
         if(I2C_LockBuffer(100))
+        {
                 u8 TxBytes = 0;
-                I2C_Buffer[TxBytes++] = REG_ACC_CTRL1;
+                I2C_Buffer[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT;
                 if(I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccConfig, sizeof(AccConfig)))
+                {
                         if(I2C_WaitForEndOfTransmission(100))
+                        {
                                 if(I2C_Error == I2C_ERROR_NONE) retval = 1;
 void NCMAG_GetMagVector(void)
+{
         // try to catch the I2C buffer within 0 ms
         if(I2C_LockBuffer(0))
+        {
-//       s16 tmp;
                 u16 TxBytes = 0;
                 // set register pointer
                 I2C_Buffer[TxBytes++] = REG_MAG_DATAX_MSB;
                 // initiate transmission
                 I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagVector, sizeof(MagVector));
         // try to catch the I2C buffer within 0 ms
         if(I2C_LockBuffer(0))
+        {
                 u16 TxBytes = 0;
                 // set register pointer
-                I2C_Buffer[TxBytes++] = REG_ACC_X_LSB;
+                I2C_Buffer[TxBytes++] = REG_ACC_X_LSB|REG_ACC_MASK_AUTOINCREMENT;
                 // initiate transmission
                 I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector));
+        }
+}
 //----------------------------------------------------------------
-void InitNC_MagnetSensor(void)
+u8 InitNC_MagnetSensor(void)
-{
         s16 xscale, yscale, zscale;
-        u8 crb_gain, cra_rate;
-//      u8  retval = 1;
+        u8 crb_gain, cra_rate;
-        switch(NCMAG_MagType)
+        switch(NCMAG_SensorType)
+        {
-                case MAG_TYPE_HMC5843:
+                case TYPE_HMC5843:
-                        crb_gain = HMC5843_CRB_GAIN_15GA;
-                        cra_rate = HMC5843_CRA_RATE_50HZ;
-                        xscale = HMC5843_TEST_XSCALE;
-                        yscale = HMC5843_TEST_YSCALE;
+                        crb_gain = HMC5843_CRB_GAIN_15GA;
-                        zscale = HMC5843_TEST_ZSCALE;
+                        cra_rate = HMC5843_CRA_RATE_50HZ;
+                        break;
                         break;
                 case TYPE_LSM303DLH:
-                case MAG_TYPE_LSM303DLH:
-                        crb_gain = LSM303DLH_CRB_GAIN_19GA;
-                        cra_rate = LSM303DLH_CRA_RATE_75HZ;
-                        xscale = LSM303DLH_TEST_XSCALE;
+                case TYPE_LSM303DLM:
-                        yscale = LSM303DLH_TEST_YSCALE;
+                        crb_gain = LSM303DLH_CRB_GAIN_19GA;
-                        zscale = LSM303DLH_TEST_ZSCALE;
+                        cra_rate = LSM303DLH_CRA_RATE_75HZ;
                         break;
                 default:
-                return;
+                return(0);
         }
+        MagConfig.cra = cra_rate|CRA_MODE_NORMAL;
+        MagConfig.crb = crb_gain;
-        MagConfig.cra = cra_rate|CRA_MODE_NORMAL;
+        MagConfig.mode = MODE_CONTINUOUS;
+        return(NCMAG_SetMagConfig());
+}
+//----------------------------------------------------------------
+u8 NCMAG_Init_ACCSensor(void)
+{
+        AccConfig.ctrl_1 = ACC_CRTL1_PM_NORMAL|ACC_CRTL1_DR_400HZ|ACC_CRTL1_XEN|ACC_CRTL1_YEN|ACC_CRTL1_ZEN;
-        MagConfig.crb = crb_gain;
+        AccConfig.ctrl_2 = 0x00;
-        MagConfig.mode = MODE_CONTINUOUS;
+        AccConfig.ctrl_3 = 0x00;
-        NCMAG_SetMagConfig();
+        AccConfig.ctrl_4 = ACC_CTRL4_BDU|ACC_CTRL4_FS_2G;
         AccConfig.ctrl_5 = ACC_CTRL5_STW_OFF;
         return(NCMAG_SetAccConfig());
+}
 // --------------------------------------------------------
-// --------------------------------------------------------
+void NCMAG_Update(void)
 void NCMAG_Update(void)
         static u32 TimerUpdate = 0;
-        static u32 TimerUpdate = 0;
+        static u8 send_config = 0;
-        static u8 send_config = 0;
+        u32 delay = 20;
         if( (I2C_State == I2C_STATE_OFF) || !NCMAG_Present )
         {
                 Compass_Heading = -1;
                 DebugOut.Analog[14]++; // count I2C error
                 return;
         }
         if(CheckDelay(TimerUpdate))
         {
-           if(Compass_Heading != -1) send_config = 0; // no re-configuration if value is valid
+                if(Compass_Heading != -1) send_config = 0; // no re-configuration if value is valid
-       if(++send_config == 25)   // 500ms
+        if(++send_config == 25)   // 500ms
+                {
-            {
+                        send_config = 0;
-                 send_config = 0;
+                InitNC_MagnetSensor();
-             InitNC_MagnetSensor();
+                        TimerUpdate = SetDelay(15);    // back into the old time-slot
+                }
+                else
+                {
+                        static u8 s = 0;
+                        // check for new calibration state
+                        Compass_UpdateCalState();
+                        if(Compass_CalState) NCMAG_Calibrate();
+                        // in case of LSM303 type
+                        switch(NCMAG_SensorType)
-                 TimerUpdate = SetDelay(15);    // back into the old time-slot
+                        {
+                                case TYPE_HMC5843:
+                                        NCMAG_GetMagVector();
+                                        delay = 20;
+                                        break;
-            }
+                                case TYPE_LSM303DLH:
-                else
+                                case TYPE_LSM303DLM:
                                         if(s){ NCMAG_GetMagVector(); s = 0;}
-                // check for new calibration state
+                                        else { NCMAG_GetAccVector(); s = 1;}
-                Compass_UpdateCalState();
+                                        delay = 10;
         s32 XMin = 0, XMax = 0, YMin = 0, YMax = 0, ZMin = 0, ZMax = 0;
         s16 xscale, yscale, zscale, scale_min, scale_max;
         u8 crb_gain, cra_rate;
         u8 i = 0, retval = 1;
-        switch(NCMAG_MagType)
+        switch(NCMAG_SensorType)
         {
-                case MAG_TYPE_HMC5843:
+                case TYPE_HMC5843:
                         crb_gain = HMC5843_CRB_GAIN_15GA;
                         cra_rate = HMC5843_CRA_RATE_50HZ;
                         xscale = HMC5843_TEST_XSCALE;
                         yscale = HMC5843_TEST_YSCALE;
                         zscale = HMC5843_TEST_ZSCALE;
                         break;
-                case MAG_TYPE_LSM303DLH:
+                case TYPE_LSM303DLH:
                         crb_gain = LSM303DLH_CRB_GAIN_19GA;
                         cra_rate = LSM303DLH_CRA_RATE_75HZ;
                         xscale = LSM303DLH_TEST_XSCALE;
                         yscale = LSM303DLH_TEST_YSCALE;
+                        zscale = LSM303DLH_TEST_ZSCALE;
+                        break;
+                case TYPE_LSM303DLM:
+                        // does not support self test feature
+                        done = retval;
-                        zscale = LSM303DLH_TEST_ZSCALE;
+                        return(retval);
                         break;
                 default:
-                return(0);
+                        return(0);
+        }
         // check scale for all axes
         // prepare scale limits
         LIMITS(xscale, scale_min, scale_max);
         xscale = (XMax - XMin)/(2*AVERAGE);
         if((xscale > scale_max) || (xscale < scale_min))
-     {
+    {
-          retval = 0;
+                retval = 0;
-      sprintf(msg, "\r\n Value X: %d not %d-%d !", xscale, scale_min,scale_max);
+        sprintf(msg, "\r\n Value X: %d not %d-%d !", xscale, scale_min,scale_max);
-          UART1_PutString(msg);
+                UART1_PutString(msg);
-     }
+    }
         LIMITS(yscale, scale_min, scale_max);
         yscale = (YMax - YMin)/(2*AVERAGE);
         if((yscale > scale_max) || (yscale < scale_min))
-     {
+    {
-          retval = 0;
+                retval = 0;
-      sprintf(msg, "\r\n Value Y: %d not %d-%d !", yscale, scale_min,scale_max);
+        sprintf(msg, "\r\n Value Y: %d not %d-%d !", yscale, scale_min,scale_max);
-          UART1_PutString(msg);
+                UART1_PutString(msg);
-     }
+    }
         LIMITS(zscale, scale_min, scale_max);
         zscale = (ZMax - ZMin)/(2*AVERAGE);
         if((zscale > scale_max) || (zscale < scale_min))
-         {
+        {
-          retval = 0;
+                retval = 0;
-      sprintf(msg, "\r\n Value Z: %d not %d-%d !", zscale, scale_min,scale_max);
+        sprintf(msg, "\r\n Value Z: %d not %d-%d !", zscale, scale_min,scale_max);
-          UART1_PutString(msg);
+                UART1_PutString(msg);
-     }
+    }
         done = retval;
         return(retval);
+}
         u8 msg[64];
         u8 retval = 0;
         u8 repeat;
         NCMAG_Present = 0;
-        NCMAG_MagType = MAG_TYPE_HMC5843;       // assuming having an HMC5843
+        NCMAG_SensorType = TYPE_HMC5843;        // assuming having an HMC5843
-        // polling for LSM302DLH option
+        // polling for LSM302DLH/DLM option by ACC address ack
         repeat = 0;
         do
+        {
                 retval = NCMAG_GetAccConfig();
                 if(retval) break; // break loop on success
                 UART1_PutString(".");
                 repeat++;
-        }while(repeat < 3);
-        if(retval) NCMAG_MagType = MAG_TYPE_LSM303DLH; // must be a LSM303DLH
-        // polling of identification
+        }while(repeat < 3);
-        repeat = 0;
-        do
+        if(retval)
+        {
+                // initialize ACC sensor
+                NCMAG_Init_ACCSensor();
+                NCMAG_SensorType = TYPE_LSM303DLH;
+                // polling of sub identification
+                repeat = 0;
+                do
-        {
+                {
-                retval = NCMAG_GetIdentification_Sub();
+                        retval = NCMAG_GetIdentification_Sub();
-                if(retval) break; // break loop on success
+                        if(retval) break; // break loop on success
-                UART1_PutString(".");
+                        UART1_PutString(".");
-                repeat++;
+                        repeat++;
+                }while(repeat < 12);
+                if(retval)
+                {
+                        if(NCMAG_Identification2.Sub == MAG_IDF_LSM303DLM)      NCMAG_SensorType = TYPE_LSM303DLM;
+                }
+        }
-        }while(repeat < 12);
+        // get id bytes
         retval = 0;
         do
+        {
                 retval = NCMAG_GetIdentification();
         if(retval)
+        {
                 u8 n1[] = "\n\r HMC5843";
                 u8 n2[] = "\n\r LSM303DLH";
                 u8 n3[] = "\n\r LSM303DLM";
-                u8* pn;
+                u8* pn = n1;
-                pn = n1;
-                if(NCMAG_MagType == MAG_TYPE_LSM303DLH)
+                switch(NCMAG_SensorType)
+                {
+                        case TYPE_HMC5843:
+                                pn = n1;
-                {
+                                break;
-                 if(NCMAG_Identification2.Sub == 0x3c) pn = n3;
+                        case TYPE_LSM303DLH:
+                                pn = n2;
+                                break;
+                        case TYPE_LSM303DLM:
+                                pn = n3;
-                 else pn = n2;
+                                break;
+                }
                 sprintf(msg, " %s ID 0x%02x/%02x/%02x-%02x", pn, NCMAG_Identification.A, NCMAG_Identification.B, NCMAG_Identification.C,NCMAG_Identification2.Sub);
                          && (NCMAG_Identification.C == MAG_IDC))
+                {
                         NCMAG_Present = 1;
-                        if(EEPROM_Init())
-                                {
-                                        NCMAG_IsCalibrated = NCMag_CalibrationRead();
-                                        if(!NCMAG_IsCalibrated) UART1_PutString("\r\n Not calibrated!");
-                                }
-                        else UART1_PutString("\r\n EEPROM data not available!!!!!!!!!!!!!!!");
-                        if(NCMAG_Identification2.Sub == 0x00)
+                        if(EEPROM_Init())
+                        {
+                                NCMAG_IsCalibrated = NCMag_CalibrationRead();
+                                if(!NCMAG_IsCalibrated) UART1_PutString("\r\n Not calibrated!");
+                        }
+                        else UART1_PutString("\r\n EEPROM data not available!!!!!!!!!!!!!!!");
-                        {
+                        // perform self test
-                         if(!NCMAG_SelfTest())
+                        if(!NCMAG_SelfTest())
+                        {
                                 UART1_PutString("\r\n Selftest failed!!!!!!!!!!!!!!!!!!!!\r\n");
                                 LED_RED_ON;
+                                NCMAG_IsCalibrated = 0;
-                                NCMAG_IsCalibrated = 0;
+                        }
-                         }      else UART1_PutString("\r\n Selftest ok");
+                        else UART1_PutString("\r\n Selftest ok");
-                        }
+                        // initialize magnetic sensor configuration
-                        else InitNC_MagnetSensor();
+                        InitNC_MagnetSensor();
+                }
                 else
+                {
                         UART1_PutString("\n\r Not compatible!");
                         UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE;
                         LED_RED_ON;
+                }
+        }
         else // nothing found
         {
-                NCMAG_MagType = MAG_TYPE_NONE;
+                NCMAG_SensorType = TYPE_NONE;
                 UART1_PutString("not found!");
+        }
         return(NCMAG_Present);

Subversion Repositories NaviCtrl

(root)/tags/V2.10b/ncmag.c @ 811 - Rev 360 → 394