WebSVN - NaviCtrl - Diff - Rev 242 and 243

 // rx data handler for mag vector request
 void HMC5843_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)
 {       // if number of byte are matching
-        if(RxBufferSize == sizeof(MagVector) )
+        if(RxBufferSize == sizeof(MagVector) )
         {       // byte order from big to little endian
                 MagVector.X = pRxBuffer[0]<<8;
                 MagVector.X+= pRxBuffer[1];
                 MagVector.Y = pRxBuffer[2]<<8;
                 MagVector.Y+= pRxBuffer[3];
         I2C_Transmission(HMC5843_SLAVE_ADDRESS, NULL, 0);
         return 1;
+}
+//----------------------------------------------------------------
+void HMC5843_GetMagVector(void)
+{
+        if(I2C_State == I2C_STATE_IDLE)
+        {
+                I2C_TxBufferSize = 0;
+                // set register pointer
+                I2C_TxBuffer[I2C_TxBufferSize++] = REGISTER_DATAX_MSB;
+                // initiate transmission
+                I2C_Transmission(HMC5843_SLAVE_ADDRESS, &HMC5843_UpdateMagVector, sizeof(MagVector));
+        }
+}
+void HMC5843_UpdateCompass(void)
+{
+        static u32 TimerCompassUpdate = 0;
+        if( (I2C_State == I2C_STATE_OFF) || !HMC5843_Present ) return;
+        if(CheckDelay(TimerCompassUpdate))
+        {
+                // check for incomming compass calibration request
+                // update CalByte from spi input queue
+                /*fifo_get(&CompassCalcStateFiFo, (u8 *)&(MK3MAG_WriteCal.CalByte));
+                // send new calstate
+                if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte)
+                {
+                        do the calibration here
+                }
+                else // request current heading */
+                {
+                        HMC5843_GetMagVector();
+                }
+                TimerCompassUpdate = SetDelay(20);    // every 20 ms are 50 Hz
+        }
+}
+u8 HMC5843_SelfTest(void)
+{
+        u32 time;
+        s32 XMin = 0, XMax = 0, YMin = 0, YMax = 0, ZMin = 0, ZMax = 0;
+        s32 scale, scale_min, scale_max;
+        u8 i = 0;
+        // activate positive bias field  of 0.55 gauss
+        HMC5843_SetConfiguration(HMC5843_CRA_RATE_50HZ|HMC5843_CRA_MODE_POSBIAS, HMC5843_CRB_GAIN_10GA, HMC5843_MODE_CONTINUOUS);
+        // wait a while
+        time = SetDelay(55);
+        while(!CheckDelay(time));
+        // averaging
+        for(i = 0; i<20; i++)
+        {
+                HMC5843_GetMagVector();
+                time = SetDelay(20);
+        while(!CheckDelay(time));
+                XMax += MagVector.X;
+                YMax += MagVector.Y;
+                ZMax += MagVector.Z;
+        }
+        // activate negative bias field of 0.55 gauss
+        HMC5843_SetConfiguration(HMC5843_CRA_RATE_50HZ|HMC5843_CRA_MODE_NEGBIAS, HMC5843_CRB_GAIN_10GA, HMC5843_MODE_CONTINUOUS);
+        // wait a while
+        time = SetDelay(55);
+    while(!CheckDelay(time));
+        // averaging
+        for(i = 0; i < 20; i++)
+        {
+                HMC5843_GetMagVector();
+                time = SetDelay(20);
+        while(!CheckDelay(time));
+                XMin += MagVector.X;
+                YMin += MagVector.Y;
+                ZMin += MagVector.Z;
+        }
+        // setup final configuration
+        HMC5843_SetConfiguration(HMC5843_CRA_RATE_50HZ|HMC5843_CRA_MODE_NORMAL, HMC5843_CRB_GAIN_10GA, HMC5843_MODE_CONTINUOUS);
+        // prepare scale limits
+        scale = 715;  // 1300 counts/Gauss * 0.55 Gauss = 715 counts
+        scale_min = (scale * 90)/100;
+        scale_max = (scale * 110)/100;
+        // check scale for all axes
+        scale = (XMax - XMin)/40;
+        if((scale > scale_max) && (scale < scale_min)) return 0;
+        scale = (YMax + YMin)/40;
+        if((scale > scale_max) && (scale < scale_min)) return 0;
+        scale = (ZMax + ZMin)/40;
+        if((scale > scale_max) && (scale < scale_min)) return 0;
+        return 1;
+}
 //----------------------------------------------------------------
 u8 HMC5843_Init(void)
+{
         u8 msg[64];
         u8 repeat;
                 repeat++;
         }while ((HMC5843_Identification.A == 0xFF) && (repeat < 12)); // 12*250ms=3s
         // if we got it
         if(HMC5843_Identification.A != 0xFF)
+        {
-                sprintf(msg, " HMC5843 ID %d/%d/%d.", HMC5843_Identification.A, HMC5843_Identification.B, HMC5843_Identification.C);
+                sprintf(msg, " HMC5843 ID %d/%d/%d", HMC5843_Identification.A, HMC5843_Identification.B, HMC5843_Identification.C);
                 UART1_PutString(msg);
                 if (    (HMC5843_Identification.A == HMC5843_IDA)
                      && (HMC5843_Identification.B == HMC5843_IDB)
                          && (HMC5843_Identification.C == HMC5843_IDC))
                 {
+                        //HMC5843_SetConfiguration(HMC5843_CRA_RATE_50HZ|HMC5843_CRA_MODE_NORMAL, HMC5843_CRB_GAIN_10GA, HMC5843_MODE_CONTINUOUS);
+                        if(!HMC5843_SelfTest())
+                        {
+                                UART1_PutString("\n\r HMC5843 selftest failed!");
+                                LED_RED_ON;
-                        HMC5843_SetConfiguration(HMC5843_CRA_RATE_50HZ|HMC5843_CRA_MODE_NORMAL, HMC5843_CRB_GAIN_10GA, HMC5843_MODE_CONTINUOUS);
+                        }
-                        HMC5843_Present = 1;
+                        else HMC5843_Present = 1;
+                }
                 else
+                {
                         UART1_PutString("\n\r HMC5843 not compatible!");
                         LED_RED_ON;
+                }
+        }
         return(HMC5843_Present);
-}
-//----------------------------------------------------------------
-void HMC5843_GetMagVector(void)
-{
-        if(I2C_State == I2C_STATE_IDLE)
-        {
-                I2C_TxBufferSize = 0;
-                // set register pointer
-                I2C_TxBuffer[I2C_TxBufferSize++] = REGISTER_DATAX_MSB;
-                // initiate transmission
-                I2C_Transmission(HMC5843_SLAVE_ADDRESS, &HMC5843_UpdateMagVector, sizeof(MagVector));
-        }
-}
-void HMC5843_UpdateCompass(void)
-{
-        static u32 TimerCompassUpdate = 0;
-        if( (I2C_State == I2C_STATE_OFF) || !HMC5843_Present ) return;
-        if(CheckDelay(TimerCompassUpdate))
-        {
-                // check for incomming compass calibration request
-                // update CalByte from spi input queue
-                /*fifo_get(&CompassCalcStateFiFo, (u8 *)&(MK3MAG_WriteCal.CalByte));
-                // send new calstate
-                if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte)
-                {
-                        do the calibration here
-                }
-                else // request current heading */
-                {
-                        HMC5843_GetMagVector();
-                }
-                TimerCompassUpdate = SetDelay(20);    // every 20 ms are 50 Hz

Subversion Repositories NaviCtrl

(root)/tags/V0.24e/ncmag.c @ 337 - Rev 242 → 243