WebSVN - NaviCtrl - Diff - Rev 242 and 243

 /*#######################################################################################*/
 /* !!! 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 "i2c.h"
 #include "timer1.h"
 #include "led.h"
 #include "spi_slave.h"
 #include "uart1.h"
 #include "compass.h"
 #define HMC5843_SLAVE_ADDRESS           0x3C  // i2C slave address
 u8 HMC5843_Present = 0;
 #define REGISTER_CRA            0
 #define REGISTER_CRB            1
 #define REGISTER_MODE           2
 #define REGISTER_DATAX_MSB      3
 #define REGISTER_DATAX_LSB      4
 #define REGISTER_DATAY_MSB      5
 #define REGISTER_DATAY_LSB      6
 #define REGISTER_DATAZ_MSB      7
 #define REGISTER_DATAZ_LSB      8
 #define REGISTER_STATUS         9
 #define REGISTER_IDA            10
 #define REGISTER_IDB            11
 #define REGISTER_IDC            12
 #define HMC5843_IDA 0x48
 #define HMC5843_IDB 0x34
 #define HMC5843_IDC     0x33
 // bit mask for rate
 #define HMC5843_CRA_RATE_MASK           0x1C
 #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
 // 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 B;
   u8 C;
 } __attribute__((packed)) HMC5843_Identification_t;
 volatile HMC5843_Identification_t HMC5843_Identification;
 // ---------- call back handlers -----------------------------------------
 // rx data handler for id info request
 void HMC5843_UpdateIdentification(u8* pRxBuffer, u8 RxBufferSize)
 {       // if number of byte are matching
         if(RxBufferSize == sizeof(HMC5843_Identification) )
+        {
                 memcpy((u8 *)&HMC5843_Identification, pRxBuffer, sizeof(HMC5843_Identification));
+        }
+}
 // rx data handler for mag vector request
 void HMC5843_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.
+}
 // ----------------------------------------------------------------------------------------
 void HMC5843_GetIdentification(void)
+{
         if(I2C_State == I2C_STATE_IDLE)
+        {
                 I2C_TxBufferSize = 0;
                 // set register pointer
                 I2C_TxBuffer[I2C_TxBufferSize++] = REGISTER_IDA;
                 // initiate transmission
                 I2C_Transmission(HMC5843_SLAVE_ADDRESS, &HMC5843_UpdateIdentification, sizeof(HMC5843_Identification));
+        }
+}
 // ----------------------------------------------------------------------------------------
 u8 HMC5843_SetConfiguration(u8 cra, u8 crb, u8 mode)
+{
         u32 timeout = SetDelay(100);
         // wait for free I2C bus
         while(I2C_State != I2C_STATE_IDLE)
+        {
                 if(CheckDelay(timeout)) return 0;
+        }
         I2C_TxBufferSize = 0;
         I2C_TxBuffer[I2C_TxBufferSize++] = REGISTER_CRA;
         I2C_TxBuffer[I2C_TxBufferSize++] = cra;
         I2C_TxBuffer[I2C_TxBufferSize++] = crb;
         I2C_TxBuffer[I2C_TxBufferSize++] = mode;
         // initiate transmission
         I2C_Transmission(HMC5843_SLAVE_ADDRESS, NULL, 0);
         return 1;
+}
 //----------------------------------------------------------------
+void HMC5843_GetMagVector(void)
+{
+        if(I2C_State == I2C_STATE_IDLE)
+        {
+                I2C_TxBufferSize = 0;
+                // set register pointer
+                I2C_TxBuffer[I2C_TxBufferSize++] = REGISTER_DATAX_MSB;
+                // initiate transmission
+                I2C_Transmission(HMC5843_SLAVE_ADDRESS, &HMC5843_UpdateMagVector, sizeof(MagVector));
+        }
+}
+void HMC5843_UpdateCompass(void)
+{
+        static u32 TimerCompassUpdate = 0;
+        if( (I2C_State == I2C_STATE_OFF) || !HMC5843_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();
+                }
+                TimerCompassUpdate = SetDelay(20);    // every 20 ms are 50 Hz
+        }
+}
+u8 HMC5843_SelfTest(void)
+{
+        u32 time;
+        s32 XMin = 0, XMax = 0, YMin = 0, YMax = 0, ZMin = 0, ZMax = 0;
+        s32 scale, scale_min, scale_max;
+        u8 i = 0;
+        // activate positive bias field  of 0.55 gauss
+        HMC5843_SetConfiguration(HMC5843_CRA_RATE_50HZ|HMC5843_CRA_MODE_POSBIAS, HMC5843_CRB_GAIN_10GA, HMC5843_MODE_CONTINUOUS);
+        // wait a while
+        time = SetDelay(55);
+        while(!CheckDelay(time));
+        // averaging
+        for(i = 0; i<20; i++)
+        {
+                HMC5843_GetMagVector();
+                time = SetDelay(20);
+        while(!CheckDelay(time));
+                XMax += MagVector.X;
+                YMax += MagVector.Y;
+                ZMax += MagVector.Z;
+        }
+        // activate negative bias field of 0.55 gauss
+        HMC5843_SetConfiguration(HMC5843_CRA_RATE_50HZ|HMC5843_CRA_MODE_NEGBIAS, HMC5843_CRB_GAIN_10GA, HMC5843_MODE_CONTINUOUS);
+        // wait a while
+        time = SetDelay(55);
+    while(!CheckDelay(time));
+        // averaging
+        for(i = 0; i < 20; i++)
+        {
+                HMC5843_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);
+        // prepare scale limits
+        scale = 715;  // 1300 counts/Gauss * 0.55 Gauss = 715 counts
+        scale_min = (scale * 90)/100;
+        scale_max = (scale * 110)/100;
+        // check scale for all axes
+        scale = (XMax - XMin)/40;
+        if((scale > scale_max) && (scale < scale_min)) return 0;
+        scale = (YMax + YMin)/40;
+        if((scale > scale_max) && (scale < scale_min)) return 0;
+        scale = (ZMax + ZMin)/40;
+        if((scale > scale_max) && (scale < scale_min)) return 0;
+        return 1;
+}
+//----------------------------------------------------------------
 u8 HMC5843_Init(void)
+{
         u8 msg[64];
         u8 repeat;
         u32 timeout;
         HMC5843_Present = 0;
         HMC5843_Identification.A = 0xFF;
         HMC5843_Identification.B = 0xFF;
         HMC5843_Identification.C = 0xFF;
         // polling of identification
         repeat = 0;
         do
+        {
                 HMC5843_GetIdentification();
                 timeout = SetDelay(250);
                 do
+                {
                         if (HMC5843_Identification.A != 0xFF) break; // break loop on success
                 }while (!CheckDelay(timeout));
                 UART1_PutString(".");
                 repeat++;
         }while ((HMC5843_Identification.A == 0xFF) && (repeat < 12)); // 12*250ms=3s
         // if we got it
         if(HMC5843_Identification.A != 0xFF)
+        {
-                sprintf(msg, " HMC5843 ID %d/%d/%d.", HMC5843_Identification.A, HMC5843_Identification.B, HMC5843_Identification.C);
+                sprintf(msg, " HMC5843 ID %d/%d/%d", HMC5843_Identification.A, HMC5843_Identification.B, HMC5843_Identification.C);
                 UART1_PutString(msg);
                 if (    (HMC5843_Identification.A == HMC5843_IDA)
                      && (HMC5843_Identification.B == HMC5843_IDB)
                          && (HMC5843_Identification.C == HMC5843_IDC))
+                {
-                        HMC5843_SetConfiguration(HMC5843_CRA_RATE_50HZ|HMC5843_CRA_MODE_NORMAL, HMC5843_CRB_GAIN_10GA, HMC5843_MODE_CONTINUOUS);
+                        //HMC5843_SetConfiguration(HMC5843_CRA_RATE_50HZ|HMC5843_CRA_MODE_NORMAL, HMC5843_CRB_GAIN_10GA, HMC5843_MODE_CONTINUOUS);
+                        if(!HMC5843_SelfTest())
+                        {
+                                UART1_PutString("\n\r HMC5843 selftest failed!");
+                                LED_RED_ON;
+                        }
-                        HMC5843_Present = 1;
+                        else HMC5843_Present = 1;
+                }
                 else
+                {
                         UART1_PutString("\n\r HMC5843 not compatible!");
                         LED_RED_ON;
+                }
+        }
         return(HMC5843_Present);
+}
-//----------------------------------------------------------------
-void HMC5843_GetMagVector(void)
-{
-        if(I2C_State == I2C_STATE_IDLE)
-        {
-                I2C_TxBufferSize = 0;
-                // set register pointer
-                I2C_TxBuffer[I2C_TxBufferSize++] = REGISTER_DATAX_MSB;
-                // initiate transmission
-                I2C_Transmission(HMC5843_SLAVE_ADDRESS, &HMC5843_UpdateMagVector, sizeof(MagVector));
-        }
-}
-void HMC5843_UpdateCompass(void)
-{
-        static u32 TimerCompassUpdate = 0;
-        if( (I2C_State == I2C_STATE_OFF) || !HMC5843_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();
-                }
-                TimerCompassUpdate = SetDelay(20);    // every 20 ms are 50 Hz
-        }

Subversion Repositories NaviCtrl

(root)/tags/V2.08a/ncmag.c @ 935 - Rev 242 → 243