WebSVN - NaviCtrl - Diff - Rev 488 and 489

 // + www.MikroKopter.com
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Software Nutzungsbedingungen (english version: see below)
 // + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
 // + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
 // + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
 // + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
 // + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
 // + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
 // + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
 // + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
 // + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
 // + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
 // + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
 // + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
 // + des Mitverschuldens offen.
 // + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
 // + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
 // + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
 // + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
 // +  Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Software LICENSING TERMS
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
 // + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware
 // + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
 // + The Software may only be used with the Licensor's products.
 // + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
 // + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
 // + agreement shall be the property of the Licensor.
 // + #### END OF LICENSING TERMS ####
 // + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #include <math.h>
 #include <stdio.h>
+#include <stdlib.h>
 #include <string.h>
 #include "91x_lib.h"
 #include "ncmag.h"
-#include "i2c1.h"
-#include "i2c0.h"
+#include "i2c.h"
 #include "timer1.h"
 #include "led.h"
 #include "uart1.h"
 #include "eeprom.h"
 #include "mymath.h"
 #include "spi_slave.h"
 u8 NCMAG_Present = 0;
-u8 NCMAG_IsCalibrated = 0;
-u8 I2C_CompassPort = 1;
-u8 ExtCompassOrientation = 0;
-u8 *I2C_BufferPnt;
-u8 *I2C_ErrorPnt;
-I2C_TransmissionFunc_t                          I2C_TransmissionFunc;
-I2C_LockBufferFunc_t                            I2C_LockBufferFunc;
-I2C_WaitForEndOfTransmissionFunc_t      I2C_WaitForEndOfTransmissionFunc;
+u8 NCMAG_IsCalibrated = 0;
 // supported magnetic sensor types
 #define TYPE_NONE                       0
 #define TYPE_HMC5843            1
+#define TYPE_LSM303DLH          2
+#define TYPE_LSM303DLM          3
+u8 NCMAG_SensorType = TYPE_NONE;
+u8 NCMAG_Orientation = 0; // 0 means unknown!
+// two calibrtion sets for extern and intern sensor
+#define EEPROM_ADR_MAG_CALIBRATION_INTERN       50
+#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;
-#define TYPE_LSM303DLH          2
+        Scaling_t MagZ;
-#define TYPE_LSM303DLM          3
+        u8 Version;
         u8 crc;
-u8 NCMAG_SensorType = TYPE_NONE;
+} __attribute__((packed)) Calibration_t;
 #define HMC5843_CRA_RATE_20HZ           0x14
 #define HMC5843_CRA_RATE_50HZ           0x18
 // bit mask for gain
 #define HMC5843_CRB_GAIN_07GA           0x00
 #define HMC5843_CRB_GAIN_10GA           0x20    //default
 #define HMC5843_CRB_GAIN_15GA           0x40    // <--- we use this
 #define HMC5843_CRB_GAIN_20GA           0x60
 #define HMC5843_CRB_GAIN_32GA           0x80
 #define HMC5843_CRB_GAIN_38GA           0xA0
 #define HMC5843_CRB_GAIN_45GA           0xC0
 #define HMC5843_CRB_GAIN_65GA           0xE0
         u8 mode;
 } __attribute__((packed)) MagConfig_t;
-volatile MagConfig_t MagConfig;
 volatile MagConfig_t MagConfig;
 // self test value
 #define LSM303DLH_TEST_XSCALE   495
 #define LSM303DLH_TEST_YSCALE   495
 // 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
         u8 ctrl_5;
 } __attribute__((packed)) AccConfig_t;
 volatile AccConfig_t AccConfig;
 // write calibration data for external and internal sensor seperately
-u8 NCMag_CalibrationWrite(u8 intern) // two calibrtion sets for extern and intern sensor
+u8 NCMag_CalibrationWrite(I2C_TypeDef* I2Cx)
 {
         u16 address;
-        u8 i, crc = MAG_CALIBRATION_COMPATIBLE;
+        u8 i = 0, crc = MAG_CALIBRATION_COMPATIBLE;
-        EEPROM_Result_t eres;
-        u8 *pBuff = (u8*)&Calibration;
+        EEPROM_Result_t eres;
-        Calibration.Version = CALIBRATION_VERSION;
         u8 *pBuff = (u8*)&Calibration;
-        if(intern == I2C_INTERN_1) address = EEPROM_ADR_MAG_CALIBRATION_INTERN;
-        else
+        if (I2Cx == NCMAG_PORT_EXTERN)
+        {
+                address = EEPROM_ADR_MAG_CALIBRATION_EXTERN;
+                Calibration.Version = CALIBRATION_VERSION + (NCMAG_Orientation<<4);;
+        }
+        else if (I2Cx == NCMAG_PORT_INTERN)
+        {
+                address = EEPROM_ADR_MAG_CALIBRATION_INTERN;
+                Calibration.Version = CALIBRATION_VERSION;
          address = EEPROM_ADR_MAG_CALIBRATION_EXTERN;
-         Calibration.Version = CALIBRATION_VERSION + ExtCompassOrientation * 16;
+        else return(i);
-        }
         for(i = 0; i<(sizeof(Calibration)-1); i++)
+        {
                 crc += pBuff[i];
+        }
         Calibration.crc = ~crc;
         eres = EEPROM_WriteBlock(address, pBuff, sizeof(Calibration));
         if(EEPROM_SUCCESS == eres) i = 1;
         else i = 0;
+        return(i);
         return(i);
-}
+// read calibration data for external and internal sensor seperately
 u8 NCMag_CalibrationRead(I2C_TypeDef* I2Cx)
