WebSVN - NaviCtrl - Diff - Rev 475 and 480

                           if(!ExtCompassOrientation) ExtCompassOrientation = GetExtCompassOrientation();
                           UART1_PutString(" - External sensor ");
                           sprintf(msg, "with orientation: %d ",ExtCompassOrientation);
                           UART1_PutString(msg);
+                         }
-                         else UART1_PutString(" - Internal sensor");
+                         else UART1_PutString(" - Internal sensor ");
+                        }
                         break;
                 case 2: // 2nd step of calibration
                         // find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane
                         if(X < Xmin)            { Xmin = X; BeepTime = 20;}
 // ----------------------------------------------------------------------------------------
 void NCMAG_GetMagVector(void)
+{
         // try to catch the I2C buffer within 0 ms
-        if(I2C_LockBufferFunc(0))
+        if(I2C_LockBufferFunc(5))
+        {
                 u16 TxBytes = 0;
                 // set register pointer
                 I2C_BufferPnt[TxBytes++] = REG_MAG_DATAX_MSB;
         AccConfig.ctrl_4 = ACC_CTRL4_BDU | ACC_CTRL4_FS_8G;
         AccConfig.ctrl_5 = ACC_CTRL5_STW_OFF;
         return(NCMAG_SetAccConfig());
+}
 // --------------------------------------------------------
-void NCMAG_Update(void)
+void NCMAG_Update(u8 init)
+{
         static u32 TimerUpdate = 0;
         static s8 send_config = 0;
         u32 delay = 20;
+        if(init) TimerUpdate = SetDelay(10);
         // todo State Handling for both busses  !!
-        if((I2C1_State == I2C_STATE_OFF) || (I2C_CompassPort == 0 && I2C0_State == I2C_STATE_OFF) || !NCMAG_Present )
+        if((I2C1_State == I2C_STATE_OFF) || (I2C_CompassPort == 0 && I2C0_State == I2C_STATE_OFF)/* || !NCMAG_Present*/ )
+        {
                 Compass_Heading = -1;
                 DebugOut.Analog[14]++; // count I2C error
+                TimerUpdate = SetDelay(10);
                 return;
+        }
-        if(CheckDelay(TimerUpdate))
+        if(CheckDelay(TimerUpdate))// && I2C0_State == I2C_STATE_IDLE && I2C1_State == I2C_STATE_IDLE)
+        {
                 if(Compass_Heading != -1) send_config = 0; // no re-configuration if value is valid
         if(++send_config == 25)   // 500ms
+                {
                         send_config = -25;    // next try after 1 second
                                         delay = 20;
                                         break;
                                 case TYPE_LSM303DLH:
                                 case TYPE_LSM303DLM:
                                         delay = 20;
+//delay = 2;
                                         if(s-- || (I2C_CompassPort == I2C_INTERN_1)) NCMAG_GetMagVector();
                                         else
+                                         {
                                           if(AccRawVector.X + AccRawVector.Y + AccRawVector.Z == 0) NCMAG_Init_ACCSensor();
-                                          NCMAG_GetAccVector(0);
+                                          NCMAG_GetAccVector(5);
                                           delay = 10;
                                           s = 40;  // about 0,8 sec
                                          };
                                         if(!s) delay = 10; // ACC-Reading in the next step after 10ms
+//if(!s) delay = 2; // ACC-Reading in the next step after 10ms
                                         break;
+                        }
                         if(send_config == 24) TimerUpdate = SetDelay(15);    // next event is the re-configuration
                         else TimerUpdate = SetDelay(delay);    // every 20 ms are 50 Hz
+                }
 //----------------------------------------------------------------
 u8 NCMAG_Init(void)
+{
         u8 msg[64];
         u8 retval = 0;
-        u8 repeat;
+        u8 repeat = 0;
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Search external sensor
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         I2C_CompassPort = I2C_EXTERN_0;
         NCMAG_SelectI2CBus(I2C_CompassPort);
-        // get id bytes
+        // get id bytes
         retval = 0;
-        do
+    for(repeat = 0; repeat < 5; repeat++)
+        {
 //              retval = NCMAG_GetIdentification();
                 retval = NCMAG_GetAccConfig();            // only the sensor with ACC is supported
                 if(retval) break; // break loop on success
-//              UART1_PutString("+");
+                UART1_PutString("_");
-                repeat++;
         } while(repeat < 5);
 //retval = 1;
         if(!retval)
+        {
          UART1_PutString(" internal sensor");
           if(ExtCompassOrientation != Calibration.Version / 16)
+           {
             sprintf(msg, "\n\r! Warning: calibrated orientation was %d !",Calibration.Version / 16);
             UART1_PutString(msg);
+           }
-          else UART1_PutString("ok");
+          else UART1_PutString("ok ");
+         }
+        }
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         NCMAG_Present = 0;
         NCMAG_SensorType = TYPE_HMC5843;        // assuming having an HMC5843
-        // polling for LSM302DLH/DLM option by ACC address ack
-        repeat = 0;
+        // polling for LSM302DLH/DLM option by ACC address ack
-        do
+    for(repeat = 0; repeat < 3; repeat++)
+        {
-                retval = NCMAG_GetAccConfig();
-                if(retval) break; // break loop on success
+                retval = NCMAG_GetAccConfig();
-//              UART1_PutString("*");
-                repeat++;
+                if(retval) break; // break loop on success
         } while(repeat < 3);
         if(retval)
+        {
                 // initialize ACC sensor
                 NCMAG_Init_ACCSensor();
-                NCMAG_SensorType = TYPE_LSM303DLH;
-                // polling of sub identification
+                NCMAG_SensorType = TYPE_LSM303DLH;
-                repeat = 0;
+                // polling of sub identification
-                do
+            for(repeat = 0; repeat < 12; repeat++)
-                {
-                        retval = NCMAG_GetIdentification_Sub();
+                {
-                        if(retval) break; // break loop on success
-//                      UART1_PutString(".");
+                        retval = NCMAG_GetIdentification_Sub();
-                        repeat++;
+                        if(retval) break; // break loop on success
                 }while(repeat < 12);
                 if(retval)
+                {
                         if(NCMAG_Identification2.Sub == MAG_IDF_LSM303DLM)      NCMAG_SensorType = TYPE_LSM303DLM;
                 }
-        }
         // get id bytes
-        retval = 0;
+        // get id bytes
-        do
+    for(repeat = 0; repeat < 3; repeat++)
-        {
-                retval = NCMAG_GetIdentification();
+        {
-                if(retval) break; // break loop on success
-//              UART1_PutString("#");
+                retval = NCMAG_GetIdentification();
-                repeat++;
+                if(retval) break; // break loop on success
         } while(repeat < 12);

Subversion Repositories NaviCtrl

(root)/tags/V2.02a/ncmag.c @ 481 - Rev 475 → 480