WebSVN - NaviCtrl - Diff - Rev 252 and 253

 /*#######################################################################################*/
 /* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
 /*#######################################################################################*/
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Copyright (c) 2010 Ingo Busker, Holger Buss
 // + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
 // + FOR NON COMMERCIAL USE ONLY
 // + www.MikroKopter.com
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
 // + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
 // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
 // + bzgl. der Nutzungsbedingungen aufzunehmen.
 // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
 // + Verkauf von Luftbildaufnahmen, usw.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
 // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
 // + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
 // + eindeutig als Ursprung verlinkt werden
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
 // + Benutzung auf eigene Gefahr
 // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
 // + mit unserer Zustimmung zulässig
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Redistributions of source code (with or without modifications) must retain the above copyright notice,
 // + this list of conditions and the following disclaimer.
 // +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
 // +     from this software without specific prior written permission.
 // +   * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
 // +     for non-commercial use (directly or indirectly)
 // +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
 // +     with our written permission
 // +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
 // +     clearly linked as origin
 // +   * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
 //
 // +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 // +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 // +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 // +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 // +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 // +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 // +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 // +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 // +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 // +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // +  POSSIBILITY OF SUCH DAMAGE.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #include <string.h>
 #include "91x_lib.h"
-#include "hmc5843.h"
+#include "ncmag.h"
 #include "i2c.h"
 #include "timer1.h"
 #include "led.h"
 #include "spi_slave.h"
 #include "uart1.h"
-#include "compass.h"
+u8 NCMAG_Present = 0;
+#define MAG_TYPE_NONE           0
-#define HMC5843_SLAVE_ADDRESS           0x3C  // i2C slave address
+#define MAG_TYPE_HMC5843        1
+#define MAG_TYPE_LSM303DLH      2
+u8 NCMAG_MagType = MAG_TYPE_NONE;
+// 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
+// bit mask for configuration mode
+#define CRA_MODE_MASK           0x03
+#define CRA_MODE_NORMAL         0x00    //default
+#define CRA_MODE_POSBIAS        0x01
-u8 HMC5843_Present = 0;
-#define REGISTER_CRA            0
+#define CRA_MODE_NEGBIAS        0x02
-#define REGISTER_CRB            1
+#define CRA_MODE_SELFTEST       0x03
-#define REGISTER_MODE           2
-#define REGISTER_DATAX_MSB      3
+// bit mask for measurement mode
-#define REGISTER_DATAX_LSB      4
+#define MODE_MASK                       0xFF
-#define REGISTER_DATAY_MSB      5
+#define MODE_CONTINUOUS         0x00
+#define MODE_SINGLE                     0x01    // default
-#define REGISTER_DATAY_LSB      6
+#define MODE_IDLE                       0x02
-#define REGISTER_DATAZ_MSB      7
+#define MODE_SLEEP                      0x03
-#define REGISTER_DATAZ_LSB      8
+// bit mask for rate
-#define REGISTER_STATUS         9
+#define CRA_RATE_MASK           0x1C
-#define REGISTER_IDA            10
-#define REGISTER_IDB            11
+// bit mask for gain
-#define REGISTER_IDC            12
+#define CRB_GAIN_MASK           0xE0
+// ids
+#define MAG_IDA         0x48
-#define HMC5843_IDA 0x48
+#define MAG_IDB         0x34
-#define HMC5843_IDB 0x34
-#define HMC5843_IDC     0x33
+#define MAG_IDC         0x33
-// bit mask for rate
+// the special HMC5843 interface
-#define HMC5843_CRA_RATE_MASK           0x1C
+// 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
-// bit mask for mode
-#define HMC5843_CRA_MODE_MASK           0x03
-#define HMC5843_CRA_MODE_NORMAL         0x00    //default
-#define HMC5843_CRA_MODE_POSBIAS        0x01
-#define HMC5843_CRA_MODE_NEGBIAS        0x02
-#define HMC5843_CRA_MODE_SELFTEST       0x03
 // bit mask for gain
-#define HMC5843_CRB_GAIN_MASK           0xE0
 #define HMC5843_CRB_GAIN_07GA           0x00
 #define HMC5843_CRB_GAIN_10GA           0x20    //default
 #define HMC5843_CRB_GAIN_15GA           0x40
 #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             715