-u8 NCMag_CalibrationRead(u8 intern)     // two calibrtion sets for extern and intern sensor
 {
-        u8 i, crc = MAG_CALIBRATION_COMPATIBLE;
+        u8 address;
+        u8 i = 0, crc = MAG_CALIBRATION_COMPATIBLE;
         u8 *pBuff = (u8*)&Calibration;
         u16 address;
         if (I2Cx == NCMAG_PORT_EXTERN)          address = EEPROM_ADR_MAG_CALIBRATION_EXTERN;
-        if(intern == I2C_INTERN_1) address = EEPROM_ADR_MAG_CALIBRATION_INTERN;
+        else if (I2Cx == NCMAG_PORT_INTERN) address = EEPROM_ADR_MAG_CALIBRATION_INTERN;
-        else address = EEPROM_ADR_MAG_CALIBRATION_EXTERN;
+        else return(0);
         if(EEPROM_SUCCESS == EEPROM_ReadBlock(address, pBuff, sizeof(Calibration)))
+        {
                 for(i = 0; i<(sizeof(Calibration)-1); i++)
+                {
                         crc += pBuff[i];
+                }
                 crc = ~crc;
                 if(Calibration.crc != crc) return(0); // crc mismatch
-                if((Calibration.Version & 0x0f) == CALIBRATION_VERSION) return(1);
+                if((Calibration.Version & 0x0F) == CALIBRATION_VERSION) return(1);
+        }
         return(0);
+}
 void NCMAG_Calibrate(void)
+{
         u8 msg[64];
                         Xmax = -10000;
                         Ymin =  10000;
                         Ymax = -10000;
                         Zmin =  10000;
                         Zmax = -10000;
                         speak = 1;
                         CompassValueErrorCount = 0;
-                        if(Compass_CalState != OldCalState) // avoid continously writing of eeprom!
+                        if(Compass_CalState != OldCalState) // only once per state
+                        {
-                         UART1_PutString("\r\nStarting compass calibration");
+                                UART1_PutString("\r\nStarting compass calibration");
-                         if(I2C_CompassPort == I2C_EXTERN_0)
+                                if(Compass_I2CPort == NCMAG_PORT_EXTERN)
-                         {
+                                {
-                          if(!ExtCompassOrientation) ExtCompassOrientation = GetExtCompassOrientation();
+                                        if(!NCMAG_Orientation) NCMAG_Orientation = NCMAG_GetOrientationFromAcc();
-                          UART1_PutString(" - External sensor ");
+                                        UART1_PutString(" - External sensor ");
-                          sprintf(msg, "with orientation: %d ",ExtCompassOrientation);
+                                        sprintf(msg, "with orientation: %d ", NCMAG_Orientation);
-                          UART1_PutString(msg);
+                                        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;}
                         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(Z < Zmin2)           { Zmin2 = Z; BeepTime = 80;}
+                        else if(Z > Zmax2)      { Zmax2 = Z; BeepTime = 80;}
-                        if(X < Xmin)            { Xmin = X; BeepTime = 20;}
+                        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(Y < Ymin)            { Ymin = Y; BeepTime = 60;}
                         else if(Y > Ymax)       { Ymax = Y; BeepTime = 60;}
                         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\nFinished: HMC5843 calibration\n\r");
                                                 MinCalibration = HMC5843_CALIBRATION_RANGE;
                                                 break;
                                         case TYPE_LSM303DLH:
                                         case TYPE_LSM303DLM:
                                                 UART1_PutString("\r\nFinished: LSM303 calibration\n\r");
                                                 MinCalibration = LSM303_CALIBRATION_RANGE;
-                                        break;
+                                                break;
                                 }
                                 if(EarthMagneticStrengthTheoretic)
-                                 {
+                                {
-                                  MinCalibration = (MinCalibration * EarthMagneticStrengthTheoretic) / 50;
+                                        MinCalibration = (MinCalibration * EarthMagneticStrengthTheoretic) / 50;
-                                  sprintf(msg, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic);
+                                        sprintf(msg, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic);
-                                  UART1_PutString(msg);
+                                        UART1_PutString(msg);
-                                 }
+                                }
-                            else UART1_PutString("without GPS\n\r");
                             else UART1_PutString("without GPS\n\r");
                                 if(Zmin2 < Zmin)          { Zmin = Zmin2; }
-                                else if(Zmax2 > Zmax) { Zmax = Zmax2; }
+                                if(Zmin2 < Zmin) Zmin = Zmin2;
                                 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;
                                 Calibration.MagZ.Range = Zmax - Zmin;
                                 Calibration.MagZ.Offset = (Zmin + Zmax) / 2;
                                 if(CompassValueErrorCount)
+                                {
                                         SpeakHoTT = SPEAK_ERR_CALIBARTION;
                                         UART1_PutString("\r\nCalibration FAILED - Compass sensor error !!!!\r\n ");
+                                }
                                 else
                                 if((Calibration.MagX.Range > MinCalibration) && (Calibration.MagY.Range > MinCalibration) && (Calibration.MagZ.Range > MinCalibration))
+                                {
-                                        NCMAG_IsCalibrated = NCMag_CalibrationWrite(I2C_CompassPort);
+                                        NCMAG_IsCalibrated = NCMag_CalibrationWrite(Compass_I2CPort);
                                         BeepTime = 2500;
                                         UART1_PutString("\r\n-> Calibration okay <-\n\r");
-SpeakHoTT = SPEAK_MIKROKOPTER;
+                                        SpeakHoTT = SPEAK_MIKROKOPTER;
+                                }
                                 else
+                                {
-SpeakHoTT = SPEAK_ERR_CALIBARTION;
+                                        SpeakHoTT = SPEAK_ERR_CALIBARTION;
                                         UART1_PutString("\r\nCalibration FAILED - Values too low: ");
                                     if(Calibration.MagX.Range < MinCalibration) UART1_PutString("X! ");
                                     if(Calibration.MagY.Range < MinCalibration) UART1_PutString("Y! ");
                                     if(Calibration.MagZ.Range < MinCalibration) UART1_PutString("Z! ");
                                         UART1_PutString("\r\n");
                                         // restore old calibration data from eeprom
                                         UART1_PutString(msg);
                                         sprintf(msg, "(Minimum ampilitude is: %i)\r\n",MinCalibration);
                                         UART1_PutString(msg);
+                        }
                         break;
                 default:
                         break;
+        }
         OldCalState = Compass_CalState;
+}
         if(RxBufferSize == sizeof(MagRawVector) )
         {       // byte order from big to little endian
                 s16 raw, X = 0, Y = 0, Z = 0;
                 raw = pRxBuffer[0]<<8;
                 raw+= pRxBuffer[1];
                 if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) X = raw;
                 else if(CompassValueErrorCount < 35) CompassValueErrorCount++; // invalid data
                 raw = pRxBuffer[2]<<8;
                 raw+= pRxBuffer[3];
             if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE)
