WebSVN - NaviCtrl - Diff - Rev 743 and 767


/*#######################################################################################*/
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
/*#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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
// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
// +  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.
// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
// + features that can be used to identify the program may not be altered or defaced by the customer.
// + The customer shall be responsible for taking reasonable precautions
// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
// + #### 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 "i2c.h"
#include "timer1.h"
#include "led.h"
#include "uart1.h"
#include "eeprom.h"
#include "mymath.h"
#include "main.h"
#include "spi_slave.h"
 
u8 NCMAG_Present = 0;
u8 NCMAG_IsCalibrated = 0;
u8 NCMAG_SensorType = TYPE_NONE;
u8 NCMAG_Orientation = 0; // 0 means unknown!
u8 NCMAG_Compass_use_Orientation = 0;
 
#define CALIBRATION_VERSION                     1
#define MAG_CALIBRATION_COMPATIBLE              0xA3
 
#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;
        Scaling_t MagZ;
        u8 Version;
        u8 crc;
} __attribute__((packed)) Calibration_t;
 
Calibration_t Calibration;              // calibration data in RAM
volatile s16vec_t AccRawVector;
volatile s16vec_t MagRawVector;
 
// i2c MAG interface
#define MAG_SLAVE_ADDRESS       0x3C    // i2C slave address mag. sensor registers
 
// register mapping
#define REG_MAG_CRA                     0x00
#define REG_MAG_CRB                     0x01
#define REG_MAG_MODE            0x02
#define REG_MAG_DATAX_MSB       0x03
#define REG_MAG_DATAX_LSB       0x04
#define REG_MAG_DATAY_MSB       0x05
#define REG_MAG_DATAY_LSB       0x06
#define REG_MAG_DATAZ_MSB       0x07
#define REG_MAG_DATAZ_LSB       0x08
#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  // WHO_AM_I _M = 0x03c when LSM303DLM is connected
 
#define ADR_LSM303D_MAG         0x08
#define REG_303D_CTRL0          0x1F  // Fifo + Interrupts
#define REG_303D_CTRL1          0x20  // ACC Update Speed
#define REG_303D_CTRL2          0x21  // ACC Gain + Filter
#define REG_303D_CTRL3          0x22  // Fifo + Interrupts
#define REG_303D_CTRL4          0x23  // Fifo + Interrupts
#define REG_303D_CTRL5          0x24  // Temperature Enable + Magn.Resolution + Magn.Rate
#define REG_303D_CTRL6          0x25  // Magn. Gain 
#define REG_303D_CTRL7          0x26  // Magn. PowerDown + Filter
// bit mask for configuration mode
#define CRA_MODE_MASK           0x03
#define CRA_MODE_NORMAL         0x00    //default
#define CRA_MODE_POSBIAS        0x01
#define CRA_MODE_NEGBIAS        0x02
#define CRA_MODE_SELFTEST       0x03
 
// bit mask for measurement mode
#define MODE_MASK                       0xFF
#define MODE_CONTINUOUS         0x00
#define MODE_SINGLE                     0x01    // default
#define MODE_IDLE                       0x02
#define MODE_SLEEP                      0x03
 
// bit mask for rate
#define CRA_RATE_MASK           0x1C
 
// bit mask for gain
#define CRB_GAIN_MASK           0xE0
 
// ids
#define MAG_IDA         0x48
#define MAG_IDB         0x34
#define MAG_IDC         0x33
#define MAG_IDF_LSM303DLM       0x3C
#define MAG_IDF_LSM303D         0x49
#define MAG_IDF_LSM303DLH       0x00
 
// the special HMC5843 interface
// bit mask for rate
#define HMC5843_CRA_RATE_0_5HZ          0x00
#define HMC5843_CRA_RATE_1HZ            0x04
#define HMC5843_CRA_RATE_2HZ            0x08
#define HMC5843_CRA_RATE_5HZ            0x0C
#define HMC5843_CRA_RATE_10HZ           0x10    //default
#define HMC5843_CRA_RATE_20HZ           0x14
#define HMC5843_CRA_RATE_50HZ           0x18    // <-- we use this
// 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
// self test value
#define HMC5843_TEST_XSCALE             555
#define HMC5843_TEST_YSCALE             555
#define HMC5843_TEST_ZSCALE             555
// calibration range
#define HMC5843_CALIBRATION_RANGE   600
 
// the special LSM302DLH interface
// bit mask for rate
#define LSM303DLH_CRA_RATE_0_75HZ       0x00
#define LSM303DLH_CRA_RATE_1_5HZ        0x04
#define LSM303DLH_CRA_RATE_3_0HZ        0x08
#define LSM303DLH_CRA_RATE_7_5HZ        0x0C
#define LSM303DLH_CRA_RATE_15HZ         0x10    //default
#define LSM303DLH_CRA_RATE_30HZ         0x14
#define LSM303DLH_CRA_RATE_75HZ         0x18    // <-- we use this
 
// LSM303D CTRL1
#define LSM303D_ACC_RATE_0HZ            0x0F
#define LSM303D_ACC_RATE_3HZ            0x1F
#define LSM303D_ACC_RATE_6HZ            0x2F
#define LSM303D_ACC_RATE_12HZ           0x3F
#define LSM303D_ACC_RATE_25HZ           0x4F
#define LSM303D_ACC_RATE_50HZ           0x5F
#define LSM303D_ACC_RATE_100HZ          0x6F
#define LSM303D_ACC_RATE_200HZ          0x7F
#define LSM303D_ACC_RATE_400HZ          0x8F
#define LSM303D_ACC_RATE_800HZ          0x9F
#define LSM303D_ACC_RATE_1600HZ         0xAF
 
// LSM303D CTRL1
#define LSM303D_ACC_SCALE_2g            0x00
#define LSM303D_ACC_SCALE_4g            0x08
#define LSM303D_ACC_SCALE_6g            0x10
#define LSM303D_ACC_SCALE_8g            0x18
#define LSM303D_ACC_SCALE_16g           0x20
 
// LSM303D CTRL5
#define LSM303D_ODR_RATE_3HZ            0x00
#define LSM303D_ODR_RATE_6HZ            0x04
#define LSM303D_ODR_RATE_12HZ           0x08
#define LSM303D_ODR_RATE_25HZ           0x0C
#define LSM303D_ODR_RATE_50HZ           0x10
#define LSM303D_ODR_RATE_100HZ          0x14 // attention: ACC Rate must be >50Hz to use this
#define LSM303D_HIGH_RESULUTION         0x60 
#define LSM303D_LOW_RESULUTION          0x00 
#define LSM303D_TEMP_ENABLE                     0x80 
 
// LSM303D CTRL6
#define LSM303D_MAG_SCALE_2GA           0x00
#define LSM303D_MAG_SCALE_4GA           0x20   
#define LSM303D_MAG_SCALE_8GA           0x40   // <-- we use this (Achtung: der LSM303D hat eine höhere Auflösung)
#define LSM303D_MAG_SCALE_12GA          0x60
 
// bit mask for gain
#define LSM303DLH_CRB_GAIN_XXGA         0x00
#define LSM303DLH_CRB_GAIN_13GA         0x20    //default
#define LSM303DLH_CRB_GAIN_19GA         0x40    
#define LSM303DLH_CRB_GAIN_25GA         0x60
#define LSM303DLH_CRB_GAIN_40GA         0x80    // <--- we use this (Since V2.03)
#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   245
#define LSM303DLH_TEST_YSCALE   245
#define LSM303DLH_TEST_ZSCALE   235
// clibration range
#define LSM303_CALIBRATION_RANGE   300 // War bis V2.02: 550 -> Auflösung von 19Ga auf 40GA reduziert
 
// the i2c ACC interface
#define ACC_SLAVE_ADDRESS               0x30    // i2c slave for acc. sensor registers
 
// multiple byte read/write mask
#define REG_MASK_AUTOINCREMENT 0x80
 
// register mapping
#define REG_ACC_CTRL1                   0x20
#define REG_ACC_CTRL2                   0x21
#define REG_ACC_CTRL3                   0x22
#define REG_ACC_CTRL4                   0x23
#define REG_ACC_CTRL5                   0x24
#define REG_ACC_HP_FILTER_RESET 0x25
#define REG_ACC_REFERENCE               0x26
#define REG_ACC_STATUS                  0x27
#define REG_ACC_X_LSB                   0x28
#define REG_ACC_X_MSB                   0x29
#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_CRTL2_FILTER8       0x10
#define ACC_CRTL2_FILTER16      0x11
#define ACC_CRTL2_FILTER32      0x12
#define ACC_CRTL2_FILTER64      0x13
 
#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
#define ACC_CTRL4_ST_ENABLE             0x02
 
#define ACC_CTRL5_STW_ON                0x03
#define ACC_CTRL5_STW_OFF               0x00
 
typedef struct
{
        u8 ctrl_1;
        u8 ctrl_2;
        u8 ctrl_3;
        u8 ctrl_4;
        u8 ctrl_5;
} __attribute__((packed)) AccConfig_t;
 
volatile AccConfig_t AccConfig;
 
// write calibration data for external and internal sensor seperately
u8 NCMag_CalibrationWrite(I2C_TypeDef* I2Cx)
{
        u16 address;
        u8 i = 0, crc = MAG_CALIBRATION_COMPATIBLE;
        EEPROM_Result_t eres;
        u8 *pBuff = (u8*)&Calibration;
        Calibration.Version = CALIBRATION_VERSION + (NCMAG_Orientation<<4);;
 
        if (I2Cx == NCMAG_PORT_EXTERN)
        {
                address = EEPROM_ADR_MAG_CALIBRATION_EXTERN;
        }
        else if (I2Cx == NCMAG_PORT_INTERN)
        {
                address = EEPROM_ADR_MAG_CALIBRATION_INTERN;
        }
        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);
}
 
// read calibration data for external and internal sensor seperately
u8 NCMag_CalibrationRead(I2C_TypeDef* I2Cx)
{
        u8 address;
        u8 i = 0, crc = MAG_CALIBRATION_COMPATIBLE;
        u8 *pBuff = (u8*)&Calibration;
 
        if (I2Cx == NCMAG_PORT_EXTERN)          address = EEPROM_ADR_MAG_CALIBRATION_EXTERN;
        else if (I2Cx == NCMAG_PORT_INTERN) address = EEPROM_ADR_MAG_CALIBRATION_INTERN;
        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);
        }
        return(0);
}
 
 
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, Zmin2 = 0, Zmax2 = 0;;
        static s16 X = 0, Y = 0, Z = 0;
        static u8 OldCalState = 0;
        s16 MinCalibration = 450;
 
        X = (X + MagRawVector.X)/2;
        Y = (Y + MagRawVector.Y)/2;
        Z = (Z + MagRawVector.Z)/2;
 
        switch(Compass_CalState)
        {
                case 1:
                        // 1st step of calibration
                        // initialize ranges
                        // used to change the orientation of the NC in the horizontal plane
                        Xmin =  10000;
                        Xmax = -10000;
                        Ymin =  10000;
                        Ymax = -10000;
                        Zmin =  10000;
                        Zmax = -10000;
                        Zmin2 =  10000;
                        Zmax2 = -10000;
                        X = 0; Y = 0; Z = 0;
                        speak = 1;
                        CompassValueErrorCount = 0;
                        if(Compass_CalState != OldCalState) // only once per state
                        {
                                UART1_PutString("\r\nStarting compass calibration");
                                if(NCMAG_Compass_use_Orientation)
                                {
                                        if(!NCMAG_Orientation) NCMAG_Orientation = NCMAG_GetOrientationFromAcc();
                                        UART1_PutString(" - External sensor ");
                                        sprintf(msg, "with orientation: %d ", NCMAG_Orientation);
                                        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;}
                        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\r\n");
                                                MinCalibration = HMC5843_CALIBRATION_RANGE;
                                                break;
 
                                        case TYPE_LSM303DLH:
                                        case TYPE_LSM303DLM:
                                        case TYPE_LSM303D:
                                        default:
                                                UART1_PutString("\r\nFinished: LSM303 calibration\r\n");
                                                MinCalibration = LSM303_CALIBRATION_RANGE;
                                                break;
                                }
                                if(EarthMagneticStrengthTheoretic)
                                {
                                        MinCalibration = (MinCalibration * EarthMagneticStrengthTheoretic) / 50;
                                        sprintf(msg, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic);
                                        UART1_PutString(msg);
                                }
                            else UART1_PutString("without GPS\r\n");
 
                                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(Compass_I2CPort);
                                        BeepTime = 2500;
                                        UART1_PutString("\r\n-> Calibration okay <-\r\n");
                                        SpeakHoTT = SPEAK_MIKROKOPTER;
                                }
                                else
                                {
                                        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
                                        NCMAG_IsCalibrated = NCMag_CalibrationRead(Compass_I2CPort);
                                }
                                        sprintf(msg, "X: (%i - %i = %i)\r\n",Xmax,Xmin,Xmax - Xmin);
                                        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",MinCalibration);
                                        UART1_PutString(msg);
                        }
                        break;
 
                default:
                        break;
        }
        OldCalState = Compass_CalState;
}
 
// ---------- call back handlers -----------------------------------------
 
// rx data handler for id info request
void NCMAG_UpdateIdentification(u8* pRxBuffer, u8 RxBufferSize)
{       // if number of bytes are matching
        if(RxBufferSize == sizeof(NCMAG_Identification) )
        {
                memcpy((u8 *)&NCMAG_Identification, pRxBuffer, sizeof(NCMAG_Identification));
        }
}
 
void NCMAG_UpdateIdentification_Sub(u8* pRxBuffer, u8 RxBufferSize)
{       // if number of bytes are matching
        if(RxBufferSize == sizeof(NCMAG_Identification2))
        {
                memcpy((u8 *)&NCMAG_Identification2, pRxBuffer, sizeof(NCMAG_Identification2));
        }
}
 
// rx data handler for magnetic sensor raw data
void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)
{      
u8 endian = 1;
        if(NCMAG_SensorType == TYPE_LSM303D) endian = 0; // here the low and high-bytes are mixed
        if(RxBufferSize == sizeof(MagRawVector) ) // if number of bytes are matching
        {       // byte order from big to little endian
                s16 raw, X = 0, Y = 0, Z = 0;
                if(endian) {raw = pRxBuffer[0]<<8; raw+= pRxBuffer[1];} else {raw = (pRxBuffer[1] << 8) + pRxBuffer[0]; raw >>= 3;}
                if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) X = raw;
                else if(CompassValueErrorCount < 35) CompassValueErrorCount++; // invalid data
                if(endian) {raw = pRxBuffer[2]<<8; raw+= pRxBuffer[3];} else {raw = (pRxBuffer[3] << 8) + pRxBuffer[2]; raw >>= 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
                if(endian) {raw = pRxBuffer[4]<<8; raw+= pRxBuffer[5];} else {raw = (pRxBuffer[5] << 8) + pRxBuffer[4]; raw >>= 3;}
                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
                // correct compass orientation
                switch(NCMAG_Orientation)
                {
                        case 0:
                        case 1:
                        default:
                                // 1:1 Mapping
                                MagRawVector.X = X;
                                MagRawVector.Y = Y;
                                MagRawVector.Z = Z;
                                break;
                        case 2:
                                MagRawVector.X = -X;
                                MagRawVector.Y = Y;
                                MagRawVector.Z = -Z;
                                break;
                        case 3:
                                MagRawVector.X = -Z;
                                MagRawVector.Y = Y;
                                MagRawVector.Z = X;
                                break;
                        case 4:
                                MagRawVector.X = Z;
                                MagRawVector.Y = Y;
                                MagRawVector.Z = -X;
                                break;
                        case 5:
                                MagRawVector.X = X;
                                MagRawVector.Y = -Z;
                                MagRawVector.Z = Y;
                                break;
                        case 6:
                                MagRawVector.X = -X;
                                MagRawVector.Y = -Z;
                                MagRawVector.Z = -Y;
                                break;
                }
        }
        if(Compass_CalState || !NCMAG_IsCalibrated)
        {       // mark out data invalid
                MagVector.X = MagRawVector.X;
                MagVector.Y = MagRawVector.Y;
                MagVector.Z = MagRawVector.Z;
                Compass_Heading = -1;
        }
        else
        {
                // update MagVector from MagRaw Vector by Scaling
                MagVector.X = (s16)((1024L*(s32)(MagRawVector.X - Calibration.MagX.Offset))/Calibration.MagX.Range);
                MagVector.Y = (s16)((1024L*(s32)(MagRawVector.Y - Calibration.MagY.Offset))/Calibration.MagY.Range);
                MagVector.Z = (s16)((1024L*(s32)(MagRawVector.Z - Calibration.MagZ.Offset))/Calibration.MagZ.Range);
                Compass_CalcHeading();
        }
}
// rx data handler  for acceleration raw data
void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize)
{       // 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));
        }
}
// 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*)(&MagConfig), pRxBuffer, sizeof(MagConfig));
        }
}
// rx data handler for reading acceleration sensor configuration
void NCMAG_UpdateAccConfig(u8* pRxBuffer, u8 RxBufferSize)
{       // if number of byte are matching
        if(RxBufferSize == sizeof(AccConfig) )
        {
                memcpy((u8*)&AccConfig, pRxBuffer, sizeof(AccConfig));
        }
}
//----------------------------------------------------------------------
 
u8 NCMAG_GetOrientationFromAcc(void)
{
        // only if external compass connected
        if(!NCMAG_Compass_use_Orientation) return(0);
         // MK must not be tilted
        if((abs(FromFlightCtrl.AngleNick) > 300) || (abs(FromFlightCtrl.AngleRoll) > 300))
        {
//              UART1_PutString("\r\nTilted");
                return(0);
        }
        if(NCMAG_SensorType == TYPE_LSM303D) return(2); // der Sensor muss immer so verbaut sein, weil der interne ACC manchmal spinnt
 
        if(abs(AccRawVector.Z) + abs(AccRawVector.X) + abs(AccRawVector.Y) > 9000) return(0);  // sensor defekt?
        // 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;
 
        // try to catch the i2c buffer within 100 ms timeout
        if(I2CBus_LockBuffer(Compass_I2CPort, 100))
        {
          if(NCMAG_SensorType == TYPE_LSM303D)
          {
                u8 TxBytes = 0;
                u8 CfgData[] = {        REG_303D_CTRL0 | REG_MASK_AUTOINCREMENT,
                                                        0,                                                      // Ctrl 0 -> Fifo
                                                        LSM303D_ACC_RATE_50HZ,          // Ctrl 1 -> ACC Update Speed
                                                        LSM303D_ACC_SCALE_8g,           // Ctrl 2 -> ACC Gain + Filter (0 = 773Hz)
                                                        0,                                                      // Ctrl 3 -> Interrupts
                                                        0,                                                      // Ctrl 4 -> Interrupts
                                                        LSM303D_ODR_RATE_50HZ | LSM303D_TEMP_ENABLE | LSM303D_LOW_RESULUTION,// Ctrl 5 -> Temperature Enable + Magn.Resolution + Magn.Rate
                                                        LSM303D_MAG_SCALE_8GA,          // Ctrl 6 -> Magn. Scale
                                                        0x80                                            // Ctrl 7 -> Magn. PowerDown + Filter
                                                 };
 
                TxBytes = sizeof(CfgData);
 
                if(I2CBus_Transmission(Compass_I2CPort, MAG_SLAVE_ADDRESS, CfgData, TxBytes, 0, 0))
                {
                        if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 100))
                        {
                                if(I2CBus(Compass_I2CPort)->Error == I2C_ERROR_NONE) retval = 1;
                        }
                }
          }
          else
          {
                u8 TxBytes = 0;
                u8 TxData[sizeof(MagConfig) + 3];
 
                TxData[TxBytes++] = REG_MAG_CRA;
                memcpy(&TxData[TxBytes], (u8*)&MagConfig, sizeof(MagConfig));
                TxBytes += sizeof(MagConfig);
                if(I2CBus_Transmission(Compass_I2CPort, MAG_SLAVE_ADDRESS, TxData, TxBytes, 0, 0))
                {
                        if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 100))
                        {
                                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))
        {
                u8 TxBytes = 0;
                u8 TxData[3];
                TxData[TxBytes++] = REG_MAG_CRA;
                if(I2CBus_Transmission(Compass_I2CPort, MAG_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateMagConfig, sizeof(MagConfig)))
                {
                        if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 100))
                        {
                                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
        if(I2CBus_LockBuffer(Compass_I2CPort, 50))
        {
                u8 TxBytes = 0;
                u8 TxData[sizeof(AccConfig) + 3];
                TxData[TxBytes++] = REG_ACC_CTRL1|REG_MASK_AUTOINCREMENT;
                memcpy(&TxData[TxBytes], (u8*)&AccConfig, sizeof(AccConfig));
                TxBytes += sizeof(AccConfig);
                if(I2CBus_Transmission(Compass_I2CPort, ACC_SLAVE_ADDRESS, TxData, TxBytes, 0, 0))
                {
                        if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 50))
                        {
                                if(I2CBus(Compass_I2CPort)->Error == I2C_ERROR_NONE) retval = 1;
                        }
                }
        }
        return(retval);
}
 
// ----------------------------------------------------------------------------------------
u8 NCMAG_GetAccConfig(void)
{
        u8 retval = 0;
        // try to catch the i2c buffer within 100 ms timeout
        if(I2CBus_LockBuffer(Compass_I2CPort, 100))
        {
                u8 TxBytes = 0;
                u8 TxData[3];
                TxData[TxBytes++] = REG_ACC_CTRL1|REG_MASK_AUTOINCREMENT;
                if(I2CBus_Transmission(Compass_I2CPort, ACC_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateAccConfig, sizeof(AccConfig)))
                {
                        if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 100))
                        {
                                if(I2CBus(Compass_I2CPort)->Error == I2C_ERROR_NONE) retval = 1;
                        }
                }
        }
        return(retval);
}
 
// ----------------------------------------------------------------------------------------
u8 NCMAG_GetIdentification(void)
{
        u8 retval = 0;
        if(NCMAG_SensorType == TYPE_LSM303D) return(1);
        // try to catch the i2c buffer within 100 ms timeout
        if(I2CBus_LockBuffer(Compass_I2CPort, 100))
        {
                u8 TxBytes = 0;
                u8 TxData[3];
                NCMAG_Identification.A = 0xFF;
                NCMAG_Identification.B = 0xFF;
                NCMAG_Identification.C = 0xFF;
                TxData[TxBytes++] = REG_MAG_IDA;
                // initiate transmission
                if(I2CBus_Transmission(Compass_I2CPort, MAG_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateIdentification, sizeof(NCMAG_Identification)))
                {
                        if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 100))
                        {
                                if(I2CBus(Compass_I2CPort)->Error == I2C_ERROR_NONE) retval = 1;
                        }
                }
        }
        return(retval);
}
 
u8 NCMAG_GetIdentification_Sub(void)
{
        u8 retval = 0;
        // try to catch the i2c buffer within 100 ms timeout
        if(I2CBus_LockBuffer(Compass_I2CPort, 100))
        {
                u8 TxBytes = 0;
                u8 TxData[3];
                NCMAG_Identification2.Sub = 0xFF;
                TxData[TxBytes++] = REG_MAG_IDF;
                // initiate transmission
                if(I2CBus_Transmission(Compass_I2CPort, MAG_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateIdentification_Sub, sizeof(NCMAG_Identification2)))
                {
                        if(I2CBus_WaitForEndOfTransmission(Compass_I2CPort, 100))
                        {
                                if(I2CBus(Compass_I2CPort)->Error == I2C_ERROR_NONE) retval = 1;
                        }
                }
        }
        return(retval);
}
 
 
// ----------------------------------------------------------------------------------------
void NCMAG_GetMagVector(u8 timeout)
{
        // try to catch the I2C buffer within timeout ms
        if(I2CBus_LockBuffer(Compass_I2CPort, timeout))
        {
                u8 TxBytes = 0;
                u8 TxData[3];
                // set register pointer
                if(NCMAG_SensorType == TYPE_LSM303D) TxData[TxBytes++] = ADR_LSM303D_MAG | REG_MASK_AUTOINCREMENT;
                else TxData[TxBytes++] = REG_MAG_DATAX_MSB;
                // initiate transmission
                I2CBus_Transmission(Compass_I2CPort, MAG_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateMagVector, sizeof(MagVector));
        }
}
 
//----------------------------------------------------------------
void NCMAG_GetAccVector(u8 timeout)
{
        // try to catch the I2C buffer within timeout ms
        if(I2CBus_LockBuffer(Compass_I2CPort, timeout))
        {
                u8 TxBytes = 0;
                u8 TxData[3];
                // set register pointer
                if(NCMAG_SensorType == TYPE_LSM303D)
                 {
                        TxData[TxBytes++] = REG_ACC_X_LSB | REG_MASK_AUTOINCREMENT;
                        I2CBus_Transmission(Compass_I2CPort, MAG_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector));
                 }
                else
                 {
                        TxData[TxBytes++] = REG_ACC_X_LSB | REG_MASK_AUTOINCREMENT;
                        // initiate transmission
                        I2CBus_Transmission(Compass_I2CPort, ACC_SLAVE_ADDRESS, TxData, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector));
                 }
        }
}
 
//----------------------------------------------------------------
u8 NCMAG_ConfigureSensor(void)
{
        switch(NCMAG_SensorType)
        {
                case TYPE_HMC5843:
                        MagConfig.cra = HMC5843_CRA_RATE_50HZ|CRA_MODE_NORMAL;
                        MagConfig.crb = HMC5843_CRB_GAIN_15GA;
                        MagConfig.mode = MODE_CONTINUOUS;
                        break;
 
                case TYPE_LSM303DLH:
                case TYPE_LSM303DLM:
//                      MagConfig.crb = LSM303DLH_CRB_GAIN_19GA;
                        MagConfig.crb = LSM303DLH_CRB_GAIN_40GA; // seit 2.03 -> 2.2014 
                        MagConfig.cra = LSM303DLH_CRA_RATE_75HZ|CRA_MODE_NORMAL;
                        MagConfig.mode = MODE_CONTINUOUS;
                        break;
                case TYPE_LSM303D:
                    // -> see defined data in NCMAG_SetMagConfig();
                        break;
                default:
                return(0);
        }
        return(NCMAG_SetMagConfig());
}
 
 
//----------------------------------------------------------------
u8 NCMAG_Init_ACCSensor(void)
{
  if(NCMAG_SensorType == TYPE_LSM303D)
  {
   return(NCMAG_SetMagConfig());
  }
  else
  {
        AccConfig.ctrl_1 = ACC_CRTL1_PM_NORMAL|ACC_CRTL1_DR_50HZ|ACC_CRTL1_XEN|ACC_CRTL1_YEN|ACC_CRTL1_ZEN;
        AccConfig.ctrl_2 = 0;
        AccConfig.ctrl_3 = 0x00;
        AccConfig.ctrl_4 = ACC_CTRL4_BDU | ACC_CTRL4_FS_8G;
        AccConfig.ctrl_5 = ACC_CTRL5_STW_OFF;
        return(NCMAG_SetAccConfig());
  }
}
// --------------------------------------------------------
void NCMAG_Update(u8 init)
{
        static u32 TimerUpdate = 0;
        static s8 send_config = 0;
        u32 delay = 20;
 
        if(init) TimerUpdate = SetDelay(10);
 
        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))
        {
                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
                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:
                                        delay = 20;      // next cycle after 20 ms
                                        NCMAG_GetMagVector(5);
                                        break;
                                case TYPE_LSM303DLH:
                                case TYPE_LSM303DLM:
                                case TYPE_LSM303D:
                                        delay = 20;      // next cycle after 20 ms
                                        if(s-- || (NCMAG_Compass_use_Orientation == 0))
                                        {
                                                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();
                                                // get new data
                                                NCMAG_GetAccVector(5);
                                                delay = 10; // next cycle after 10 ms
                                                s = 40; //reset downconter about 0,8 sec
/*
DebugOut.Analog[] = AccVector.X;
DebugOut.Analog[] = AccVector.Y;
DebugOut.Analog[] = AccVector.Z;
//s = 2;
*/
                                        }
                                        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_SelfTest(void)
{
        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;
        u8 crb_gain, cra_rate;
        u8 i = 0, retval = 1;
 
        switch(NCMAG_SensorType)
        {
                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 TYPE_LSM303DLH:
//                      crb_gain = LSM303DLH_CRB_GAIN_19GA;
                        crb_gain = LSM303DLH_CRB_GAIN_40GA; // seit 2.03 -> 2.2014 
                        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 = 1;
                        return(1); // always return success
                        break;
                case TYPE_LSM303D:
                        done = 1;
                        return(1); // always return success
                        break;
                default:
                        return(0);
        }
 
        MagConfig.cra = cra_rate|CRA_MODE_POSBIAS;
        MagConfig.crb = crb_gain;
        MagConfig.mode = MODE_CONTINUOUS;
        // activate positive bias field
        NCMAG_SetMagConfig();
        // wait for stable readings
        time = SetDelay(50);
        while(!CheckDelay(time));
        // averaging
        #define AVERAGE 20
        for(i = 0; i<AVERAGE; i++)
        {
                NCMAG_GetMagVector(5);
                time = SetDelay(20);
        while(!CheckDelay(time));
                XMax += MagRawVector.X;
                YMax += MagRawVector.Y;
                ZMax += MagRawVector.Z;
        }
        MagConfig.cra = cra_rate|CRA_MODE_NEGBIAS;
        // activate positive bias field
        NCMAG_SetMagConfig();
    // wait for stable readings
        time = SetDelay(50);
        while(!CheckDelay(time));
        // averaging
        for(i = 0; i < AVERAGE; i++)
        {
                NCMAG_GetMagVector(5);
                time = SetDelay(20);
        while(!CheckDelay(time));
                XMin += MagRawVector.X;
                YMin += MagRawVector.Y;
                ZMin += MagRawVector.Z;
        }
        // setup final configuration
        MagConfig.cra = cra_rate|CRA_MODE_NORMAL;
        // activate positive bias field
        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_CheckOrientation(void)
{       // only for external sensor
        if(NCMAG_Compass_use_Orientation)
        {
                NCMAG_Orientation = NCMAG_GetOrientationFromAcc();
                if(NCMAG_Orientation != (Calibration.Version>>4)) NCMAG_IsCalibrated = 0;
                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);
        }
        else  // full init
        {
                u8 msg[64];
                u8 retval = 0;
                u8 repeat = 0;
 
                //--------------------------------------------
                // search external sensor first
                //--------------------------------------------
                Compass_I2CPort = NCMAG_PORT_EXTERN;
                retval = 0;
                // get id bytes
                for(repeat = 0; repeat < 5; repeat++)
                {
                        if(NCMAG_GetIdentification_Sub()) break;
                        UART1_PutString("=");
                }
//sprintf(msg, "(WhoAmI=%02x)", NCMAG_Identification2.Sub);
//UART1_PutString(msg);
        if(NCMAG_Identification2.Sub == MAG_IDF_LSM303D)        
         {
                NCMAG_SensorType = TYPE_LSM303D;
                NCMAG_ConfigureSensor();
                NCMAG_Compass_use_Orientation = 1;
                retval = 1;
         }
        else
         {      // External Sensor not LSM303D
                for(repeat = 0; repeat < 3; repeat++)  // does it have ACC?
                {
                        retval = NCMAG_GetAccConfig();            // only the external sensor with ACC is supported
                        if(retval) break; // break loop on success
                        UART1_PutString("_");
                }
                // Extenal sensor not found?
                if(!retval)
                {
                        // search internal sensor afterwards
                        if(UART_VersionInfo.HWMajor >= 30)
                        {
                          UART1_PutString(" main I2C-Bus ");
                          NCMAG_Compass_use_Orientation = 1;
                         }
                        else
                         {
                          UART1_PutString(" internal sensor ");
                          NCMAG_Compass_use_Orientation = 0;
                         }
                        Compass_I2CPort = NCMAG_PORT_INTERN;
                }
                else
                {
                        UART1_PutString(" external sensor ");
                        Compass_I2CPort = NCMAG_PORT_EXTERN;
                        NCMAG_Compass_use_Orientation = 1;
                }
                //-------------------------------------------
                NCMAG_Present = 0;
                NCMAG_SensorType = TYPE_HMC5843;        // assuming having an HMC5843
               
                for(repeat = 0; repeat < 3; repeat++)
                {
                        if(NCMAG_GetIdentification_Sub()) break;
                        UART1_PutString("=");
                }
                if(NCMAG_Identification2.Sub == MAG_IDF_LSM303D) // internal Sensor the LSM303D?        
                 {
                        NCMAG_SensorType = TYPE_LSM303D;
                        NCMAG_ConfigureSensor();
                        retval = 1;
                }
                else
                for(repeat = 0; repeat < 3; repeat++) // polling for LSM303DLH/DLM option by ACC address ack
                {
                        retval = NCMAG_GetAccConfig();
                        if(retval) break; // break loop on success
                }
                if(retval)      // it is a sensor with ACC
                {
                        switch(NCMAG_Identification2.Sub)
                         {
                          case MAG_IDF_LSM303DLM:       NCMAG_SensorType = TYPE_LSM303DLM;
                                                                                NCMAG_Init_ACCSensor();
                                                                                break;
                          case MAG_IDF_LSM303DLH:       NCMAG_SensorType = TYPE_LSM303DLH;
                                                                                NCMAG_Init_ACCSensor();
                                                                                break;
                          case MAG_IDF_LSM303D:         NCMAG_SensorType = TYPE_LSM303D;
                                                                                break;
                         }
                }
                // get id bytes
                retval = 0;
                for(repeat = 0; repeat < 3; repeat++)
                {
                        retval = NCMAG_GetIdentification();
                        if(retval) break; // break loop on success
                }
        }
                // if we got an answer to id request
                if(retval)
                {
                        u8 n1[] = "\r\n HMC5843";
                        u8 n2[] = "\r\n LSM303DLH";
                        u8 n3[] = "\r\n LSM303DLM";
                        u8 n4[] = "\r\n LSM303D";
                        u8 n5[] = "\r\n ??";
                        u8* pn = n5;
 
                        switch(NCMAG_SensorType)
                        {
                                case TYPE_HMC5843:
                                        pn = n1;
                                        break;
                                case TYPE_LSM303DLH:
                                        pn = n2;
                                        break;
                                case TYPE_LSM303DLM:
                                        pn = n3;
                                        break;
                                case TYPE_LSM303D:
                                        pn = n4;
                                        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_SensorType == TYPE_LSM303D)
                        {
                                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(NCMAG_Compass_use_Orientation)
                                {
                                        // 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, "\r\n Warning: calibrated orientation was %d !",Calibration.Version >> 4);
                                                                UART1_PutString(msg);
//NCMAG_IsCalibrated = 0; // force new calibration!
                                                        }
                                                }
                                        }
                                        else
                                        {
                                                UART1_PutString("unknown!");
                                        }
                                }
 
 
                                // perform self test
                                if(!NCMAG_SelfTest())
                                {
                                        UART1_PutString("\r\n Selftest failed!!!!!!!!!!!!!!!!!!!!\r\n");
                                        LED_RED_ON;
                                        //NCMAG_IsCalibrated = 0;
                                }
                                else UART1_PutString("\r\n Selftest ok");
 
                                // initialize magnetic sensor configuration
                                NCMAG_ConfigureSensor();
                        }
                        else
                        {
                                UART1_PutString("\r\n Not compatible!");
                                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE;
                                LED_RED_ON;
                        }
                }
                else // nothing found
                {
                        NCMAG_SensorType = TYPE_NONE;
                        UART1_PutString(" not found!");
                }
        }
        return(NCMAG_Present);
}
 
 
Subversion Repositories NaviCtrl

(root)/tags/V2.20f/ncmag.c @ 875 - Rev 743 → 767