+#define HMC5843_TEST_YSCALE             715
+#define HMC5843_TEST_ZSCALE             715
+// 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
+// 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
+#define LSM303DLH_CRB_GAIN_47GA         0xA0
+#define LSM303DLH_CRB_GAIN_56GA         0xC0
+#define LSM303DLH_CRB_GAIN_81GA         0xE0
+// self test value
+#define LSM303DLH_TEST_XSCALE   655
+#define LSM303DLH_TEST_YSCALE   655
+#define LSM303DLH_TEST_ZSCALE   630
+// the i2c ACC interface
+#define ACC_SLAVE_ADDRESS               0x30    // i2c slave for acc. sensor registers
+// 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
-// bit mask for measurement mode
-#define HMC5843_MODE_MASK                       0xFF
-#define HMC5843_MODE_CONTINUOUS         0x00
-#define HMC5843_MODE_SINGLE                     0x01    // default
-#define HMC5843_MODE_IDLE                       0x02
 #define HMC5843_MODE_SLEEP                      0x03
 typedef struct
+{
-  u8 A;
+        u8 A;
-  u8 B;
+        u8 B;
-  u8 C;
+        u8 C;
-} __attribute__((packed)) HMC5843_Identification_t;
+} __attribute__((packed)) Identification_t;
+volatile Identification_t NCMAG_Identification;
+typedef struct
+{
+        u8 cra;
+        u8 crb;
+        u8 mode;
+} __attribute__((packed)) MagConfig_t;
+volatile MagConfig_t MagConfig;
+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;
 volatile HMC5843_Identification_t HMC5843_Identification;
 volatile s16vec_t AccVector;
 // ---------- call back handlers -----------------------------------------
 // rx data handler for id info request
-void HMC5843_UpdateIdentification(u8* pRxBuffer, u8 RxBufferSize)
+void NCMAG_UpdateIdentification(u8* pRxBuffer, u8 RxBufferSize)
 {       // if number of byte are matching
-        if(RxBufferSize == sizeof(HMC5843_Identification) )
+        if(RxBufferSize == sizeof(NCMAG_Identification) )
+        {
-                memcpy((u8 *)&HMC5843_Identification, pRxBuffer, sizeof(HMC5843_Identification));
+                memcpy((u8 *)&NCMAG_Identification, pRxBuffer, sizeof(NCMAG_Identification));
         }
+}
 // rx data handler for mag vector request
-void HMC5843_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)
+void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)
 {       // if number of byte are matching
         if(RxBufferSize == sizeof(MagVector) )
         {       // byte order from big to little endian
                 MagVector.X = pRxBuffer[0]<<8;
                 MagVector.X+= pRxBuffer[1];
                 MagVector.Y = pRxBuffer[2]<<8;
                 MagVector.Y+= pRxBuffer[3];
                 MagVector.Z = pRxBuffer[4]<<8;
                 MagVector.Z+= pRxBuffer[5];
+        }
         // tbd. calculate heading from mag vector and attitude
         CompassHeading = 50; // tbd.
+}
+// rx data handler for acc vector request
+void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize)
+{       // if number of byte are matching
+        if(RxBufferSize == sizeof(AccVector) )
+        {
+                memcpy((u8*)&AccVector, pRxBuffer,sizeof(AccVector));
+        }
+}
+void NCMAG_UpdateMagConfig(u8* pRxBuffer, u8 RxBufferSize)
+{       // if number of byte are matching
+        if(RxBufferSize == sizeof(MagConfig) )
+        {
+                memcpy((u8*)(&MagConfig), pRxBuffer, sizeof(MagConfig));
+        }
+}
+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_SetMagConfig(void)
+{
+        u8 retval = 0;
+        // try to catch the i2c buffer within 100 ms timeout
+        if(I2C_LockBuffer(100))
+        {
+                u8 TxBytes = 0;
+                I2C_Buffer[TxBytes++] = REG_MAG_CRA;
+                memcpy((u8*)(&I2C_Buffer[TxBytes]), (u8*)&MagConfig, sizeof(MagConfig));
+                TxBytes += sizeof(MagConfig);
+                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, 0, 0))
+                {
+                        if(I2C_WaitForEndOfTransmission(100))
+                        {
+                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
+                        }
+                }
+        }
+        return(retval);
+}
 // ----------------------------------------------------------------------------------------
+// ----------------------------------------------------------------------------------------
-u8 HMC5843_GetIdentification(void)
+u8 NCMAG_GetMagConfig(void)
+{
         u8 retval = 0;
         // try to catch the i2c buffer within 100 ms timeout
         if(I2C_LockBuffer(100))
+        {
-                u16 TxBytes = 0;
+                u8 TxBytes = 0;
-                HMC5843_Identification.A = 0xFF;
-                HMC5843_Identification.B = 0xFF;
-                HMC5843_Identification.C = 0xFF;
-                I2C_Buffer[TxBytes++] = REGISTER_IDA;
+                I2C_Buffer[TxBytes++] = REG_MAG_CRA;
-                // initiate transmission
-                if(I2C_Transmission(HMC5843_SLAVE_ADDRESS, TxBytes, &HMC5843_UpdateIdentification, sizeof(HMC5843_Identification)))
+                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagConfig, sizeof(MagConfig)))
+                {
                         if(I2C_WaitForEndOfTransmission(100))
+                        {
                                 if(I2C_Error == I2C_ERROR_NONE) retval = 1;
+                        }
+                }
+        }
         return(retval);