+                {
                         if(NCMAG_SensorType == TYPE_LSM303DLM)  Z = raw; // here Z and Y are exchanged
                         else                                                                    Y = raw;
+                }
                 else if(CompassValueErrorCount < 35) CompassValueErrorCount++; // invalid data
                 raw = pRxBuffer[4]<<8;
                 raw+= pRxBuffer[5];
                 if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE)
                 {
                         if(NCMAG_SensorType == TYPE_LSM303DLM)  Y = raw; // here Z and Y are exchanged
                         else                                                                    Z = raw;
+                }
-                }
+                else if(CompassValueErrorCount < 35) CompassValueErrorCount++; // invalid data
                 else if(CompassValueErrorCount < 35) CompassValueErrorCount++; // invalid data
+                // correct compass orientation
-          switch(ExtCompassOrientation)
+                switch(NCMAG_Orientation)
                 {
+                        case 0:
-                 case 0:
+                        case 1:
-                 case 1:
+                        default:
-                 default:
+                                // 1:1 Mapping
                                 MagRawVector.X = X;
                                 MagRawVector.Y = Y;
                                 MagRawVector.Z = Z;
                                 break;
-                 case 2:
+                        case 2:
                                 MagRawVector.X = -X;
                                 MagRawVector.Y = Y;
                                 MagRawVector.Z = -Z;
                                 break;
-                 case 3:
+                        case 3:
                                 MagRawVector.X = -Z;
                                 MagRawVector.Y = Y;
                                 MagRawVector.Z = X;
                                 break;
-                 case 4:
+                        case 4:
                                 MagRawVector.X = Z;
                                 MagRawVector.Y = Y;
                                 MagRawVector.Z = -X;
                                 break;
-                 case 5:
+                        case 5:
                                 MagRawVector.X = X;
                                 MagRawVector.Y = -Z;
                                 MagRawVector.Z = Y;
                                 break;
-                 case 6:
+                        case 6:
                                 MagRawVector.X = -X;
                                 MagRawVector.Y = -Z;
                 Compass_CalcHeading();
+        }
+}
 // rx data handler  for acceleration raw data
 void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize)
-{       // if number of byte are matching
+{       // if number of bytes are matching
         if(RxBufferSize == sizeof(AccRawVector) )
         {
+                // copy from I2C buffer
                 memcpy((u8*)&AccRawVector, pRxBuffer,sizeof(AccRawVector));
+                // scale and update Acc Vector, at the moment simply 1:1
+                memcpy((u8*)&AccVector, (u8*)&AccRawVector,sizeof(AccRawVector));
+        }
+}
-u8 GetExtCompassOrientation(void)
-{
-        if(I2C_CompassPort != I2C_EXTERN_0) return(0);
-        if((abs(FromFlightCtrl.AngleNick) > 300) || (abs(FromFlightCtrl.AngleRoll) > 300))
-         {
-//        UART1_PutString("\r\nTilted");
-          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
-        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
-        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) )
+        {
                 memcpy((u8*)&AccConfig, pRxBuffer, sizeof(AccConfig));
+        }
+}
 //----------------------------------------------------------------------
+u8 NCMAG_GetOrientationFromAcc(void)
+{
+        // only if external compass connected
+        if(Compass_I2CPort != NCMAG_PORT_EXTERN) return(0);
+         // MK must not be tilted
+        if((abs(FromFlightCtrl.AngleNick) > 300) || (abs(FromFlightCtrl.AngleRoll) > 300))
+        {
+//              UART1_PutString("\r\nTilted");
+                return(0);
+        }
+        // select orientation
+        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
+        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
+        return(0);
+}
 // ---------------------------------------------------------------------
 u8 NCMAG_SetMagConfig(void)
