WebSVN - NaviCtrl - Diff - Rev 241 and 242

 #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;
-u8 MK3MAG_StopPolling = 1;
 } __attribute__((packed)) MK3MAG_WriteAttitude_t;
-// Transfer buffers
 volatile MK3MAG_Heading_t                       MK3MAG_Heading;
-volatile MK3MAG_WriteAttitude_t         MK3MAG_WriteAttitude;
+// Transfer buffers
-volatile MK3MAG_MagVector_t                     MK3MAG_MagVector;
+volatile MK3MAG_WriteAttitude_t         MK3MAG_WriteAttitude;
-volatile MK3MAG_Version_t                       MK3MAG_Version;
-volatile MK3MAG_Cal_t                           MK3MAG_WriteCal;
-volatile MK3MAG_Cal_t                           MK3MAG_ReadCal;
-void MK3MAG_Init(void)
-{
-        MK3MAG_Version.Major = 0xFF;
-        MK3MAG_Version.Minor = 0xFF;
-        MK3MAG_Version.Patch = 0xFF;
-        MK3MAG_Version.Compatible = 0xFF;
+volatile MK3MAG_Version_t                       MK3MAG_Version;
 volatile MK3MAG_Cal_t                           MK3MAG_WriteCal;
-        MK3MAG_Heading.Heading = -1;
+volatile MK3MAG_Cal_t                           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(MK3MAG_MagVector)+1)) )
+        if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MagVector)+1)) )
         {
-                memcpy((u8 *)&MK3MAG_MagVector, pRxBuffer, sizeof(MK3MAG_MagVector));
+                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(MK3MAG_Heading)+1)) )
+        if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(CompassHeading)+1)) )
+        {
-                memcpy((u8 *)&MK3MAG_Heading, pRxBuffer, sizeof(MK3MAG_Heading));
+                memcpy((u8 *)&CompassHeading, pRxBuffer, sizeof(CompassHeading));
+        }
+}
-//----------------------------------------------------------------
-void MK3MAG_SendCommand(u8 command)
 //----------------------------------------------------------------
-        // disable I2C IRQ to check state
+void MK3MAG_SendCommand(u8 command)
         //VIC_ITCmd(I2C1_ITLine, DISABLE);
         // If I2C no transmission is in progress
         if(I2C_State == I2C_STATE_IDLE)
                                 pRxHandlerFunc = &MK3MAG_UpdateCalibration;
                                 memcpy((u8*)I2C_TxBuffer+1, (u8*)&MK3MAG_WriteCal, sizeof(MK3MAG_WriteCal));
                                 I2C_TxBufferSize += sizeof(MK3MAG_WriteCal);
                                 break;
                         case MK3MAG_CMD_READ_MAGVECT:
-                                RxBytes = sizeof(MK3MAG_MagVector)+1;
+                                RxBytes = sizeof(MagVector)+1;
                                 pRxHandlerFunc = &MK3MAG_UpdateMagVector;
                                 break;
                         case MK3MAG_CMD_READ_HEADING:
-                                RxBytes = sizeof(MK3MAG_Heading)+1;
+                                RxBytes = sizeof(CompassHeading)+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;
                                 break;
+                }
                 // update packet checksum
                 I2C_TxBuffer[I2C_TxBufferSize] = MK3MAG_CalcCRC((u8*)I2C_TxBuffer, I2C_TxBufferSize);
                 I2C_TxBufferSize++;
-                // enable I2C IRQ again
-                VIC_ITCmd(I2C1_ITLine, ENABLE);
                 // initiate I2C transmission
                 I2C_Transmission(MK3MAG_SLAVE_ADDRESS, pRxHandlerFunc, RxBytes);
         } // EOF I2C_State == I2C_IDLE
-        else // I2C_State != I2C_IDLE
-        {
-                // re-enable I2C IRQ again
-                //VIC_ITCmd(I2C1_ITLine, ENABLE);
-        }
+}
 //----------------------------------------------------------------
-void MK3MAG_GetVersion(void)
+u8 MK3MAG_Init(void)
+{
         u8 msg[64];
-        u8 repeat;
-        u32 timeout;
+        u8 repeat;
-        UART1_PutString("\r\n Getting Version from MK3MAG");
+        u32 timeout;
         // stop polling of other commands
-        MK3MAG_StopPolling = 1;
+        MK3MAG_Present = 0;
+        MK3MAG_Version.Major = 0xFF;
+        MK3MAG_Version.Minor = 0xFF;
+        MK3MAG_Version.Patch = 0xFF;
-        MK3MAG_Version.Major = 0xFF;
+        MK3MAG_Version.Compatible = 0xFF;
         MK3MAG_Version.Minor = 0xFF;
-        MK3MAG_Version.Patch = 0xFF;
+        CompassHeading = -1;
         MK3MAG_Version.Compatible = 0xFF;
         // polling of version info
                 repeat++;
         }while ((MK3MAG_Version.Major == 0xFF) && (repeat < 12)); // 12*250ms=3s
         // if we got it
         if (MK3MAG_Version.Major != 0xFF)
+        {
-                sprintf(msg, "\r\n MK3MAG V%d.%d%c", MK3MAG_Version.Major, MK3MAG_Version.Minor, 'a' + MK3MAG_Version.Patch);
+                sprintf(msg, " MK3MAG V%d.%d%c", MK3MAG_Version.Major, MK3MAG_Version.Minor, 'a' + MK3MAG_Version.Patch);
-                UART1_PutString(msg);
-                sprintf(msg, " Compatible: %d", MK3MAG_Version.Compatible);
                 UART1_PutString(msg);
+                if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)
+                {
+                        UART1_PutString("\n\r MK3MAG not compatible!");
+                        LED_RED_ON;
+                }
+                else
+                {       // version ok
+                        MK3MAG_Present = 1;
+                }
+        }
-        else UART1_PutString("\n\r No version information from MK3Mag.");
+        return(MK3MAG_Present);
-        MK3MAG_StopPolling = 0; // enable polling of heading command
+}
 //----------------------------------------------------------------
 void MK3MAG_UpdateCompass(void)
+{
         static u32 TimerCompassUpdate = 0;
-        if( (I2C_State == I2C_STATE_OFF) || MK3MAG_StopPolling ) return;
+        if( (I2C_State == I2C_STATE_OFF) || !MK3MAG_Present ) return;
         if(CheckDelay(TimerCompassUpdate))

Subversion Repositories NaviCtrl

(root)/tags/V0.28h/mk3mag.c @ 390 - Rev 241 → 242