Subversion Repositories NaviCtrl

/*#######################################################################################*/

/* !!! 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 "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

        }

}