+{
         u8 retval = 0;
-        u8 retval = 0;
         // try to catch the i2c buffer within 100 ms timeout
-        if(I2C_LockBufferFunc(100))
+        if(I2CBus_LockBuffer(Compass_I2CPort, 100))
+        {
+                u8 TxBytes = 0;
-        {
+                u8 TxData[sizeof(MagConfig) + 3];
-                u8 TxBytes = 0;
-                I2C_BufferPnt[TxBytes++] = REG_MAG_CRA;
+                TxData[TxBytes++] = REG_MAG_CRA;
-                memcpy((u8*)(&I2C_BufferPnt[TxBytes]), (u8*)&MagConfig, sizeof(MagConfig));
+                memcpy(&TxData[TxBytes], (u8*)&MagConfig, sizeof(MagConfig));
                 TxBytes += sizeof(MagConfig);
-                if(I2C_TransmissionFunc(MAG_SLAVE_ADDRESS, TxBytes, 0, 0))
+                if(I2CBus_Transmission(Compass_I2CPort, MAG_SLAVE_ADDRESS, TxData, TxBytes, 0, 0))
+                {
-                        if(I2C_WaitForEndOfTransmissionFunc(100))
+                        if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 100))
+                        {
-                                if(*I2C_ErrorPnt == I2C_ERROR_NONE) retval = 1;
+                                if(I2CBus(Compass_I2CPort)->Error == I2C_ERROR_NONE) retval = 1;
+                        }
                 }
+        }
         return(retval);
+}
 // ----------------------------------------------------------------------------------------
 u8 NCMAG_GetMagConfig(void)
 {
         u8 retval = 0;
         // try to catch the i2c buffer within 100 ms timeout
+        if(I2CBus_LockBuffer(Compass_I2CPort, 100))
-        if(I2C_LockBufferFunc(100))
+        {
-        {
+                u8 TxBytes = 0;
-                u8 TxBytes = 0;
+                u8 TxData[3];
-                I2C_BufferPnt[TxBytes++] = REG_MAG_CRA;
+                TxData[TxBytes++] = REG_MAG_CRA;
-                if(I2C_TransmissionFunc(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagConfig, sizeof(MagConfig)))
+                if(I2CBus_Transmission(Compass_I2CPort, MAG_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateMagConfig, sizeof(MagConfig)))
                 {
-                        if(I2C_WaitForEndOfTransmissionFunc(100))
+                        if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 100))
+                        {
-                                if(*I2C_ErrorPnt == I2C_ERROR_NONE) retval = 1;
+                                if(I2CBus(Compass_I2CPort)->Error == I2C_ERROR_NONE) retval = 1;
                         }
+                }
+        }
         return(retval);
+}
 // ----------------------------------------------------------------------------------------
 u8 NCMAG_SetAccConfig(void)
+{
         u8 retval = 0;
+        // try to catch the i2c buffer within 50 ms timeout
-        // try to catch the i2c buffer within 100 ms timeout
+        if(I2CBus_LockBuffer(Compass_I2CPort, 50))
-        if(I2C_LockBufferFunc(50))
+        {
                 u8 TxBytes = 0;
-                u8 TxBytes = 0;
+                u8 TxData[sizeof(AccConfig) + 3];
-                I2C_BufferPnt[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT;
+                TxData[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT;
-                memcpy((u8*)(&I2C_BufferPnt[TxBytes]), (u8*)&AccConfig, sizeof(AccConfig));
+                memcpy(&TxData[TxBytes], (u8*)&AccConfig, sizeof(AccConfig));
                 TxBytes += sizeof(AccConfig);
-                if(I2C_TransmissionFunc(ACC_SLAVE_ADDRESS, TxBytes, 0, 0))
+                if(I2CBus_Transmission(Compass_I2CPort, ACC_SLAVE_ADDRESS, TxData, TxBytes, 0, 0))
+                {
-                        if(I2C_WaitForEndOfTransmissionFunc(50))
+                        if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 50))
+                        {
-                                if(*I2C_ErrorPnt == I2C_ERROR_NONE) retval = 1;
+                                if(I2CBus(Compass_I2CPort)->Error == I2C_ERROR_NONE) retval = 1;
+                        }
+                }
+        }
         return(retval);
+}
 // ----------------------------------------------------------------------------------------
 u8 NCMAG_GetAccConfig(void)
+{
+        u8 retval = 0;
-        u8 retval = 0;
+        // try to catch the i2c buffer within 100 ms timeout
-        // try to catch the i2c buffer within 100 ms timeout
+        if(I2CBus_LockBuffer(Compass_I2CPort, 100))
         if(I2C_LockBufferFunc(100))
-        {
+                u8 TxBytes = 0;
-                u8 TxBytes = 0;
+                u8 TxData[3];
-                I2C_BufferPnt[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT;
+                TxData[TxBytes++] = REG_ACC_CTRL1|REG_ACC_MASK_AUTOINCREMENT;
-                if(I2C_TransmissionFunc(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccConfig, sizeof(AccConfig)))
+                if(I2CBus_Transmission(Compass_I2CPort, ACC_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateAccConfig, sizeof(AccConfig)))
+                {
-                        if(I2C_WaitForEndOfTransmissionFunc(100))
+                        if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 100))
                         {
-                                if(*I2C_ErrorPnt == I2C_ERROR_NONE) retval = 1;
+                                if(I2CBus(Compass_I2CPort)->Error == I2C_ERROR_NONE) retval = 1;
+                        }
+                }
+        }
         return(retval);
+}
 // ----------------------------------------------------------------------------------------
 u8 NCMAG_GetIdentification(void)
