WebSVN - NaviCtrl - Diff - Rev 464 and 465


/*#######################################################################################*/
/* !!! 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 <stdio.h>
#include <string.h>
#include "91x_lib.h"
#include "mk3mag.h"
#include "i2c.h"
#include "i2c0.h"
#include "timer1.h"
#include "led.h"
#include "main.h"
#include "uart1.h"
#include "compass.h"
#include "spi_slave.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
 
// use I2C1 for communication
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,speak = 0;
        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;
                                        speak = 1;
                                        break;
                                case 2:
                                        if(speak) SpeakHoTT = SPEAK_CALIBRATE; speak = 0;
 
                                                 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 3:
                                        speak = 1;
                                        break;
                                case 4:
                                        if(speak) SpeakHoTT = SPEAK_CALIBRATE;  speak = 0;
                                                 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");
SpeakHoTT = SPEAK_MIKROKOPTER;
                                        }
                                        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");
SpeakHoTT = SPEAK_ERR_CALIBARTION;
                                        }
                                        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 464	Rev 465
1	/#######################################################################################/	1	/#######################################################################################/
2	/* !!! THIS IS NOT FREE SOFTWARE !!! */	2	/* !!! THIS IS NOT FREE SOFTWARE !!! */
3	/#######################################################################################/	3	/#######################################################################################/
4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5	// + www.MikroKopter.com	5	// + www.MikroKopter.com
6	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	6	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
7	// + Software Nutzungsbedingungen (english version: see below)	7	// + Software Nutzungsbedingungen (english version: see below)
8	// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -	8	// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
9	// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den	9	// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
10	// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool	10	// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
11	// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.	11	// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
12	// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.	12	// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
13	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	13	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
14	// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im	14	// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
15	// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.	15	// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
16	// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie	16	// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
17	// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.	17	// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
18	// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren	18	// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
19	// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.	19	// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
20	// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren	20	// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
21	// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand	21	// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
22	// + des Mitverschuldens offen.	22	// + des Mitverschuldens offen.
23	// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.	23	// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
24	// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.	24	// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
25	// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.	25	// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
26	// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang	26	// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
27	// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.	27	// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
28	// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.	28	// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
29	// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'	29	// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
30	// + 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.	30	// + 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.
31	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	31	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
32	// + Software LICENSING TERMS	32	// + Software LICENSING TERMS
33	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	33	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
34	// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -	34	// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
35	// + 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	35	// + 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
36	// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.	36	// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
37	// + The Software may only be used with the Licensor's products.	37	// + The Software may only be used with the Licensor's products.
38	// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this	38	// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
39	// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this	39	// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
40	// + agreement shall be the property of the Licensor.	40	// + agreement shall be the property of the Licensor.
41	// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other	41	// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
42	// + features that can be used to identify the program may not be altered or defaced by the customer.	42	// + features that can be used to identify the program may not be altered or defaced by the customer.
43	// + The customer shall be responsible for taking reasonable precautions	43	// + The customer shall be responsible for taking reasonable precautions
44	// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the	44	// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
45	// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and	45	// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
46	// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product	46	// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
47	// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.	47	// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
48	// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test	48	// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
49	// + the software for his purpose before any operational usage. The customer will backup his data before using the software.	49	// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
50	// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data	50	// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
51	// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.	51	// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
52	// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.	52	// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
53	// + #### END OF LICENSING TERMS ####	53	// + #### END OF LICENSING TERMS ####
54	// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.	54	// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
55	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	55	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
56	#include <stdio.h>	56	#include <stdio.h>
57	#include <string.h>	57	#include <string.h>
58	#include "91x_lib.h"	58	#include "91x_lib.h"
59	#include "mk3mag.h"	59	#include "mk3mag.h"
60	#include "i2c.h"	60	#include "i2c.h"
61	#include "i2c0.h"	61	#include "i2c0.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	#include "spi_slave.h"	67	#include "spi_slave.h"
68		68
69	#define MK3MAG_SLAVE_ADDRESS 0x50 // i2C slave address	69	#define MK3MAG_SLAVE_ADDRESS 0x50 // i2C slave address
70		70
71	u8 MK3MAG_Present = 0;	71	u8 MK3MAG_Present = 0;
72		72
73	typedef struct	73	typedef struct
74	{	74	{
75	s16 Nick;	75	s16 Nick;
76	s16 Roll;	76	s16 Roll;
77	} __attribute__((packed)) MK3MAG_WriteAttitude_t;	77	} __attribute__((packed)) MK3MAG_WriteAttitude_t;
78		78
79	typedef struct	79	typedef struct
80	{	80	{
81	u8 Major;	81	u8 Major;
82	u8 Minor;	82	u8 Minor;
83	u8 Patch;	83	u8 Patch;
84	u8 Compatible;	84	u8 Compatible;
85	} __attribute__((packed)) MK3MAG_Version_t;	85	} __attribute__((packed)) MK3MAG_Version_t;
86		86
87	typedef struct	87	typedef struct
88	{	88	{
89	u8 CalByte;	89	u8 CalByte;
90	u8 Dummy1;	90	u8 Dummy1;
91	u8 Dummy2;	91	u8 Dummy2;
92	} __attribute__((packed)) MK3MAG_Cal_t;	92	} __attribute__((packed)) MK3MAG_Cal_t;
93		93
94	// Transfer buffers	94	// Transfer buffers
95	volatile MK3MAG_WriteAttitude_t MK3MAG_WriteAttitude;	95	volatile MK3MAG_WriteAttitude_t MK3MAG_WriteAttitude;
96	volatile MK3MAG_Version_t MK3MAG_Version;	96	volatile MK3MAG_Version_t MK3MAG_Version;
97	volatile MK3MAG_Cal_t MK3MAG_WriteCal;	97	volatile MK3MAG_Cal_t MK3MAG_WriteCal;
98	volatile MK3MAG_Cal_t MK3MAG_ReadCal;	98	volatile MK3MAG_Cal_t MK3MAG_ReadCal;
99		99
100	// -------------------------------------------------------------------------------	100	// -------------------------------------------------------------------------------
101	// the calculation of the MK3MAG packet checksum	101	// the calculation of the MK3MAG packet checksum
102		102
103	// calculate the crc of bytecount bytes in buffer	103	// calculate the crc of bytecount bytes in buffer
104	u8 MK3MAG_CalcCRC(u8* pBuffer, u8 bytecount)	104	u8 MK3MAG_CalcCRC(u8* pBuffer, u8 bytecount)
105	{	105	{
106	u8 i, crc = 0;	106	u8 i, crc = 0;
107	for(i=0; i < bytecount; i++)	107	for(i=0; i < bytecount; i++)
108	{	108	{
109	crc += pBuffer[i];	109	crc += pBuffer[i];
110	}	110	}
111	crc = ~crc;	111	crc = ~crc;
112	return crc;	112	return crc;
113	}	113	}
114		114
115	// assuming the last byte in buffer is the crc byte	115	// assuming the last byte in buffer is the crc byte
116	u8 MK3MAG_CheckCRC(u8* pBuffer, u8 BuffLen)	116	u8 MK3MAG_CheckCRC(u8* pBuffer, u8 BuffLen)
117	{	117	{
118	u8 crc = 0, retval = 0;	118	u8 crc = 0, retval = 0;
119	if(BuffLen == 0) return(retval);	119	if(BuffLen == 0) return(retval);
120	crc = MK3MAG_CalcCRC(pBuffer, BuffLen-1);	120	crc = MK3MAG_CalcCRC(pBuffer, BuffLen-1);
121	if(crc == pBuffer[BuffLen-1])	121	if(crc == pBuffer[BuffLen-1])
122	{	122	{
123	retval = 1;	123	retval = 1;
124	DebugOut.Analog[15]++; // count mk3mag ok	124	DebugOut.Analog[15]++; // count mk3mag ok
125	}	125	}
126	else	126	else
127	{	127	{
128	retval = 0;	128	retval = 0;
129	DebugOut.Analog[14]++; // count mk3mag crc error	129	DebugOut.Analog[14]++; // count mk3mag crc error
130	}	130	}
131	return(retval);	131	return(retval);
132	}	132	}
133		133
134	// -------------------------------------------------------------------------------	134	// -------------------------------------------------------------------------------
135	// the I2C rx data handler functions	135	// the I2C rx data handler functions
136		136
137	// rx data handler for version info request	137	// rx data handler for version info request
138	void MK3MAG_UpdateVersion(u8* pRxBuffer, u8 RxBufferSize)	138	void MK3MAG_UpdateVersion(u8* pRxBuffer, u8 RxBufferSize)
139	{ // if crc is ok and number of byte are matching	139	{ // if crc is ok and number of byte are matching
140	if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MK3MAG_Version)+1)) )	140	if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MK3MAG_Version)+1)) )
141	{	141	{
142	memcpy((u8 *)&MK3MAG_Version, pRxBuffer, sizeof(MK3MAG_Version));	142	memcpy((u8 *)&MK3MAG_Version, pRxBuffer, sizeof(MK3MAG_Version));
143	}	143	}
144	}	144	}
145		145
146	// rx data handler for calibration request	146	// rx data handler for calibration request
147	void MK3MAG_UpdateCalibration(u8* pRxBuffer, u8 RxBufferSize)	147	void MK3MAG_UpdateCalibration(u8* pRxBuffer, u8 RxBufferSize)
148	{ // if crc is ok and number of byte are matching	148	{ // if crc is ok and number of byte are matching
149	if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MK3MAG_ReadCal)+1)) )	149	if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MK3MAG_ReadCal)+1)) )
150	{	150	{
151	memcpy((u8 *)&MK3MAG_ReadCal, pRxBuffer, sizeof(MK3MAG_ReadCal));	151	memcpy((u8 *)&MK3MAG_ReadCal, pRxBuffer, sizeof(MK3MAG_ReadCal));
152	}	152	}
153	}	153	}
154		154
155	// rx data handler for magnetic vector request	155	// rx data handler for magnetic vector request
156	void MK3MAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)	156	void MK3MAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)
157	{ // if crc is ok and number of byte are matching	157	{ // if crc is ok and number of byte are matching
158	if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MagVector)+1)) )	158	if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MagVector)+1)) )
159	{	159	{
160	s16vec_t pMagVector = (s16vec_t)pRxBuffer;	160	s16vec_t pMagVector = (s16vec_t)pRxBuffer;
161	MagVector.X = pMagVector->Y;	161	MagVector.X = pMagVector->Y;
162	MagVector.Y = pMagVector->X;	162	MagVector.Y = pMagVector->X;
163	MagVector.Z = -pMagVector->Z;	163	MagVector.Z = -pMagVector->Z;
164	Compass_CalcHeading();	164	Compass_CalcHeading();
165	}	165	}
166	}	166	}
167		167
168	//----------------------------------------------------------------	168	//----------------------------------------------------------------
169	#define MK3MAG_CMD_VERSION 0x01	169	#define MK3MAG_CMD_VERSION 0x01
170	#define MK3MAG_CMD_READ_MAGVECT 0x02	170	#define MK3MAG_CMD_READ_MAGVECT 0x02
171	#define MK3MAG_CMD_WRITE_CAL 0x04	171	#define MK3MAG_CMD_WRITE_CAL 0x04
172		172
173	// use I2C1 for communication	173	// use I2C1 for communication
174	void MK3MAG_SendCommandI2C1(u8 command)	174	void MK3MAG_SendCommand(u8 command)
175	{	175	{
176	// try to catch the I2C buffer	176	// try to catch the I2C buffer
177	if(I2C_LockBuffer(0))	177	if(I2C_LockBuffer(0))
178	{	178	{
179	u16 TxBytes = 0;	179	u16 TxBytes = 0;
180	u16 RxBytes = 0;	180	u16 RxBytes = 0;
181	I2C_pRxHandler_t pRxHandlerFunc = NULL;	181	I2C_pRxHandler_t pRxHandlerFunc = NULL;
182		182
183	// update current command id	183	// update current command id
184	I2C_Buffer[TxBytes++] = command;	184	I2C_Buffer[TxBytes++] = command;
185		185
186	// set pointers to data area with respect to the command id	186	// set pointers to data area with respect to the command id
187	switch (command)	187	switch (command)
188	{	188	{
189	case MK3MAG_CMD_VERSION:	189	case MK3MAG_CMD_VERSION:
190	RxBytes = sizeof(MK3MAG_Version)+1;	190	RxBytes = sizeof(MK3MAG_Version)+1;
191	pRxHandlerFunc = &MK3MAG_UpdateVersion;	191	pRxHandlerFunc = &MK3MAG_UpdateVersion;
192	break;	192	break;
193	case MK3MAG_CMD_WRITE_CAL:	193	case MK3MAG_CMD_WRITE_CAL:
194	RxBytes = sizeof(MK3MAG_ReadCal)+1;	194	RxBytes = sizeof(MK3MAG_ReadCal)+1;
195	pRxHandlerFunc = &MK3MAG_UpdateCalibration;	195	pRxHandlerFunc = &MK3MAG_UpdateCalibration;
196	memcpy((u8)I2C_Buffer+1, (u8)&MK3MAG_WriteCal, sizeof(MK3MAG_WriteCal));	196	memcpy((u8)I2C_Buffer+1, (u8)&MK3MAG_WriteCal, sizeof(MK3MAG_WriteCal));
197	TxBytes += sizeof(MK3MAG_WriteCal);	197	TxBytes += sizeof(MK3MAG_WriteCal);
198	break;	198	break;
199	case MK3MAG_CMD_READ_MAGVECT:	199	case MK3MAG_CMD_READ_MAGVECT:
200	RxBytes = sizeof(MagVector)+1;	200	RxBytes = sizeof(MagVector)+1;
201	pRxHandlerFunc = &MK3MAG_UpdateMagVector;	201	pRxHandlerFunc = &MK3MAG_UpdateMagVector;
202	break;	202	break;
203	default: // unknown command id	203	default: // unknown command id
204	RxBytes = 0;	204	RxBytes = 0;
205	pRxHandlerFunc = NULL;	205	pRxHandlerFunc = NULL;
206	break;	206	break;
207	}	207	}
208	// update packet checksum	208	// update packet checksum
209	I2C_Buffer[TxBytes] = MK3MAG_CalcCRC((u8*)I2C_Buffer, TxBytes);	209	I2C_Buffer[TxBytes] = MK3MAG_CalcCRC((u8*)I2C_Buffer, TxBytes);
210	TxBytes++;	210	TxBytes++;
211	// initiate I2C transmission	211	// initiate I2C transmission
212	I2C_Transmission(MK3MAG_SLAVE_ADDRESS, TxBytes, pRxHandlerFunc, RxBytes);	212	I2C_Transmission(MK3MAG_SLAVE_ADDRESS, TxBytes, pRxHandlerFunc, RxBytes);
213	} // EOF I2C_State == I2C_IDLE	213	} // EOF I2C_State == I2C_IDLE
214	}	214	}
215		-
216	// use I2C0 for communication	-
217	void MK3MAG_SendCommandI2C0(u8 command)	-
218	{	-
219	// try to catch the I2C buffer	-
220	if(I2C0_LockBuffer(0))	-
221	{	-
222	u16 TxBytes = 0;	-
223	u16 RxBytes = 0;	-
224	I2C_pRxHandler_t pRxHandlerFunc = NULL;	-
225		-
226	// update current command id	-
227	I2C0_Buffer[TxBytes++] = command;	-
228		-
229	// set pointers to data area with respect to the command id	-
230	switch (command)	-
231	{	-
232	case MK3MAG_CMD_VERSION:	-
233	RxBytes = sizeof(MK3MAG_Version)+1;	-
234	pRxHandlerFunc = &MK3MAG_UpdateVersion;	-
235	break;	-
236	case MK3MAG_CMD_WRITE_CAL:	-
237	RxBytes = sizeof(MK3MAG_ReadCal)+1;	-
238	pRxHandlerFunc = &MK3MAG_UpdateCalibration;	-
239	memcpy((u8)I2C0_Buffer+1, (u8)&MK3MAG_WriteCal, sizeof(MK3MAG_WriteCal));	-
240	TxBytes += sizeof(MK3MAG_WriteCal);	-
241	break;	-
242	case MK3MAG_CMD_READ_MAGVECT:	-
243	RxBytes = sizeof(MagVector)+1;	-
244	pRxHandlerFunc = &MK3MAG_UpdateMagVector;	-
245	break;	-
246	default: // unknown command id	-
247	RxBytes = 0;	-
248	pRxHandlerFunc = NULL;	-
249	break;	-
250	}	-
251	// update packet checksum	-
252	I2C0_Buffer[TxBytes] = MK3MAG_CalcCRC((u8*)I2C0_Buffer, TxBytes);	-
253	TxBytes++;	-
254	// initiate I2C transmission	-
255	I2C0_Transmission(MK3MAG_SLAVE_ADDRESS, TxBytes, pRxHandlerFunc, RxBytes);	-
256	} // EOF I2C_State == I2C_IDLE	-
257	}	-
258		-
259	void MK3MAG_SendCommand(u8 command)	-
260	{	-
261	// MK3MAG_SendCommandI2C1(command);	-
262	MK3MAG_SendCommandI2C0(command);	-
263	}	215
264		216
265	//----------------------------------------------------------------	217	//----------------------------------------------------------------
266	u8 MK3MAG_Init(void)	218	u8 MK3MAG_Init(void)
267	{	219	{
268	u8 msg[64];	220	u8 msg[64];
269	u8 repeat;	221	u8 repeat;
270	u32 timeout;	222	u32 timeout;
271		223
272	MK3MAG_Present = 0;	224	MK3MAG_Present = 0;
273		225
274	MK3MAG_Version.Major = 0xFF;	226	MK3MAG_Version.Major = 0xFF;
275	MK3MAG_Version.Minor = 0xFF;	227	MK3MAG_Version.Minor = 0xFF;
276	MK3MAG_Version.Patch = 0xFF;	228	MK3MAG_Version.Patch = 0xFF;
277	MK3MAG_Version.Compatible = 0xFF;	229	MK3MAG_Version.Compatible = 0xFF;
278		230
279	Compass_Heading = -1;	231	Compass_Heading = -1;
280		232
281	// polling of version info	233	// polling of version info
282	repeat = 0;	234	repeat = 0;
283	do	235	do
284	{	236	{
285	MK3MAG_SendCommand(MK3MAG_CMD_VERSION);	237	MK3MAG_SendCommand(MK3MAG_CMD_VERSION);
286	if(Version_HW > 11) timeout = SetDelay(100);	238	if(Version_HW > 11) timeout = SetDelay(100);
287	else timeout = SetDelay(250);	239	else timeout = SetDelay(250);
288		240
289	do	241	do
290	{	242	{
291	if (MK3MAG_Version.Major != 0xFF) break; // break loop on success	243	if (MK3MAG_Version.Major != 0xFF) break; // break loop on success
292	}while (!CheckDelay(timeout));	244	}while (!CheckDelay(timeout));
293	UART1_PutString(".");	245	UART1_PutString(".");
294	repeat++;	246	repeat++;
295	}while ((MK3MAG_Version.Major == 0xFF) && (repeat < 12)); // 12*250ms=3s	247	}while ((MK3MAG_Version.Major == 0xFF) && (repeat < 12)); // 12*250ms=3s
296	// if we got it	248	// if we got it
297	if (MK3MAG_Version.Major != 0xFF)	249	if (MK3MAG_Version.Major != 0xFF)
298	{	250	{
299	sprintf(msg, " MK3MAG V%d.%d%c", MK3MAG_Version.Major, MK3MAG_Version.Minor, 'a' + MK3MAG_Version.Patch);	251	sprintf(msg, " MK3MAG V%d.%d%c", MK3MAG_Version.Major, MK3MAG_Version.Minor, 'a' + MK3MAG_Version.Patch);
300	UART1_PutString(msg);	252	UART1_PutString(msg);
301	if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)	253	if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)
302	{	254	{
303	UART1_PutString("\n\r MK3MAG not compatible!");	255	UART1_PutString("\n\r MK3MAG not compatible!");
304	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_COMPASS_INCOMPATIBLE;	256	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_COMPASS_INCOMPATIBLE;
305	LED_RED_ON;	257	LED_RED_ON;
306	}	258	}
307	else	259	else
308	{ // version ok	260	{ // version ok
309	MK3MAG_Present = 1;	261	MK3MAG_Present = 1;
310	}	262	}
311	}	263	}
312	return(MK3MAG_Present);	264	return(MK3MAG_Present);
313	}	265	}
314		266
315	//----------------------------------------------------------------	267	//----------------------------------------------------------------
316	void MK3MAG_Update(void)	268	void MK3MAG_Update(void)
317	{	269	{
318	static u32 TimerUpdate = 0;	270	static u32 TimerUpdate = 0;
319	static s16 x_max,y_max,z_max,x_min,y_min,z_min;	271	static s16 x_max,y_max,z_max,x_min,y_min,z_min;
320	static u8 last_state,speak = 0;	272	static u8 last_state,speak = 0;
321	u8 msg[64];	273	u8 msg[64];
322	u16 MinCaclibration = 500;	274	u16 MinCaclibration = 500;
323		275
324	if( (I2C_State == I2C_STATE_OFF) \|\| !MK3MAG_Present ) return;	276	if( (I2C_State == I2C_STATE_OFF) \|\| !MK3MAG_Present ) return;
325		277
326	if(CheckDelay(TimerUpdate))	278	if(CheckDelay(TimerUpdate))
327	{	279	{
328	// check for incomming compass calibration request	280	// check for incomming compass calibration request
329	Compass_UpdateCalState();	281	Compass_UpdateCalState();
330	MK3MAG_WriteCal.CalByte = Compass_CalState;	282	MK3MAG_WriteCal.CalByte = Compass_CalState;
331		283
332	if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte)	284	if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte)
333	{ // send new calibration state	285	{ // send new calibration state
334	MK3MAG_SendCommand(MK3MAG_CMD_WRITE_CAL);	286	MK3MAG_SendCommand(MK3MAG_CMD_WRITE_CAL);
335	}	287	}
336	else	288	else
337	{ // calibration state matches	289	{ // calibration state matches
338	MK3MAG_SendCommand(MK3MAG_CMD_READ_MAGVECT); // initiate magvector transfer	290	MK3MAG_SendCommand(MK3MAG_CMD_READ_MAGVECT); // initiate magvector transfer
339		291
340	switch(Compass_CalState)	292	switch(Compass_CalState)
341	{	293	{
342	case 1:	294	case 1:
343	if(last_state != Compass_CalState)	295	if(last_state != Compass_CalState)
344	{	296	{
345	UART1_PutString("\r\nMK3Mag calibration\n\r");	297	UART1_PutString("\r\nMK3Mag calibration\n\r");
346	if(EarthMagneticStrengthTheoretic)	298	if(EarthMagneticStrengthTheoretic)
347	{	299	{
348	MinCaclibration = (MinCaclibration * EarthMagneticStrengthTheoretic) / 50;	300	MinCaclibration = (MinCaclibration * EarthMagneticStrengthTheoretic) / 50;
349	sprintf(msg, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic);	301	sprintf(msg, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic);
350	UART1_PutString(msg);	302	UART1_PutString(msg);
351	}	303	}
352	else UART1_PutString("without GPS\n\r");	304	else UART1_PutString("without GPS\n\r");
353	}	305	}
354	x_max = -30000; y_max = -30000; z_max = -30000;	306	x_max = -30000; y_max = -30000; z_max = -30000;
355	x_min = 30000; y_min = 30000; z_min = 30000;	307	x_min = 30000; y_min = 30000; z_min = 30000;
356	speak = 1;	308	speak = 1;
357	break;	309	break;
358	case 2:	310	case 2:
359	if(speak) SpeakHoTT = SPEAK_CALIBRATE; speak = 0;	311	if(speak) SpeakHoTT = SPEAK_CALIBRATE; speak = 0;
360		312
361	if(MagVector.X > x_max) { x_max = MagVector.X; BeepTime = 60; }	313	if(MagVector.X > x_max) { x_max = MagVector.X; BeepTime = 60; }
362	else if(MagVector.X < x_min) { x_min = MagVector.X; BeepTime = 20; }	314	else if(MagVector.X < x_min) { x_min = MagVector.X; BeepTime = 20; }
363	if(MagVector.Y > y_max) { y_max = MagVector.Y; BeepTime = 60; }	315	if(MagVector.Y > y_max) { y_max = MagVector.Y; BeepTime = 60; }
364	else if(MagVector.Y < y_min) { y_min = MagVector.Y; BeepTime = 20; }	316	else if(MagVector.Y < y_min) { y_min = MagVector.Y; BeepTime = 20; }
365		317
366	break;	318	break;
367	case 3:	319	case 3:
368	speak = 1;	320	speak = 1;
369	break;	321	break;
370	case 4:	322	case 4:
371	if(speak) SpeakHoTT = SPEAK_CALIBRATE; speak = 0;	323	if(speak) SpeakHoTT = SPEAK_CALIBRATE; speak = 0;
372	if(MagVector.Z > z_max) { z_max = MagVector.Z; BeepTime = 80; }	324	if(MagVector.Z > z_max) { z_max = MagVector.Z; BeepTime = 80; }
373	else if(MagVector.Z < z_min) { z_min = MagVector.Z; BeepTime = 80; }	325	else if(MagVector.Z < z_min) { z_min = MagVector.Z; BeepTime = 80; }
374	break;	326	break;
375	case 5:	327	case 5:
376	if(last_state == Compass_CalState) break;	328	if(last_state == Compass_CalState) break;
377	if(((x_max - x_min) > MinCaclibration) && ((y_max - y_min) > MinCaclibration) && ((z_max - z_min) > MinCaclibration))	329	if(((x_max - x_min) > MinCaclibration) && ((y_max - y_min) > MinCaclibration) && ((z_max - z_min) > MinCaclibration))
378	{	330	{
379	BeepTime = 2500;	331	BeepTime = 2500;
380	UART1_PutString("\r\n-> Calibration okay <-\n\r");	332	UART1_PutString("\r\n-> Calibration okay <-\n\r");
381	SpeakHoTT = SPEAK_MIKROKOPTER;	333	SpeakHoTT = SPEAK_MIKROKOPTER;
382	}	334	}
383	else	335	else
384	{	336	{
385	UART1_PutString("\r\nCalibration FAILED - Values too low: ");	337	UART1_PutString("\r\nCalibration FAILED - Values too low: ");
386	if((x_max - x_min) < MinCaclibration) UART1_PutString("X! ");	338	if((x_max - x_min) < MinCaclibration) UART1_PutString("X! ");
387	if((y_max - y_min) < MinCaclibration) UART1_PutString("Y! ");	339	if((y_max - y_min) < MinCaclibration) UART1_PutString("Y! ");
388	if((z_max - z_min) < MinCaclibration) UART1_PutString("Z! ");	340	if((z_max - z_min) < MinCaclibration) UART1_PutString("Z! ");
389	UART1_PutString("\r\n");	341	UART1_PutString("\r\n");
390	SpeakHoTT = SPEAK_ERR_CALIBARTION;	342	SpeakHoTT = SPEAK_ERR_CALIBARTION;
391	}	343	}
392	UART1_PutString(msg);	344	UART1_PutString(msg);
393	sprintf(msg, "\r\nX: (%i - %i = %i)\r\n",x_max,x_min,x_max - x_min);	345	sprintf(msg, "\r\nX: (%i - %i = %i)\r\n",x_max,x_min,x_max - x_min);
394	UART1_PutString(msg);	346	UART1_PutString(msg);
395	sprintf(msg, "Y: (%i - %i = %i)\r\n",y_max,y_min,y_max - y_min);	347	sprintf(msg, "Y: (%i - %i = %i)\r\n",y_max,y_min,y_max - y_min);
396	UART1_PutString(msg);	348	UART1_PutString(msg);
397	sprintf(msg, "Z: (%i - %i = %i)\r\n",z_max,z_min,z_max - z_min);	349	sprintf(msg, "Z: (%i - %i = %i)\r\n",z_max,z_min,z_max - z_min);
398	UART1_PutString(msg);	350	UART1_PutString(msg);
399	sprintf(msg, "(Minimum ampilitude is: %i)\r\n",MinCaclibration);	351	sprintf(msg, "(Minimum ampilitude is: %i)\r\n",MinCaclibration);
400	UART1_PutString(msg);	352	UART1_PutString(msg);
401	break;	353	break;
402	default:	354	default:
403	break;	355	break;
404	}	356	}
405	last_state = Compass_CalState;	357	last_state = Compass_CalState;
406	}	358	}
407	TimerUpdate = SetDelay(20); // every 20 ms are 50 Hz	359	TimerUpdate = SetDelay(20); // every 20 ms are 50 Hz
408	}	360	}
409	}	361	}
410		362

Subversion Repositories NaviCtrl

(root)/tags/V2.16a/mk3mag.c @ 811 - Rev 464 → 465