+}
+// ----------------------------------------------------------------------------------------
+u8 NCMAG_SetAccConfig(void)
+{
+        u8 retval = 0;
+        // try to catch the i2c buffer within 100 ms timeout
+        if(I2C_LockBuffer(100))
+        {
+                u8 TxBytes = 0;
+                I2C_Buffer[TxBytes++] = REG_ACC_CTRL1;
+                memcpy((u8*)(&I2C_Buffer[TxBytes]), (u8*)&AccConfig, sizeof(AccConfig));
+                TxBytes += sizeof(AccConfig);
+                if(I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, 0, 0))
+                {
+                        if(I2C_WaitForEndOfTransmission(100))
+                        {
+                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
+                        }
+                }
+        }
+        return(retval);
+}
 // ----------------------------------------------------------------------------------------
-u8 HMC5843_SetConfiguration(u8 cra, u8 crb, u8 mode)
+u8 NCMAG_GetAccConfig(void)
+{
         u8 retval = 0;
-        // tra to catch the I2c buffer within 100 ms timeout
+        // try to catch the i2c buffer within 100 ms timeout
         if(I2C_LockBuffer(100))
+        {
-                u16 TxBytes = 0;
+                u8 TxBytes = 0;
-                I2C_Buffer[TxBytes++] = REGISTER_CRA;
+                I2C_Buffer[TxBytes++] = REG_ACC_CTRL1;
+                if(I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccConfig, sizeof(AccConfig)))
+                {
+                        if(I2C_WaitForEndOfTransmission(100))
+                        {
+                                if(I2C_Error == I2C_ERROR_NONE) retval = 1;
+                        }
+                }
+        }
+        return(retval);
+}
+// ----------------------------------------------------------------------------------------
+u8 NCMAG_GetIdentification(void)
+{
+        u8 retval = 0;
+        // try to catch the i2c buffer within 100 ms timeout
+        if(I2C_LockBuffer(100))
+        {
-                I2C_Buffer[TxBytes++] = cra;
+                u16 TxBytes = 0;
+                NCMAG_Identification.A = 0xFF;
+                NCMAG_Identification.B = 0xFF;
-                I2C_Buffer[TxBytes++] = crb;
+                NCMAG_Identification.C = 0xFF;
-                I2C_Buffer[TxBytes++] = mode;
+                I2C_Buffer[TxBytes++] = REG_MAG_IDA;
                 // initiate transmission
-                if(I2C_Transmission(HMC5843_SLAVE_ADDRESS, TxBytes, NULL, 0))
+                if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateIdentification, sizeof(NCMAG_Identification)))
+                {
-                        if(I2C_WaitForEndOfTransmission(50))
+                        if(I2C_WaitForEndOfTransmission(100))
+                        {
                                 if(I2C_Error == I2C_ERROR_NONE) retval = 1;
+                        }
+                }
+        }
         return(retval);
+}
+// ----------------------------------------------------------------------------------------
+void NCMAG_GetMagVector(void)
+{
+        // try to catch the I2C buffer within 0 ms
+        if(I2C_LockBuffer(0))
+        {
+                u16 TxBytes = 0;
+                // set register pointer
+                I2C_Buffer[TxBytes++] = REG_MAG_DATAX_MSB;
+                // initiate transmission
+                I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateMagVector, sizeof(MagVector));
+        }
+}
 //----------------------------------------------------------------