+{
         u8 retval = 0;
-        u8 retval = 0;
+        // try to catch the i2c buffer within 100 ms timeout
-        // try to catch the i2c buffer within 100 ms timeout
+        if(I2CBus_LockBuffer(Compass_I2CPort, 100))
-        if(I2C_LockBufferFunc(100))
+        {
                 u8 TxBytes = 0;
-                u16 TxBytes = 0;
+                u8 TxData[3];
                 NCMAG_Identification.A = 0xFF;
                 NCMAG_Identification.B = 0xFF;
                 NCMAG_Identification.C = 0xFF;
-                I2C_BufferPnt[TxBytes++] = REG_MAG_IDA;
+                TxData[TxBytes++] = REG_MAG_IDA;
                 // initiate transmission
-                if(I2C_TransmissionFunc(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateIdentification, sizeof(NCMAG_Identification)))
+                if(I2CBus_Transmission(Compass_I2CPort, MAG_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateIdentification, sizeof(NCMAG_Identification)))
+                {
-                        if(I2C_WaitForEndOfTransmissionFunc(100))
+                        if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 100))
+                        {
-                                if(*I2C_ErrorPnt == I2C_ERROR_NONE) retval = 1;
+                                if(I2CBus(Compass_I2CPort)->Error == I2C_ERROR_NONE) retval = 1;
+                        }
+                }
+        }
         return(retval);
+}
+u8 NCMAG_GetIdentification_Sub(void)
 u8 NCMAG_GetIdentification_Sub(void)
-{
+        u8 retval = 0;
-        u8 retval = 0;
+        // try to catch the i2c buffer within 100 ms timeout
-        // try to catch the i2c buffer within 100 ms timeout
+        if(I2CBus_LockBuffer(Compass_I2CPort, 100))
         if(I2C_LockBufferFunc(100))
-        {
+                u8 TxBytes = 0;
-                u16 TxBytes = 0;
+                u8 TxData[3];
                 NCMAG_Identification2.Sub = 0xFF;
-                I2C_BufferPnt[TxBytes++] = REG_MAG_IDF;
+                TxData[TxBytes++] = REG_MAG_IDF;
                 // initiate transmission
-                if(I2C_TransmissionFunc(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateIdentification_Sub, sizeof(NCMAG_Identification2)))
+                if(I2CBus_Transmission(Compass_I2CPort, MAG_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateIdentification_Sub, sizeof(NCMAG_Identification2)))
+                {
-                        if(I2C_WaitForEndOfTransmissionFunc(100))
+                        if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 100))
+                        {
-                                if(*I2C_ErrorPnt == I2C_ERROR_NONE) retval = 1;
+                                if(I2CBus(Compass_I2CPort)->Error == I2C_ERROR_NONE) retval = 1;
+                        }
                 }
         }
         return(retval);
 }
+// ----------------------------------------------------------------------------------------
-// ----------------------------------------------------------------------------------------
+void NCMAG_GetMagVector(u8 timeout)
-void NCMAG_GetMagVector(void)
+{
         // try to catch the I2C buffer within timeout ms
-        // try to catch the I2C buffer within 0 ms
+        if(I2CBus_LockBuffer(Compass_I2CPort, timeout))
         if(I2C_LockBufferFunc(5))
                 u8 TxBytes = 0;
-                u16 TxBytes = 0;
+                u8 TxData[3];
                 // set register pointer
-                I2C_BufferPnt[TxBytes++] = REG_MAG_DATAX_MSB;
+                TxData[TxBytes++] = REG_MAG_DATAX_MSB;
                 // initiate transmission
-                I2C_TransmissionFunc(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagVector, sizeof(MagVector));
+                I2CBus_Transmission(Compass_I2CPort, MAG_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateMagVector, sizeof(MagVector));
+        }
+}
-}
 //----------------------------------------------------------------
