WebSVN - NaviCtrl - Diff - Rev 254 and 256


/*#######################################################################################*/
/* !!! 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 <string.h>
#include "91x_lib.h"
#include "mk3mag.h"
#include "i2c.h"
#include "timer1.h"
#include "led.h"
#include "spi_slave.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;
 
// 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)) )
        {
                memcpy((u8 *)&MagVector, pRxBuffer, sizeof(MagVector));
        }                        
}
 
// rx data handler for heading request
void MK3MAG_UpdateHeading(u8* pRxBuffer, u8 RxBufferSize)
{       // if crc is ok and number of byte are matching
        if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(Compass_Heading)+1)) )
        {
                memcpy((u8 *)&Compass_Heading, pRxBuffer, sizeof(Compass_Heading));
        }                        
}
 
//----------------------------------------------------------------
void MK3MAG_SendCommand(u8 command)
{
        // trya 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;
                        case MK3MAG_CMD_READ_HEADING:
                                RxBytes = sizeof(Compass_Heading)+1;
                                pRxHandlerFunc = &MK3MAG_UpdateHeading;
                                // update attitude from spi rx buffer
                                VIC_ITCmd(SSP0_ITLine, DISABLE); // avoid spi buffer update during copy
                                MK3MAG_WriteAttitude.Roll = FromFlightCtrl.AngleRoll;
                                MK3MAG_WriteAttitude.Nick = FromFlightCtrl.AngleNick;
                                VIC_ITCmd(SSP0_ITLine, ENABLE);
                                memcpy((u8*)I2C_Buffer+1, (u8*)&MK3MAG_WriteAttitude, sizeof(MK3MAG_WriteAttitude));
                                TxBytes += sizeof(MK3MAG_WriteAttitude);
                                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);
                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_UpdateCompass(void)
{
        static u32 TimerCompassUpdate = 0;
 
        if( (I2C_State == I2C_STATE_OFF) || !MK3MAG_Present ) return;
       
        if(CheckDelay(TimerCompassUpdate))
        {
                // check for incomming compass calibration request
                Compass_UpdateCalState();
                MK3MAG_WriteCal.CalByte = Compass_CalState;
                // send new calstate
                if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte)
                {
                        MK3MAG_SendCommand(MK3MAG_CMD_WRITE_CAL);
                }
                else // request current heading
                {
                        MK3MAG_SendCommand(MK3MAG_CMD_READ_HEADING);
                }
                TimerCompassUpdate = SetDelay(20);    // every 20 ms are 50 Hz
        }
}
 
Subversion Repositories NaviCtrl

(root)/tags/V0.28h/mk3mag.c @ 574 - Rev 254 → 256