-void HMC5843_GetMagVector(void)
+void NCMAG_GetAccVector(void)
+{
         // try to catch the I2C buffer within 0 ms
         if(I2C_LockBuffer(0))
+        {
                 u16 TxBytes = 0;
                 // set register pointer
-                I2C_Buffer[TxBytes++] = REGISTER_DATAX_MSB;
+                I2C_Buffer[TxBytes++] = REG_ACC_X_LSB;
                 // initiate transmission
-                I2C_Transmission(HMC5843_SLAVE_ADDRESS, TxBytes, &HMC5843_UpdateMagVector, sizeof(MagVector));
+                I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccVector));
+        }
+}
+// --------------------------------------------------------
-void HMC5843_UpdateCompass(void)
+void NCMAG_UpdateCompass(void)
+{
         static u32 TimerCompassUpdate = 0;
-        if( (I2C_State == I2C_STATE_OFF) || !HMC5843_Present ) return;
+        if( (I2C_State == I2C_STATE_OFF) || !NCMAG_Present ) return;
         if(CheckDelay(TimerCompassUpdate))
+        {
                 // check for incomming compass calibration request
                 // update CalByte from spi input queue
                 /*fifo_get(&CompassCalcStateFiFo, (u8 *)&(MK3MAG_WriteCal.CalByte));
                 // send new calstate
                 if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte)
+                {
                         do the calibration here
+                }
                 else // request current heading */
+                {
-                        HMC5843_GetMagVector();
+                        NCMAG_GetMagVector();
                 }
                 TimerCompassUpdate = SetDelay(20);    // every 20 ms are 50 Hz
+        }
+}
-u8 HMC5843_SelfTest(void)
+u8 NCMAG_SelfTest(void)
+{
         #define LIMITS(value, min, max) {min = (90 * value)/100; max = (110 * 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;
-        s16 scale, scale_min, scale_max;
+        u8 crb_gain, cra_rate;
+        u8 i = 0, retval = 1;
+        switch(NCMAG_MagType)
+        {
+                case MAG_TYPE_HMC5843:
+                        crb_gain = HMC5843_CRB_GAIN_10GA;
+                        cra_rate = HMC5843_CRA_RATE_50HZ;
+                        xscale = HMC5843_TEST_XSCALE;
+                        yscale = HMC5843_TEST_YSCALE;
+                        zscale = HMC5843_TEST_ZSCALE;
+                        break;
+                case MAG_TYPE_LSM303DLH:
+                        crb_gain = LSM303DLH_CRB_GAIN_13GA;
+                        cra_rate = LSM303DLH_CRA_RATE_75HZ;
+                        xscale = LSM303DLH_TEST_XSCALE;
+                        yscale = LSM303DLH_TEST_YSCALE;
+                        zscale = LSM303DLH_TEST_ZSCALE;
+                        break;
+                default:
+                return(0);
+        }
+        MagConfig.cra = cra_rate|CRA_MODE_POSBIAS;
-        u8 i = 0;
+        MagConfig.crb = crb_gain;
+        MagConfig.mode = MODE_CONTINUOUS;
-        // activate positive bias field  of 0.55 gauss
+        // activate positive bias field
-        HMC5843_SetConfiguration(HMC5843_CRA_RATE_50HZ|HMC5843_CRA_MODE_POSBIAS, HMC5843_CRB_GAIN_10GA, HMC5843_MODE_CONTINUOUS);
+        NCMAG_SetMagConfig();
         // wait for stable readings
         time = SetDelay(50);
         while(!CheckDelay(time));
         // averaging
+        #define AVERAGE 20
-        for(i = 0; i<20; i++)
+        for(i = 0; i<AVERAGE; i++)
+        {
-                HMC5843_GetMagVector();
+                NCMAG_GetMagVector();
                 time = SetDelay(20);
         while(!CheckDelay(time));
                 XMax += MagVector.X;
                 YMax += MagVector.Y;
                 ZMax += MagVector.Z;
+        }
+        MagConfig.cra = cra_rate|CRA_MODE_NEGBIAS;
-        // activate negative bias field of 0.55 gauss
+        // activate positive bias field
-        HMC5843_SetConfiguration(HMC5843_CRA_RATE_50HZ|HMC5843_CRA_MODE_NEGBIAS, HMC5843_CRB_GAIN_10GA, HMC5843_MODE_CONTINUOUS);
+        NCMAG_SetMagConfig();
     // wait for stable readings
         time = SetDelay(50);
         while(!CheckDelay(time));
         // averaging
-        for(i = 0; i < 20; i++)
+        for(i = 0; i < AVERAGE; i++)
+        {
-                HMC5843_GetMagVector();
+                NCMAG_GetMagVector();
                 time = SetDelay(20);
         while(!CheckDelay(time));
                 XMin += MagVector.X;
                 YMin += MagVector.Y;
                 ZMin += MagVector.Z;
+        }
         // setup final configuration
-        HMC5843_SetConfiguration(HMC5843_CRA_RATE_50HZ|HMC5843_CRA_MODE_NORMAL, HMC5843_CRB_GAIN_10GA, HMC5843_MODE_CONTINUOUS);
+        MagConfig.cra = cra_rate|CRA_MODE_NORMAL;
+        // activate positive bias field
+        NCMAG_SetMagConfig();
         // prepare scale limits
-        scale = 715;  // 1300 counts/Gauss * 0.55 Gauss = 715 counts
-        scale_min = (scale *  90)/100;
+        LIMITS(xscale, scale_min, scale_max);
-        scale_max = (scale * 110)/100;
         // check scale for all axes
-        scale = (XMax - XMin)/40;
+        xscale = (XMax - XMin)/(2*AVERAGE);
-        if((scale > scale_max) || (scale < scale_min)) return(0);
+        if((xscale > scale_max) || (xscale < scale_min)) retval = 0;
+        LIMITS(yscale, scale_min, scale_max);
-        scale = (YMax - YMin)/40;
+        yscale = (YMax - YMin)/(2*AVERAGE);
-        if((scale > scale_max) || (scale < scale_min)) return(0);
+        if((yscale > scale_max) || (yscale < scale_min)) retval = 0;
+        LIMITS(zscale, scale_min, scale_max);
-        scale = (ZMax - ZMin)/40;
+        zscale = (ZMax - ZMin)/(2*AVERAGE);
-        if((scale > scale_max) || (scale < scale_min)) return(0);
+        if((zscale > scale_max) || (zscale < scale_min)) retval = 0;
-        return(1);
+        return(retval);
+}
 //----------------------------------------------------------------