-//----------------------------------------------------------------
+void NCMAG_GetAccVector(u8 timeout)
-void NCMAG_GetAccVector(u8 timeout)
-{
-        // try to catch the I2C buffer within 0 ms
         if(I2C_LockBufferFunc(timeout))
         // try to catch the I2C buffer within timeout ms
-                u16 TxBytes = 0;
+        if(I2CBus_LockBuffer(Compass_I2CPort, timeout))
-                // set register pointer
+        {
-                I2C_BufferPnt[TxBytes++] = REG_ACC_X_LSB|REG_ACC_MASK_AUTOINCREMENT;
+                u8 TxBytes = 0;
-                // initiate transmission
+                u8 TxData[3];
-                I2C_TransmissionFunc(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector));
+                // set register pointer
-//DebugOut.Analog[16] = AccRawVector.X;
+                TxData[TxBytes++] = REG_ACC_X_LSB|REG_ACC_MASK_AUTOINCREMENT;
 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( (I2CBus(Compass_I2CPort)->State == I2C_STATE_UNDEF) /*|| !NCMAG_Present*/ )
+        {
                 Compass_Heading = -1;
                 DebugOut.Analog[14]++; // count I2C error
                 TimerUpdate = SetDelay(10);
                 return;
         }
-        if(CheckDelay(TimerUpdate))// && I2C0_State == I2C_STATE_IDLE && I2C1_State == I2C_STATE_IDLE)
+        if(CheckDelay(TimerUpdate))
+        {
                 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
-                InitNC_MagnetSensor();
+                NCMAG_ConfigureSensor();
                         TimerUpdate = SetDelay(20);    // 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)
                         {
+                                case TYPE_HMC5843:
-                                case TYPE_HMC5843:
+                                        delay = 20;      // next cycle after 20 ms
-                                        NCMAG_GetMagVector();
-                                        delay = 20;
+                                        NCMAG_GetMagVector(5);
                                         break;
                                 case TYPE_LSM303DLH:
-                                case TYPE_LSM303DLM:
-                                        delay = 20;
+                                case TYPE_LSM303DLM:
-//delay = 2;
-                                        if(s-- || (I2C_CompassPort == I2C_INTERN_1)) NCMAG_GetMagVector();
+                                        if(s-- || (Compass_I2CPort == NCMAG_PORT_INTERN))
+                                        {
+                                                delay = 20;      // next cycle after 20 ms
-                                        else
+                                                NCMAG_GetMagVector(5);
+                                        }
+                                        else // having an external compass, read every 50th cycle the ACC vec
-                                         {
+                                        {       // try to initialize if no data are there
+                                                if((AccRawVector.X + AccRawVector.Y + AccRawVector.Z) == 0) NCMAG_Init_ACCSensor();
-                                          if(AccRawVector.X + AccRawVector.Y + AccRawVector.Z == 0) NCMAG_Init_ACCSensor();
+                                                // get new data
-                                          NCMAG_GetAccVector(5);
+                                                NCMAG_GetAccVector(5);
-                                          delay = 10;
+                                                delay = 10; // next cycle after 10 ms
-                                          s = 40;  // about 0,8 sec
+                                                s = 40; //reset downconter 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 msg[64];
         static u8 done = 0;
         if(done) return(1);        // just make it once
         #define LIMITS(value, min, max) {min = (80 * value)/100; max = (120 * value)/100;}
         u32 time;
         s32 XMin = 0, XMax = 0, YMin = 0, YMax = 0, ZMin = 0, ZMax = 0;
         s16 xscale, yscale, zscale, scale_min, scale_max;
                         yscale = LSM303DLH_TEST_YSCALE;
                         zscale = LSM303DLH_TEST_ZSCALE;
                         break;
                 case TYPE_LSM303DLM:
                         // does not support self test feature
-                        done = retval;
+                        done = 1;
-                        return(retval);
+                        return(1); // always return success
                         break;
                 default:
         while(!CheckDelay(time));
         // averaging
         #define AVERAGE 20
         for(i = 0; i<AVERAGE; i++)
+        {
-                NCMAG_GetMagVector();
+                NCMAG_GetMagVector(5);
                 time = SetDelay(20);
         while(!CheckDelay(time));
                 XMax += MagRawVector.X;
                 YMax += MagRawVector.Y;
                 ZMax += MagRawVector.Z;
         time = SetDelay(50);
         while(!CheckDelay(time));
         // averaging
         for(i = 0; i < AVERAGE; i++)
+        {
-                NCMAG_GetMagVector();
+                NCMAG_GetMagVector(5);
                 time = SetDelay(20);
         while(!CheckDelay(time));
                 XMin += MagRawVector.X;
                 YMin += MagRawVector.Y;
                 ZMin += MagRawVector.Z;
         NCMAG_SetMagConfig();
         // 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;
         sprintf(msg, "\r\n Value X: %d not %d-%d !", xscale, scale_min,scale_max);
                 UART1_PutString(msg);
+    }
         LIMITS(yscale, scale_min, scale_max);
         yscale = (YMax - YMin)/(2*AVERAGE);
         if((yscale > scale_max) || (yscale < scale_min))
+    {
                 retval = 0;
         sprintf(msg, "\r\n Value Y: %d not %d-%d !", yscale, scale_min,scale_max);
                 UART1_PutString(msg);
+    }
         LIMITS(zscale, scale_min, scale_max);
         zscale = (ZMax - ZMin)/(2*AVERAGE);
         if((zscale > scale_max) || (zscale < scale_min))
+        {
                 retval = 0;
         sprintf(msg, "\r\n Value Z: %d not %d-%d !", zscale, scale_min,scale_max);
                 UART1_PutString(msg);
+    }
         done = retval;
         return(retval);
+}
-//----------------------------------------------------------------
-void NCMAG_SelectI2CBus(u8 busno)
-{
-  if (busno == 0)
-  {
-    I2C_WaitForEndOfTransmissionFunc = &I2C0_WaitForEndOfTransmission;
-        I2C_LockBufferFunc = &I2C0_LockBuffer;
-        I2C_TransmissionFunc = &I2C0_Transmission;
+void NCMAG_CheckOrientation(void)
-        I2C_BufferPnt = I2C0_Buffer;
-        I2C_ErrorPnt = &I2C0_Error;
-  }
+{       // only for external sensor
-   else
+        if(Compass_I2CPort == NCMAG_PORT_EXTERN)
-  {
-    I2C_WaitForEndOfTransmissionFunc = &I2C1_WaitForEndOfTransmission;
+        {
-        I2C_LockBufferFunc = &I2C1_LockBuffer;
+                NCMAG_Orientation = NCMAG_GetOrientationFromAcc();
-        I2C_TransmissionFunc = &I2C1_Transmission;
-        I2C_BufferPnt = I2C1_Buffer;
+                if(NCMAG_Orientation != (Calibration.Version>>4)) NCMAG_IsCalibrated = 0;
-        I2C_ErrorPnt = &I2C1_Error;
+                else NCMAG_IsCalibrated = 1;
-  }
+        }
+}
 //----------------------------------------------------------------
+u8 NCMAG_Init(void)
+{
+        MagRawVector.X = 0;
+    MagRawVector.Y = 0;
+    MagRawVector.Z = 0;
+        AccRawVector.X = 0;
+        AccRawVector.Y = 0;
+        AccRawVector.Z = 0;
+        if(NCMAG_Present) // do only short init ! , full init was called before
+        {
+                // reset I2C Bus
+                I2CBus_Deinit(Compass_I2CPort);
+                I2CBus_Init(Compass_I2CPort);
+                // try to reconfigure senor
+                NCMAG_ConfigureSensor();
+                NCMAG_Update(1);
+        }
-u8 NCMAG_Init(void)
+        else  // full init
-{
+        {
-        u8 msg[64];
+                u8 msg[64];
-        u8 retval = 0;
+                u8 retval = 0;
-        u8 repeat = 0;
+                u8 repeat = 0;
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+                //--------------------------------------------
-// Search external sensor
+                // search external sensor first
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-        I2C_CompassPort = I2C_EXTERN_0;
-        NCMAG_SelectI2CBus(I2C_CompassPort);
+                //--------------------------------------------
+                Compass_I2CPort = NCMAG_PORT_EXTERN;
-        // get id bytes
+                // get id bytes
-        retval = 0;
+                retval = 0;
-    for(repeat = 0; repeat < 5; repeat++)
+                for(repeat = 0; repeat < 5; repeat++)
-        {
+                {
-//              retval = NCMAG_GetIdentification();
+                        //retval = NCMAG_GetIdentification();
-                retval = NCMAG_GetAccConfig();            // only the sensor with ACC is supported
+                        retval = NCMAG_GetAccConfig();            // only the external sensor with ACC is supported
-                if(retval) break; // break loop on success
+                        if(retval) break; // break loop on success
-                UART1_PutString("_");
+                        UART1_PutString("_");
-        }
+                }
-//retval = 1;
+                // Extenal sensor not found?
+                if(!retval)
-        if(!retval)
+                {
-        {
+                        // search internal sensor afterwards
-         UART1_PutString(" internal sensor ");
-         I2C_CompassPort = I2C_INTERN_1;
+                        UART1_PutString(" internal sensor");
-         NCMAG_SelectI2CBus(I2C_CompassPort);
+                        Compass_I2CPort = NCMAG_PORT_INTERN;
-        }
+                }
-        else
+                else
-        {
+                {
+                        UART1_PutString(" external sensor");
+                        Compass_I2CPort = NCMAG_PORT_EXTERN;
-         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 && (ExtCompassOrientation == Calibration.Version / 16)) break;
-           //UART1_Putchar('-');
-          }
-//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! Warning: calibrated orientation was %d !",Calibration.Version / 16);
-            UART1_PutString(msg);
-           }
-          else UART1_PutString("ok ");
-         }
-        }
+                }
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+                //-------------------------------------------
-        NCMAG_Present = 0;
-        NCMAG_SensorType = TYPE_HMC5843;        // assuming having an HMC5843
-        // polling for LSM302DLH/DLM option by ACC address ack
-    for(repeat = 0; repeat < 3; repeat++)
-        {
-                retval = NCMAG_GetAccConfig();
+                NCMAG_Present = 0;
-                if(retval) break; // break loop on success
-        }
-        if(retval)
-        {
-                // initialize ACC sensor
-                NCMAG_Init_ACCSensor();
+                NCMAG_SensorType = TYPE_HMC5843;        // assuming having an HMC5843
-                NCMAG_SensorType = TYPE_LSM303DLH;
+                // polling for LSM302DLH/DLM option by ACC address ack
-                // polling of sub identification
+                repeat = 0;
-            for(repeat = 0; repeat < 12; repeat++)
+                for(repeat = 0; repeat < 3; repeat++)
+                {
-                        retval = NCMAG_GetIdentification_Sub();
+                        retval = NCMAG_GetAccConfig();
                         if(retval) break; // break loop on success
                 }
                 if(retval)
                 {
+                        // initialize ACC sensor
+                        NCMAG_Init_ACCSensor();
-                        if(NCMAG_Identification2.Sub == MAG_IDF_LSM303DLM)      NCMAG_SensorType = TYPE_LSM303DLM;
-                }
+                        NCMAG_SensorType = TYPE_LSM303DLH;
-        }
+                        // polling of sub identification
-        // get id bytes
+                        repeat = 0;
-    for(repeat = 0; repeat < 3; repeat++)
+                        for(repeat = 0; repeat < 12; repeat++)
-        {
+                        {
-                retval = NCMAG_GetIdentification();
-                if(retval) break; // break loop on success
-        }
+                                retval = NCMAG_GetIdentification_Sub();
+                                if(retval) break; // break loop on success
-        // if we got an answer to id request
+                        }
-        if(retval)
+                        if(retval)
+                        {
-        {
+                                if(NCMAG_Identification2.Sub == MAG_IDF_LSM303DLM)      NCMAG_SensorType = TYPE_LSM303DLM;
-                u8 n1[] = "\n\r HMC5843";
+                        }
-                u8 n2[] = "\n\r LSM303DLH";
-                u8 n3[] = "\n\r LSM303DLM";
+                }
-                u8* pn = n1;
+                // get id bytes
-                switch(NCMAG_SensorType)
-                {
-                        case TYPE_HMC5843:
+                retval = 0;
-                                pn = n1;
-                                break;
-                        case TYPE_LSM303DLH:
+                for(repeat = 0; repeat < 3; repeat++)
-                                pn = n2;
-                                break;
                         case TYPE_LSM303DLM:
-                                pn = n3;
-                                break;
-                }
+                        retval = NCMAG_GetIdentification();
+                        if(retval) break; // break loop on success
-                sprintf(msg, " %s ID 0x%02x/%02x/%02x-%02x", pn, NCMAG_Identification.A, NCMAG_Identification.B, NCMAG_Identification.C,NCMAG_Identification2.Sub);
                 UART1_PutString(msg);
+                // if we got an answer to id request
+                if(retval)
-                if (    (NCMAG_Identification.A == MAG_IDA)
+                {
-                     && (NCMAG_Identification.B == MAG_IDB)
+                        u8 n1[] = "\n\r HMC5843";
-                         && (NCMAG_Identification.C == MAG_IDC))
+                        u8 n2[] = "\n\r LSM303DLH";
+                        u8 n3[] = "\n\r LSM303DLM";
+                        u8* pn = n1;
-                {
+                        switch(NCMAG_SensorType)
+                        {
+                                case TYPE_HMC5843:
-                        NCMAG_Present = 1;
+                                        pn = n1;
+                                        break;
+                                case TYPE_LSM303DLH:
                                         pn = n2;
-                        if(EEPROM_Init())
-                        {
-                                NCMAG_IsCalibrated = NCMag_CalibrationRead(I2C_CompassPort);
-                                if(!NCMAG_IsCalibrated) UART1_PutString("\r\n Not calibrated!");
-                        }
-                        else UART1_PutString("\r\n EEPROM data not available!!!!!!!!!!!!!!!");
-                        // perform self test
-                        if(!NCMAG_SelfTest())
-                        {
+                                        break;
+                                case TYPE_LSM303DLM:
+                                        pn = n3;
+                                        break;
+                        }
+                        sprintf(msg, " %s ID 0x%02x/%02x/%02x-%02x", pn, NCMAG_Identification.A, NCMAG_Identification.B, NCMAG_Identification.C,NCMAG_Identification2.Sub);
+                        UART1_PutString(msg);
+                        if (    (NCMAG_Identification.A == MAG_IDA)
+                             && (NCMAG_Identification.B == MAG_IDB)
+                                 && (NCMAG_Identification.C == MAG_IDC))
+                        {
+                                NCMAG_Present = 1;
+                                if(EEPROM_Init())
+                                {
+                                        NCMAG_IsCalibrated = NCMag_CalibrationRead(Compass_I2CPort);
+                                        if(!NCMAG_IsCalibrated) UART1_PutString("\r\n Not calibrated!");
+                                }
+                                else UART1_PutString("\r\n EEPROM data not available!!!!!!!!!!!!!!!");
+                                // in case of an external sensor, try to get the orientation by acc readings
+                                if(Compass_I2CPort == NCMAG_PORT_EXTERN)
+                                {
+                                        // try to get orientation by acc sensor values
+                                        for(repeat = 0; repeat < 100; repeat++)
+                                        {
+                                                NCMAG_GetAccVector(10); // only the sensor with ACC is supported
+                                                NCMAG_Orientation = NCMAG_GetOrientationFromAcc();
+                                                if(NCMAG_Orientation && (NCMAG_Orientation == Calibration.Version >> 4)) break;
+                                        }
+                                        // check orientation result if available
+                                        sprintf(msg, "\r\n Orientation: ");
+                                        UART1_PutString(msg);
+                                        if(NCMAG_Orientation)
+                                        {
+                                                sprintf(msg, "%d ", NCMAG_Orientation);
+                                                UART1_PutString(msg);
+                                                if(NCMAG_IsCalibrated) // check against calibration data orientation
+                                                {
+                                                        if(NCMAG_Orientation != Calibration.Version >> 4)
+                                                        {
+                                                                sprintf(msg, "\n\r Warning: calibrated orientation was %d !",Calibration.Version >> 4);
+                                                                UART1_PutString(msg);
+                                                        }
+                                                }
+                                        }
+                                        else
+                                        {
+                                                UART1_PutString("unknown!");
+                                        }
+                                }
+                                // perform self test
+                                if(!NCMAG_SelfTest())
+                                {
-                                UART1_PutString("\r\n Selftest failed!!!!!!!!!!!!!!!!!!!!\r\n");
+                                        UART1_PutString("\r\n Selftest failed!!!!!!!!!!!!!!!!!!!!\r\n");
-                                LED_RED_ON;
+                                        LED_RED_ON;
+                                        //NCMAG_IsCalibrated = 0;
+                                }
+                                else UART1_PutString("\r\n Selftest ok");
+                                // initialize magnetic sensor configuration
+                                NCMAG_ConfigureSensor();
+                        }
-//                              NCMAG_IsCalibrated = 0;
+                        else
-                        }
+                        {
-                        else UART1_PutString("\r\n Selftest ok");
+                                UART1_PutString("\n\r Not compatible!");
+                                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE;
+                                LED_RED_ON;
-                        // initialize magnetic sensor configuration
-                        InitNC_MagnetSensor();
+                        }
                 else
-                {
-                        UART1_PutString("\n\r Not compatible!");
-                        UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE;
-                        LED_RED_ON;
-                }
                 else // nothing found
         else // nothing found

Subversion Repositories NaviCtrl

(root)/tags/V2.06d/ncmag.c @ 811 - Rev 488 → 489