WebSVN - NaviCtrl - Diff - Rev 339 and 342


/*#######################################################################################*/
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
/*#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 2008 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 <stdio.h>
#include <string.h>
#include "91x_lib.h"
#include "mk3mag.h"
#include "i2c.h"
#include "timer1.h"
#include "led.h"
#include "main.h"
#include "uart1.h"
#include "compass.h"
 
#define MK3MAG_SLAVE_ADDRESS            0x50  // i2C slave address
 
u8 MK3MAG_Present = 0;
 
typedef struct
{
  s16 Nick;
  s16 Roll;
} __attribute__((packed)) MK3MAG_WriteAttitude_t;
 
typedef struct
{
  u8 Major;
  u8 Minor;
  u8 Patch;
  u8 Compatible;
} __attribute__((packed)) MK3MAG_Version_t;
 
typedef struct
{
  u8 CalByte;
  u8 Dummy1;
  u8 Dummy2;
} __attribute__((packed)) MK3MAG_Cal_t;
 
// Transfer buffers
volatile MK3MAG_WriteAttitude_t         MK3MAG_WriteAttitude;
volatile MK3MAG_Version_t                       MK3MAG_Version;
volatile MK3MAG_Cal_t                           MK3MAG_WriteCal;
volatile MK3MAG_Cal_t                           MK3MAG_ReadCal;
 
// -------------------------------------------------------------------------------
// the calculation of the MK3MAG packet checksum
 
// calculate the crc of  bytecount bytes in buffer
u8 MK3MAG_CalcCRC(u8* pBuffer, u8 bytecount)
{
        u8 i, crc = 0;
        for(i=0; i < bytecount; i++)
        {
                crc += pBuffer[i];
        }
        crc = ~crc;
        return crc;
}
 
// assuming the last byte in buffer is the crc byte
u8 MK3MAG_CheckCRC(u8* pBuffer, u8 BuffLen)
{
        u8 crc = 0, retval = 0;
        if(BuffLen == 0) return(retval);
        crc = MK3MAG_CalcCRC(pBuffer, BuffLen-1);
        if(crc == pBuffer[BuffLen-1])
        {
                retval = 1;
                DebugOut.Analog[15]++; // count mk3mag ok
        }
        else
        {
                retval = 0;
                DebugOut.Analog[14]++; // count mk3mag crc error
        }
        return(retval);
}
 
// -------------------------------------------------------------------------------
// the I2C rx data handler functions
 
// rx data handler for version info request
void MK3MAG_UpdateVersion(u8* pRxBuffer, u8 RxBufferSize)
{       // if crc is ok and number of byte are matching
        if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MK3MAG_Version)+1)) )
        {
                memcpy((u8 *)&MK3MAG_Version, pRxBuffer, sizeof(MK3MAG_Version));
        }                
}
 
// rx data handler for calibration request
void MK3MAG_UpdateCalibration(u8* pRxBuffer, u8 RxBufferSize)
{       // if crc is ok and number of byte are matching
        if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MK3MAG_ReadCal)+1)) )
        {
                memcpy((u8 *)&MK3MAG_ReadCal, pRxBuffer, sizeof(MK3MAG_ReadCal));
        }                        
}
 
// rx data handler for magnetic vector request
void MK3MAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)
{       // if crc is ok and number of byte are matching
        if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MagVector)+1)) )
        {
                s16vec_t *pMagVector = (s16vec_t*)pRxBuffer;
                MagVector.X = pMagVector->Y;
                MagVector.Y = pMagVector->X;
                MagVector.Z = -pMagVector->Z;    
                Compass_CalcHeading();
        }                        
}
 
//----------------------------------------------------------------
#define MK3MAG_CMD_VERSION              0x01
#define MK3MAG_CMD_READ_MAGVECT         0x02
#define MK3MAG_CMD_WRITE_CAL            0x04
 
void MK3MAG_SendCommand(u8 command)
{
        // try to catch the I2C buffer
        if(I2C_LockBuffer(0))
        {
                u16 TxBytes = 0;
                u16 RxBytes = 0;
                I2C_pRxHandler_t pRxHandlerFunc = NULL;
               
                // update current command id
                I2C_Buffer[TxBytes++] = command;
 
                // set pointers to data area with respect to the command id
                switch (command)
                {
                        case MK3MAG_CMD_VERSION:
                                RxBytes = sizeof(MK3MAG_Version)+1;
                                pRxHandlerFunc = &MK3MAG_UpdateVersion;
                                break;
                        case MK3MAG_CMD_WRITE_CAL:
                                RxBytes = sizeof(MK3MAG_ReadCal)+1;
                                pRxHandlerFunc = &MK3MAG_UpdateCalibration;
                                memcpy((u8*)I2C_Buffer+1, (u8*)&MK3MAG_WriteCal, sizeof(MK3MAG_WriteCal));
                                TxBytes += sizeof(MK3MAG_WriteCal);
                                break;
                        case MK3MAG_CMD_READ_MAGVECT:
                                RxBytes = sizeof(MagVector)+1;
                                pRxHandlerFunc = &MK3MAG_UpdateMagVector;
                                break;
                        default: // unknown command id
                                RxBytes = 0;
                                pRxHandlerFunc = NULL;
                                break;
                }
                // update packet checksum
                I2C_Buffer[TxBytes] = MK3MAG_CalcCRC((u8*)I2C_Buffer, TxBytes);
                TxBytes++;
                // initiate I2C transmission
                I2C_Transmission(MK3MAG_SLAVE_ADDRESS, TxBytes, pRxHandlerFunc, RxBytes);
        } // EOF I2C_State == I2C_IDLE  
}
 
 
//----------------------------------------------------------------
u8 MK3MAG_Init(void)
{
        u8 msg[64];
        u8 repeat;
        u32 timeout;
       
        MK3MAG_Present = 0;
 
        MK3MAG_Version.Major = 0xFF;
        MK3MAG_Version.Minor = 0xFF;
        MK3MAG_Version.Patch = 0xFF;
        MK3MAG_Version.Compatible = 0xFF;
 
        Compass_Heading = -1;
 
        // polling of version info
        repeat = 0;
        do
        {
                MK3MAG_SendCommand(MK3MAG_CMD_VERSION);
                if(Version_HW > 11) timeout = SetDelay(100);
                else timeout = SetDelay(250);
 
                do
                {
                        if (MK3MAG_Version.Major != 0xFF) break; // break loop on success
                }while (!CheckDelay(timeout));
                UART1_PutString(".");
                repeat++;
        }while ((MK3MAG_Version.Major == 0xFF) && (repeat < 12)); // 12*250ms=3s
        // if we got it
        if (MK3MAG_Version.Major != 0xFF)
        {
                sprintf(msg, " MK3MAG V%d.%d%c", MK3MAG_Version.Major, MK3MAG_Version.Minor, 'a' + MK3MAG_Version.Patch);
                UART1_PutString(msg);
                if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)
                {
                        UART1_PutString("\n\r MK3MAG not compatible!");
                        UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE;
                        LED_RED_ON;
                }
                else
                {       // version ok
                        MK3MAG_Present = 1;
                }
        }
        return(MK3MAG_Present);
}
 
//----------------------------------------------------------------
void MK3MAG_Update(void)
{
        static u32 TimerUpdate = 0;
        static s16 x_max,y_max,z_max,x_min,y_min,z_min;
        static u8 last_state;
        u8 msg[64];
        u16 MinCaclibration = 500;
 
        if( (I2C_State == I2C_STATE_OFF) || !MK3MAG_Present ) return;
       
        if(CheckDelay(TimerUpdate))
        {
                // check for incomming compass calibration request
                Compass_UpdateCalState();
                MK3MAG_WriteCal.CalByte = Compass_CalState;
       
                if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte)
                {       // send new calibration state
                        MK3MAG_SendCommand(MK3MAG_CMD_WRITE_CAL);
                }
                else
                {       // calibration state matches
                        MK3MAG_SendCommand(MK3MAG_CMD_READ_MAGVECT); // initiate magvector transfer
               
                        switch(Compass_CalState)
                        {
                                case 1:
                                    if(last_state != Compass_CalState)
                                         {
                                          UART1_PutString("\r\nMK3Mag calibration\n\r");
                                      if(EarthMagneticStrengthTheoretic)
                                                {
                                                  MinCaclibration = (MinCaclibration * EarthMagneticStrengthTheoretic) / 50;
                                                  sprintf(msg, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic);
                                                  UART1_PutString(msg);
                                                 }
                                           else UART1_PutString("without GPS\n\r");
                                         }
 
                                        x_max = -30000; y_max = -30000; z_max = -30000;
                                        x_min = 30000; y_min = 30000; z_min = 30000;
                                        break;
                               
                                case 2:
                                                 if(MagVector.X > x_max) { x_max = MagVector.X; BeepTime = 60; }
                                        else if(MagVector.X < x_min) { x_min = MagVector.X; BeepTime = 20; }
                                                 if(MagVector.Y > y_max) { y_max = MagVector.Y; BeepTime = 60; }
                                        else if(MagVector.Y < y_min) { y_min = MagVector.Y; BeepTime = 20; }
                                        break;
                               
                                case 4:
                                                 if(MagVector.Z > z_max) { z_max = MagVector.Z; BeepTime = 80; }
                                        else if(MagVector.Z < z_min) { z_min = MagVector.Z; BeepTime = 80; }
                                        break;
                                case 5:
                                    if(last_state == Compass_CalState) break;
                                        if(((x_max - x_min) > MinCaclibration) && ((y_max - y_min) > MinCaclibration) && ((z_max - z_min) > MinCaclibration))
                                        {
                                        BeepTime = 2500;
                                        UART1_PutString("\r\n-> Calibration okay <-\n\r");
                                        }
                                        else
                                        {
                                        UART1_PutString("\r\nCalibration FAILED - Values too low: ");
                                    if((x_max - x_min) < MinCaclibration) UART1_PutString("X! ");
                                    if((y_max - y_min) < MinCaclibration) UART1_PutString("Y! ");
                                    if((z_max - z_min) < MinCaclibration) UART1_PutString("Z! ");
                                        UART1_PutString("\r\n");
 
                                        }
                                        UART1_PutString(msg);
                                        sprintf(msg, "\r\nX: (%i - %i = %i)\r\n",x_max,x_min,x_max - x_min);
                                        UART1_PutString(msg);
                                        sprintf(msg, "Y: (%i - %i = %i)\r\n",y_max,y_min,y_max - y_min);
                                        UART1_PutString(msg);
                                        sprintf(msg, "Z: (%i - %i = %i)\r\n",z_max,z_min,z_max - z_min);
                                        UART1_PutString(msg);
 
                                        sprintf(msg, "(Minimum ampilitude is: %i)\r\n",MinCaclibration);
                                        UART1_PutString(msg);
                                        break;
                                default:
                                        break;
                        }
                        last_state = Compass_CalState;
                }
                TimerUpdate = SetDelay(20);    // every 20 ms are 50 Hz
        }
}
 

Rev 339	Rev 342
1	/#######################################################################################/	1	/#######################################################################################/
2	/* !!! THIS IS NOT FREE SOFTWARE !!! */	2	/* !!! THIS IS NOT FREE SOFTWARE !!! */
3	/#######################################################################################/	3	/#######################################################################################/
4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5	// + Copyright (c) 2008 Ingo Busker, Holger Buss	5	// + Copyright (c) 2008 Ingo Busker, Holger Buss
6	// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY	6	// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
7	// + FOR NON COMMERCIAL USE ONLY	7	// + FOR NON COMMERCIAL USE ONLY
8	// + www.MikroKopter.com	8	// + www.MikroKopter.com
9	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	9	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
10	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),	10	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
11	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.	11	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
12	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt	12	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
13	// + bzgl. der Nutzungsbedingungen aufzunehmen.	13	// + bzgl. der Nutzungsbedingungen aufzunehmen.
14	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,	14	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
15	// + Verkauf von Luftbildaufnahmen, usw.	15	// + Verkauf von Luftbildaufnahmen, usw.
16	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	16	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
17	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,	17	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
18	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen	18	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts	20	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
21	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"	21	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
22	// + eindeutig als Ursprung verlinkt werden	22	// + eindeutig als Ursprung verlinkt werden
23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion	24	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
25	// + Benutzung auf eigene Gefahr	25	// + Benutzung auf eigene Gefahr
26	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden	26	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
27	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	27	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
28	// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur	28	// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
29	// + mit unserer Zustimmung zulässig	29	// + mit unserer Zustimmung zulässig
30	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	30	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen	31	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
32	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	32	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
33	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,	33	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
34	// + this list of conditions and the following disclaimer.	34	// + this list of conditions and the following disclaimer.
35	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived	35	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
36	// + from this software without specific prior written permission.	36	// + from this software without specific prior written permission.
37	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted	37	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
38	// + for non-commercial use (directly or indirectly)	38	// + for non-commercial use (directly or indirectly)
39	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted	39	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
40	// + with our written permission	40	// + with our written permission
41	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be	41	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
42	// + clearly linked as origin	42	// + clearly linked as origin
43	// + * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed	43	// + * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
44	//	44	//
45	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"	45	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
46	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE	46	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
47	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE	47	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
48	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE	48	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
49	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR	49	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
50	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF	50	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
51	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS	51	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
52	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN	52	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
53	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)	53	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
54	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE	54	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
55	// + POSSIBILITY OF SUCH DAMAGE.	55	// + POSSIBILITY OF SUCH DAMAGE.
56	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	56	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
57	#include <stdio.h>	57	#include <stdio.h>
58	#include <string.h>	58	#include <string.h>
59	#include "91x_lib.h"	59	#include "91x_lib.h"
60	#include "mk3mag.h"	60	#include "mk3mag.h"
61	#include "i2c.h"	61	#include "i2c.h"
62	#include "timer1.h"	62	#include "timer1.h"
63	#include "led.h"	63	#include "led.h"
64	#include "main.h"	64	#include "main.h"
65	#include "uart1.h"	65	#include "uart1.h"
66	#include "compass.h"	66	#include "compass.h"
67		67
68	#define MK3MAG_SLAVE_ADDRESS 0x50 // i2C slave address	68	#define MK3MAG_SLAVE_ADDRESS 0x50 // i2C slave address
69		69
70	u8 MK3MAG_Present = 0;	70	u8 MK3MAG_Present = 0;
71		71
72	typedef struct	72	typedef struct
73	{	73	{
74	s16 Nick;	74	s16 Nick;
75	s16 Roll;	75	s16 Roll;
76	} __attribute__((packed)) MK3MAG_WriteAttitude_t;	76	} __attribute__((packed)) MK3MAG_WriteAttitude_t;
77		77
78	typedef struct	78	typedef struct
79	{	79	{
80	u8 Major;	80	u8 Major;
81	u8 Minor;	81	u8 Minor;
82	u8 Patch;	82	u8 Patch;
83	u8 Compatible;	83	u8 Compatible;
84	} __attribute__((packed)) MK3MAG_Version_t;	84	} __attribute__((packed)) MK3MAG_Version_t;
85		85
86	typedef struct	86	typedef struct
87	{	87	{
88	u8 CalByte;	88	u8 CalByte;
89	u8 Dummy1;	89	u8 Dummy1;
90	u8 Dummy2;	90	u8 Dummy2;
91	} __attribute__((packed)) MK3MAG_Cal_t;	91	} __attribute__((packed)) MK3MAG_Cal_t;
92		92
93	// Transfer buffers	93	// Transfer buffers
94	volatile MK3MAG_WriteAttitude_t MK3MAG_WriteAttitude;	94	volatile MK3MAG_WriteAttitude_t MK3MAG_WriteAttitude;
95	volatile MK3MAG_Version_t MK3MAG_Version;	95	volatile MK3MAG_Version_t MK3MAG_Version;
96	volatile MK3MAG_Cal_t MK3MAG_WriteCal;	96	volatile MK3MAG_Cal_t MK3MAG_WriteCal;
97	volatile MK3MAG_Cal_t MK3MAG_ReadCal;	97	volatile MK3MAG_Cal_t MK3MAG_ReadCal;
98		98
99	// -------------------------------------------------------------------------------	99	// -------------------------------------------------------------------------------
100	// the calculation of the MK3MAG packet checksum	100	// the calculation of the MK3MAG packet checksum
101		101
102	// calculate the crc of bytecount bytes in buffer	102	// calculate the crc of bytecount bytes in buffer
103	u8 MK3MAG_CalcCRC(u8* pBuffer, u8 bytecount)	103	u8 MK3MAG_CalcCRC(u8* pBuffer, u8 bytecount)
104	{	104	{
105	u8 i, crc = 0;	105	u8 i, crc = 0;
106	for(i=0; i < bytecount; i++)	106	for(i=0; i < bytecount; i++)
107	{	107	{
108	crc += pBuffer[i];	108	crc += pBuffer[i];
109	}	109	}
110	crc = ~crc;	110	crc = ~crc;
111	return crc;	111	return crc;
112	}	112	}
113		113
114	// assuming the last byte in buffer is the crc byte	114	// assuming the last byte in buffer is the crc byte
115	u8 MK3MAG_CheckCRC(u8* pBuffer, u8 BuffLen)	115	u8 MK3MAG_CheckCRC(u8* pBuffer, u8 BuffLen)
116	{	116	{
117	u8 crc = 0, retval = 0;	117	u8 crc = 0, retval = 0;
118	if(BuffLen == 0) return(retval);	118	if(BuffLen == 0) return(retval);
119	crc = MK3MAG_CalcCRC(pBuffer, BuffLen-1);	119	crc = MK3MAG_CalcCRC(pBuffer, BuffLen-1);
120	if(crc == pBuffer[BuffLen-1])	120	if(crc == pBuffer[BuffLen-1])
121	{	121	{
122	retval = 1;	122	retval = 1;
123	DebugOut.Analog[15]++; // count mk3mag ok	123	DebugOut.Analog[15]++; // count mk3mag ok
124	}	124	}
125	else	125	else
126	{	126	{
127	retval = 0;	127	retval = 0;
128	DebugOut.Analog[14]++; // count mk3mag crc error	128	DebugOut.Analog[14]++; // count mk3mag crc error
129	}	129	}
130	return(retval);	130	return(retval);
131	}	131	}
132		132
133	// -------------------------------------------------------------------------------	133	// -------------------------------------------------------------------------------
134	// the I2C rx data handler functions	134	// the I2C rx data handler functions
135		135
136	// rx data handler for version info request	136	// rx data handler for version info request
137	void MK3MAG_UpdateVersion(u8* pRxBuffer, u8 RxBufferSize)	137	void MK3MAG_UpdateVersion(u8* pRxBuffer, u8 RxBufferSize)
138	{ // if crc is ok and number of byte are matching	138	{ // if crc is ok and number of byte are matching
139	if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MK3MAG_Version)+1)) )	139	if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MK3MAG_Version)+1)) )
140	{	140	{
141	memcpy((u8 *)&MK3MAG_Version, pRxBuffer, sizeof(MK3MAG_Version));	141	memcpy((u8 *)&MK3MAG_Version, pRxBuffer, sizeof(MK3MAG_Version));
142	}	142	}
143	}	143	}
144		144
145	// rx data handler for calibration request	145	// rx data handler for calibration request
146	void MK3MAG_UpdateCalibration(u8* pRxBuffer, u8 RxBufferSize)	146	void MK3MAG_UpdateCalibration(u8* pRxBuffer, u8 RxBufferSize)
147	{ // if crc is ok and number of byte are matching	147	{ // if crc is ok and number of byte are matching
148	if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MK3MAG_ReadCal)+1)) )	148	if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MK3MAG_ReadCal)+1)) )
149	{	149	{
150	memcpy((u8 *)&MK3MAG_ReadCal, pRxBuffer, sizeof(MK3MAG_ReadCal));	150	memcpy((u8 *)&MK3MAG_ReadCal, pRxBuffer, sizeof(MK3MAG_ReadCal));
151	}	151	}
152	}	152	}
153		153
154	// rx data handler for magnetic vector request	154	// rx data handler for magnetic vector request
155	void MK3MAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)	155	void MK3MAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)
156	{ // if crc is ok and number of byte are matching	156	{ // if crc is ok and number of byte are matching
157	if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MagVector)+1)) )	157	if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MagVector)+1)) )
158	{	158	{
159	s16vec_t pMagVector = (s16vec_t)pRxBuffer;	159	s16vec_t pMagVector = (s16vec_t)pRxBuffer;
160	MagVector.X = pMagVector->Y;	160	MagVector.X = pMagVector->Y;
161	MagVector.Y = pMagVector->X;	161	MagVector.Y = pMagVector->X;
162	MagVector.Z = -pMagVector->Z;	162	MagVector.Z = -pMagVector->Z;
163	Compass_CalcHeading();	163	Compass_CalcHeading();
164	}	164	}
165	}	165	}
166		166
167	//----------------------------------------------------------------	167	//----------------------------------------------------------------
168	#define MK3MAG_CMD_VERSION 0x01	168	#define MK3MAG_CMD_VERSION 0x01
169	#define MK3MAG_CMD_READ_MAGVECT 0x02	169	#define MK3MAG_CMD_READ_MAGVECT 0x02
170	#define MK3MAG_CMD_WRITE_CAL 0x04	170	#define MK3MAG_CMD_WRITE_CAL 0x04
171		171
172	void MK3MAG_SendCommand(u8 command)	172	void MK3MAG_SendCommand(u8 command)
173	{	173	{
174	// try to catch the I2C buffer	174	// try to catch the I2C buffer
175	if(I2C_LockBuffer(0))	175	if(I2C_LockBuffer(0))
176	{	176	{
177	u16 TxBytes = 0;	177	u16 TxBytes = 0;
178	u16 RxBytes = 0;	178	u16 RxBytes = 0;
179	I2C_pRxHandler_t pRxHandlerFunc = NULL;	179	I2C_pRxHandler_t pRxHandlerFunc = NULL;
180		180
181	// update current command id	181	// update current command id
182	I2C_Buffer[TxBytes++] = command;	182	I2C_Buffer[TxBytes++] = command;
183		183
184	// set pointers to data area with respect to the command id	184	// set pointers to data area with respect to the command id
185	switch (command)	185	switch (command)
186	{	186	{
187	case MK3MAG_CMD_VERSION:	187	case MK3MAG_CMD_VERSION:
188	RxBytes = sizeof(MK3MAG_Version)+1;	188	RxBytes = sizeof(MK3MAG_Version)+1;
189	pRxHandlerFunc = &MK3MAG_UpdateVersion;	189	pRxHandlerFunc = &MK3MAG_UpdateVersion;
190	break;	190	break;
191	case MK3MAG_CMD_WRITE_CAL:	191	case MK3MAG_CMD_WRITE_CAL:
192	RxBytes = sizeof(MK3MAG_ReadCal)+1;	192	RxBytes = sizeof(MK3MAG_ReadCal)+1;
193	pRxHandlerFunc = &MK3MAG_UpdateCalibration;	193	pRxHandlerFunc = &MK3MAG_UpdateCalibration;
194	memcpy((u8)I2C_Buffer+1, (u8)&MK3MAG_WriteCal, sizeof(MK3MAG_WriteCal));	194	memcpy((u8)I2C_Buffer+1, (u8)&MK3MAG_WriteCal, sizeof(MK3MAG_WriteCal));
195	TxBytes += sizeof(MK3MAG_WriteCal);	195	TxBytes += sizeof(MK3MAG_WriteCal);
196	break;	196	break;
197	case MK3MAG_CMD_READ_MAGVECT:	197	case MK3MAG_CMD_READ_MAGVECT:
198	RxBytes = sizeof(MagVector)+1;	198	RxBytes = sizeof(MagVector)+1;
199	pRxHandlerFunc = &MK3MAG_UpdateMagVector;	199	pRxHandlerFunc = &MK3MAG_UpdateMagVector;
200	break;	200	break;
201	default: // unknown command id	201	default: // unknown command id
202	RxBytes = 0;	202	RxBytes = 0;
203	pRxHandlerFunc = NULL;	203	pRxHandlerFunc = NULL;
204	break;	204	break;
205	}	205	}
206	// update packet checksum	206	// update packet checksum
207	I2C_Buffer[TxBytes] = MK3MAG_CalcCRC((u8*)I2C_Buffer, TxBytes);	207	I2C_Buffer[TxBytes] = MK3MAG_CalcCRC((u8*)I2C_Buffer, TxBytes);
208	TxBytes++;	208	TxBytes++;
209	// initiate I2C transmission	209	// initiate I2C transmission
210	I2C_Transmission(MK3MAG_SLAVE_ADDRESS, TxBytes, pRxHandlerFunc, RxBytes);	210	I2C_Transmission(MK3MAG_SLAVE_ADDRESS, TxBytes, pRxHandlerFunc, RxBytes);
211	} // EOF I2C_State == I2C_IDLE	211	} // EOF I2C_State == I2C_IDLE
212	}	212	}
213		213
214		214
215	//----------------------------------------------------------------	215	//----------------------------------------------------------------
216	u8 MK3MAG_Init(void)	216	u8 MK3MAG_Init(void)
217	{	217	{
218	u8 msg[64];	218	u8 msg[64];
219	u8 repeat;	219	u8 repeat;
220	u32 timeout;	220	u32 timeout;
221		221
222	MK3MAG_Present = 0;	222	MK3MAG_Present = 0;
223		223
224	MK3MAG_Version.Major = 0xFF;	224	MK3MAG_Version.Major = 0xFF;
225	MK3MAG_Version.Minor = 0xFF;	225	MK3MAG_Version.Minor = 0xFF;
226	MK3MAG_Version.Patch = 0xFF;	226	MK3MAG_Version.Patch = 0xFF;
227	MK3MAG_Version.Compatible = 0xFF;	227	MK3MAG_Version.Compatible = 0xFF;
228		228
229	Compass_Heading = -1;	229	Compass_Heading = -1;
230		230
231	// polling of version info	231	// polling of version info
232	repeat = 0;	232	repeat = 0;
233	do	233	do
234	{	234	{
235	MK3MAG_SendCommand(MK3MAG_CMD_VERSION);	235	MK3MAG_SendCommand(MK3MAG_CMD_VERSION);
-		236	if(Version_HW > 11) timeout = SetDelay(100);
236	timeout = SetDelay(250);	237	else timeout = SetDelay(250);
-		238
237	do	239	do
238	{	240	{
239	if (MK3MAG_Version.Major != 0xFF) break; // break loop on success	241	if (MK3MAG_Version.Major != 0xFF) break; // break loop on success
240	}while (!CheckDelay(timeout));	242	}while (!CheckDelay(timeout));
241	UART1_PutString(".");	243	UART1_PutString(".");
242	repeat++;	244	repeat++;
243	}while ((MK3MAG_Version.Major == 0xFF) && (repeat < 12)); // 12*250ms=3s	245	}while ((MK3MAG_Version.Major == 0xFF) && (repeat < 12)); // 12*250ms=3s
244	// if we got it	246	// if we got it
245	if (MK3MAG_Version.Major != 0xFF)	247	if (MK3MAG_Version.Major != 0xFF)
246	{	248	{
247	sprintf(msg, " MK3MAG V%d.%d%c", MK3MAG_Version.Major, MK3MAG_Version.Minor, 'a' + MK3MAG_Version.Patch);	249	sprintf(msg, " MK3MAG V%d.%d%c", MK3MAG_Version.Major, MK3MAG_Version.Minor, 'a' + MK3MAG_Version.Patch);
248	UART1_PutString(msg);	250	UART1_PutString(msg);
249	if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)	251	if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)
250	{	252	{
251	UART1_PutString("\n\r MK3MAG not compatible!");	253	UART1_PutString("\n\r MK3MAG not compatible!");
252	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_COMPASS_INCOMPATIBLE;	254	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_COMPASS_INCOMPATIBLE;
253	LED_RED_ON;	255	LED_RED_ON;
254	}	256	}
255	else	257	else
256	{ // version ok	258	{ // version ok
257	MK3MAG_Present = 1;	259	MK3MAG_Present = 1;
258	}	260	}
259	}	261	}
260	return(MK3MAG_Present);	262	return(MK3MAG_Present);
261	}	263	}
262		264
263	//----------------------------------------------------------------	265	//----------------------------------------------------------------
264	void MK3MAG_Update(void)	266	void MK3MAG_Update(void)
265	{	267	{
266	static u32 TimerUpdate = 0;	268	static u32 TimerUpdate = 0;
267	static s16 x_max,y_max,z_max,x_min,y_min,z_min;	269	static s16 x_max,y_max,z_max,x_min,y_min,z_min;
268	static u8 last_state;	270	static u8 last_state;
269	u8 msg[64];	271	u8 msg[64];
270	u16 MinCaclibration = 500;	272	u16 MinCaclibration = 500;
271		273
272	if( (I2C_State == I2C_STATE_OFF) \|\| !MK3MAG_Present ) return;	274	if( (I2C_State == I2C_STATE_OFF) \|\| !MK3MAG_Present ) return;
273		275
274	if(CheckDelay(TimerUpdate))	276	if(CheckDelay(TimerUpdate))
275	{	277	{
276	// check for incomming compass calibration request	278	// check for incomming compass calibration request
277	Compass_UpdateCalState();	279	Compass_UpdateCalState();
278	MK3MAG_WriteCal.CalByte = Compass_CalState;	280	MK3MAG_WriteCal.CalByte = Compass_CalState;
279		281
280	if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte)	282	if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte)
281	{ // send new calibration state	283	{ // send new calibration state
282	MK3MAG_SendCommand(MK3MAG_CMD_WRITE_CAL);	284	MK3MAG_SendCommand(MK3MAG_CMD_WRITE_CAL);
283	}	285	}
284	else	286	else
285	{ // calibration state matches	287	{ // calibration state matches
286	MK3MAG_SendCommand(MK3MAG_CMD_READ_MAGVECT); // initiate magvector transfer	288	MK3MAG_SendCommand(MK3MAG_CMD_READ_MAGVECT); // initiate magvector transfer
287		289
288	switch(Compass_CalState)	290	switch(Compass_CalState)
289	{	291	{
290	case 1:	292	case 1:
291	if(last_state != Compass_CalState)	293	if(last_state != Compass_CalState)
292	{	294	{
293	UART1_PutString("\r\nMK3Mag calibration\n\r");	295	UART1_PutString("\r\nMK3Mag calibration\n\r");
-		296	if(EarthMagneticStrengthTheoretic)
-		297	{
-		298	MinCaclibration = (MinCaclibration * EarthMagneticStrengthTheoretic) / 50;
-		299	sprintf(msg, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic);
-		300	UART1_PutString(msg);
-		301	}
-		302	else UART1_PutString("without GPS\n\r");
294	}	303	}
295		-
296	x_max = -30000; y_max = -30000; z_max = -30000;	304	x_max = -30000; y_max = -30000; z_max = -30000;
297	x_min = 30000; y_min = 30000; z_min = 30000;	305	x_min = 30000; y_min = 30000; z_min = 30000;
298	break;	306	break;
299		307
300	case 2:	308	case 2:
301	if(MagVector.X > x_max) { x_max = MagVector.X; BeepTime = 60; }	309	if(MagVector.X > x_max) { x_max = MagVector.X; BeepTime = 60; }
302	else if(MagVector.X < x_min) { x_min = MagVector.X; BeepTime = 20; }	310	else if(MagVector.X < x_min) { x_min = MagVector.X; BeepTime = 20; }
303	if(MagVector.Y > y_max) { y_max = MagVector.Y; BeepTime = 60; }	311	if(MagVector.Y > y_max) { y_max = MagVector.Y; BeepTime = 60; }
304	else if(MagVector.Y < y_min) { y_min = MagVector.Y; BeepTime = 20; }	312	else if(MagVector.Y < y_min) { y_min = MagVector.Y; BeepTime = 20; }
305	break;	313	break;
306		314
307	case 4:	315	case 4:
308	if(MagVector.Z > z_max) { z_max = MagVector.Z; BeepTime = 80; }	316	if(MagVector.Z > z_max) { z_max = MagVector.Z; BeepTime = 80; }
309	else if(MagVector.Z < z_min) { z_min = MagVector.Z; BeepTime = 80; }	317	else if(MagVector.Z < z_min) { z_min = MagVector.Z; BeepTime = 80; }
310	break;	318	break;
311	case 5:	319	case 5:
312	if(last_state == Compass_CalState) break;	320	if(last_state == Compass_CalState) break;
313	if(((x_max - x_min) > MinCaclibration) && ((y_max - y_min) > MinCaclibration) && ((z_max - z_min) > MinCaclibration))	321	if(((x_max - x_min) > MinCaclibration) && ((y_max - y_min) > MinCaclibration) && ((z_max - z_min) > MinCaclibration))
314	{	322	{
315	BeepTime = 2500;	323	BeepTime = 2500;
316	UART1_PutString("\r\n Calibration okay\n\r");	324	UART1_PutString("\r\n-> Calibration okay <-\n\r");
317	}	325	}
318	else	326	else
319	{	327	{
320	UART1_PutString("\r\nCalibration FAILED - Values too low: ");	328	UART1_PutString("\r\nCalibration FAILED - Values too low: ");
321	if((x_max - x_min) < MinCaclibration) UART1_PutString("X! ");	329	if((x_max - x_min) < MinCaclibration) UART1_PutString("X! ");
322	if((y_max - y_min) < MinCaclibration) UART1_PutString("y! ");	330	if((y_max - y_min) < MinCaclibration) UART1_PutString("Y! ");
323	if((z_max - z_min) < MinCaclibration) UART1_PutString("Z! ");	331	if((z_max - z_min) < MinCaclibration) UART1_PutString("Z! ");
324	UART1_PutString("\r\n");	332	UART1_PutString("\r\n");
325	sprintf(msg, "Minimum is: %i \r\n",MinCaclibration);	-
326	}	333	}
327	UART1_PutString(msg);	334	UART1_PutString(msg);
328	sprintf(msg, "\r\nX: (%i - %i = %i)\r\n",x_max,x_min,x_max - x_min);	335	sprintf(msg, "\r\nX: (%i - %i = %i)\r\n",x_max,x_min,x_max - x_min);
329	UART1_PutString(msg);	336	UART1_PutString(msg);
330	sprintf(msg, "Y: (%i - %i = %i)\r\n",y_max,y_min,y_max - y_min);	337	sprintf(msg, "Y: (%i - %i = %i)\r\n",y_max,y_min,y_max - y_min);
331	UART1_PutString(msg);	338	UART1_PutString(msg);
332	sprintf(msg, "Z: (%i - %i = %i)\r\n",z_max,z_min,z_max - z_min);	339	sprintf(msg, "Z: (%i - %i = %i)\r\n",z_max,z_min,z_max - z_min);
333	UART1_PutString(msg);	340	UART1_PutString(msg);
334		-
-		341	sprintf(msg, "(Minimum ampilitude is: %i)\r\n",MinCaclibration);
-		342	UART1_PutString(msg);
335	break;	343	break;
336	default:	344	default:
337	break;	345	break;
338	}	346	}
339	last_state = Compass_CalState;	347	last_state = Compass_CalState;
340	}	348	}
341	TimerUpdate = SetDelay(20); // every 20 ms are 50 Hz	349	TimerUpdate = SetDelay(20); // every 20 ms are 50 Hz
342	}	350	}
343	}	351	}
344		352

Subversion Repositories NaviCtrl

(root)/tags/V0.26e/mk3mag.c @ 658 - Rev 339 → 342