-u8 HMC5843_Init(void)
+u8 NCMAG_Init(void)
+{
         u8 msg[64];
         u8 retval = 0;
         u8 repeat;
+        NCMAG_Present = 0;
+        NCMAG_MagType = MAG_TYPE_HMC5843;       // assuming having an HMC5843
+        // polling for LSM302DLH option
+        repeat = 0;
-        HMC5843_Present = 0;
+        do
+        {
+                retval = NCMAG_GetAccConfig();
+                if(retval) break; // break loop on success
+                UART1_PutString(".");
+                repeat++;
+        }while(repeat < 3);
         if(retval) NCMAG_MagType = MAG_TYPE_LSM303DLH; // must be a LSM303DLH
         // polling of identification
         repeat = 0;
         do
+        {
-                retval = HMC5843_GetIdentification();
+                retval = NCMAG_GetIdentification();
                 if(retval) break; // break loop on success
                 UART1_PutString(".");
                 repeat++;
         }while(repeat < 12);
-        // if we got it
+        // if we got an answer to id request
         if(retval)
+        {
+                u8 n1[] = "HMC5843";
+                u8 n2[] = "LSM303DLH";
+                u8* pn;
+                if(NCMAG_MagType == MAG_TYPE_LSM303DLH) pn = n2;
+                else pn = n1;
-                sprintf(msg, " HMC5843 ID %d/%d/%d", HMC5843_Identification.A, HMC5843_Identification.B, HMC5843_Identification.C);
+                sprintf(msg, " %s ID%d/%d/%d", pn, NCMAG_Identification.A, NCMAG_Identification.B, NCMAG_Identification.C);
                 UART1_PutString(msg);
-                if (    (HMC5843_Identification.A == HMC5843_IDA)
+                if (    (NCMAG_Identification.A == MAG_IDA)
-                     && (HMC5843_Identification.B == HMC5843_IDB)
+                     && (NCMAG_Identification.B == MAG_IDB)
-                         && (HMC5843_Identification.C == HMC5843_IDC))
+                         && (NCMAG_Identification.C == MAG_IDC))
+                {
-                        //HMC5843_SetConfiguration(HMC5843_CRA_RATE_50HZ|HMC5843_CRA_MODE_NORMAL, HMC5843_CRB_GAIN_10GA, HMC5843_MODE_CONTINUOUS);
-                        if(!HMC5843_SelfTest())
+                        if(!NCMAG_SelfTest())
+                        {
-                                UART1_PutString("\n\r HMC5843 selftest failed!");
+                                UART1_PutString(" Selftest failed!");
                                 LED_RED_ON;
+                        }
-                        else HMC5843_Present = 1;
+                        else NCMAG_Present = 1;
+                }
                 else
+                {
-                        UART1_PutString("\n\r HMC5843 not compatible!");
+                        UART1_PutString("\n\r not compatible!");
                         LED_RED_ON;
+                }
+        }
-        // else no answer
+        else // nothing found
+        {
+                NCMAG_MagType = MAG_TYPE_NONE;
+                UART1_PutString("not found!");
+        }
-        return(HMC5843_Present);
+        return(NCMAG_Present);
+}

Subversion Repositories NaviCtrl

(root)/tags/V2.10b/ncmag.c @ 811 - Rev 252 → 253