WebSVN - NaviCtrl - Diff - Rev 474 and 475

 void NCMAG_Calibrate(void)
+{
         u8 msg[64];
         static u8 speak = 0;
-        static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
+        static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0, Zmin2 = 0, Zmax2 = 0;
         static s16 X = 0, Y = 0, Z = 0;
         static u8 OldCalState = 0;
         s16 MinCalibration = 450;
                         Ymin =  10000;
                         Ymax = -10000;
                         Zmin =  10000;
                         Zmax = -10000;
                         speak = 1;
+                        if(Compass_CalState != OldCalState) // avoid continously writing of eeprom!
+                        {
+                         UART1_PutString("\r\nStarting compass calibration");
+                         if(I2C_CompassPort == I2C_EXTERN_0)
+                         {
+                          if(!ExtCompassOrientation) ExtCompassOrientation = GetExtCompassOrientation();
+                          UART1_PutString(" - External sensor ");
+                          sprintf(msg, "with orientation: %d ",ExtCompassOrientation);
+                          UART1_PutString(msg);
+                         }
+                         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;}
                         else if(X > Xmax)       { Xmax = X; BeepTime = 20;}
                         if(Y < Ymin)            { Ymin = Y; BeepTime = 60;}
                         else if(Y > Ymax)       { Ymax = Y; BeepTime = 60;}
+                        if(Z < Zmin)        { Zmin = Z; } // silent
+                        else if(Z > Zmax)   { Zmax = Z; }
                         if(speak) SpeakHoTT = SPEAK_CALIBRATE; speak = 0;
                         break;
                 case 3: // 3rd step of calibration
                         // used to change the orientation of the MK3MAG vertical to the horizontal plane
                         speak = 1;
                         break;
                 case 4:
                         // find Min and Max of the Z-Sensor
+                        if(Z < Zmin2)     { Zmin2 = Z; BeepTime = 80;}
+                        else if(Z > Zmax2) { Zmax2 = Z; BeepTime = 80;}
+                        if(X < Xmin)            { Xmin = X; BeepTime = 20;}
+                        else if(X > Xmax)       { Xmax = X; BeepTime = 20;}
+                        if(Y < Ymin)            { Ymin = Y; BeepTime = 60;}
-                        if(Z < Zmin)      { Zmin = Z; BeepTime = 80;}
+                        else if(Y > Ymax)       { Ymax = Y; BeepTime = 60;}
                         else if(Z > Zmax) { Zmax = Z; BeepTime = 80;}
                         if(speak) SpeakHoTT = SPEAK_CALIBRATE; speak = 0;
                         break;
                 case 5:
                         // Save values
                         if(Compass_CalState != OldCalState) // avoid continously writing of eeprom!
+                        {
                                 switch(NCMAG_SensorType)
+                                {
                                         case TYPE_HMC5843:
-                                                UART1_PutString("\r\nHMC5843 calibration\n\r");
+                                                UART1_PutString("\r\nFinished: HMC5843 calibration\n\r");
                                                 MinCalibration = HMC5843_CALIBRATION_RANGE;
                                                 break;
                                         case TYPE_LSM303DLH:
                                         case TYPE_LSM303DLM:
-                                                UART1_PutString("\r\n\r\nLSM303 calibration\n\r");
+                                                UART1_PutString("\r\nFinished: LSM303 calibration\n\r");
                                                 MinCalibration = LSM303_CALIBRATION_RANGE;
                                   sprintf(msg, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic);
                                   UART1_PutString(msg);
+                                 }
                             else UART1_PutString("without GPS\n\r");
+                                if(Zmin2 < Zmin)          { Zmin = Zmin2; }
+                                else if(Zmax2 > Zmax) { Zmax = Zmax2; }
                                 Calibration.MagX.Range = Xmax - Xmin;
                                 Calibration.MagX.Offset = (Xmin + Xmax) / 2;
                                 Calibration.MagY.Range = Ymax - Ymin;
                                 Calibration.MagY.Offset = (Ymin + Ymax) / 2;
+}
 u8 GetExtCompassOrientation(void)
+{
-        if(I2C_CompassPort != I2C_EXTERN_0) return(0);
-        if(abs(FromFlightCtrl.AngleNick) > 300) return(0); // MK tilted
+        if(I2C_CompassPort != I2C_EXTERN_0) return(0);
+        if((abs(FromFlightCtrl.AngleNick) > 300) || (abs(FromFlightCtrl.AngleRoll) > 300))
+         {
+//        UART1_PutString("\r\nTilted");
-        if(abs(FromFlightCtrl.AngleRoll) > 300) return(0);
+          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
          for(repeat = 0; repeat < 100; repeat++)
+          {
            NCMAG_GetAccVector(10); // only the sensor with ACC is supported
        ExtCompassOrientation = GetExtCompassOrientation();
+           if(ExtCompassOrientation && (ExtCompassOrientation == Calibration.Version / 16)) break;
-           if(ExtCompassOrientation) break;
+           //UART1_Putchar('-');
+          }
 //DebugOut.Analog[19] = repeat;
           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);
+            sprintf(msg, "\n\r! Warning: calibrated orientation was %d !",Calibration.Version / 16);
             UART1_PutString(msg);
+           }
           else UART1_PutString("ok");
+         }

Subversion Repositories NaviCtrl

(root)/tags/V2.06i/ncmag.c @ 895 - Rev 474 → 475