WebSVN - NaviCtrl - Diff - Rev 254 and 256

 #include "timer1.h"
 #include "led.h"
 #include "spi_slave.h"
 #include "uart1.h"
 #include "eeprom.h"
+#include "mymath.h"
 u8 NCMAG_Present = 0;
 u8 NCMAG_IsCalibrated = 0;
 u8 NCMAG_MagType = MAG_TYPE_NONE;
 #define CALIBRATION_VERSION 1
+#define EEPROM_ADR_MAG_CALIBRATION 50
+#define NCMAG_MIN_RAWVALUE -2047
+#define NCMAG_MAX_RAWVALUE  2047
-#define EEPROM_ADR_MAG_CALIBRATION 50
+#define NCMAG_INVALID_DATA -4096
 typedef struct
+{
         s16 Range;
         s16 Offset;
-}  Scaling_t;
+} __attribute__((packed)) Scaling_t;
 typedef struct
+{
         Scaling_t MagX;
         Scaling_t MagY;
         Scaling_t MagZ;
         u8 Version;
         u8 crc;
-}  Calibration_t;
+} __attribute__((packed)) Calibration_t;
         u8 i, crc = 0xAA;
         EEPROM_Result_t eres;
         u8 *pBuff = (u8*)&Calibration;
         Calibration.Version = CALIBRATION_VERSION;
-        for(i = 0; i<sizeof(Calibration)-1; i++)
+        for(i = 0; i<(sizeof(Calibration)-1); i++)
+        {
                 crc += pBuff[i];
+        }
         Calibration.crc = ~crc;
-        eres = EEPROM_WriteBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration));
-        DebugOut.Analog[25] = eres;
+        eres = EEPROM_WriteBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration));
         if(EEPROM_SUCCESS == eres) i = 1;
         else i = 0;
         return(i);
         u8 i, crc = 0xAA;
         u8 *pBuff = (u8*)&Calibration;
         if(EEPROM_SUCCESS == EEPROM_ReadBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration)))
         {
-                for(i = 0; i<sizeof(Calibration)-1; i++)
+                for(i = 0; i<(sizeof(Calibration)-1); i++)
+                {
                         crc += pBuff[i];
+                }
                 crc = ~crc;
                 if(Calibration.crc != crc) return(0); // crc mismatch
+                if(Calibration.Version == CALIBRATION_VERSION)
+                {
+                        //#ifdef DEBUG
+                        u8 msg[50];
+                        UART1_PutString("\r\n");
+                        sprintf(msg, "XRange = %d, XOffset = %d \r\n", Calibration.MagX.Range, Calibration.MagX.Offset);
+                        UART1_PutString(msg);
+                        sprintf(msg, "YRange = %d, YOffset = %d \r\n", Calibration.MagY.Range, Calibration.MagY.Offset);
+                        UART1_PutString(msg);
+                        sprintf(msg, "ZRange = %d, ZOffset = %d \r\n", Calibration.MagZ.Range, Calibration.MagZ.Offset);
+                        UART1_PutString(msg);
+                        //#endif
+                        return(1);
                 if(Calibration.Version == CALIBRATION_VERSION) return(1);
+        }
         return(0);
+}
+void NCMAG_Calibrate(void)
 void NCMAG_Calibrate(void)
+        static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
+        static s16 X = 0, Y = 0, Z = 0;
+        static u8 OldCalState = 0;
         X = (4*X + MagRawVector.X + 3)/5;
-        static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
+        Y = (4*Y + MagRawVector.Y + 3)/5;
-        static u8 OldCalState = 0;
+        Z = (4*Z + MagRawVector.Z + 3)/5;
         switch(Compass_CalState)
                         Zmax = -10000;
                         break;
                 case 2: // 2nd step of calibration
                         // find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane
-                        if(MagRawVector.X < Xmin) Xmin = MagRawVector.X;
+                        if(X < Xmin) Xmin = X;
-                        if(MagRawVector.X > Xmax) Xmax = MagRawVector.X;
+                        else if(X > Xmax) Xmax = X;
-                        if(MagRawVector.Y < Ymin) Ymin = MagRawVector.Y;
+                        if(Y < Ymin) Ymin = Y;
-                        if(MagRawVector.Y > Ymax) Ymax = MagRawVector.Y;
+                        else if(Y > Ymax) Ymax = Y;
                         break;
                 case 3: // 3rd step of calibration
                         // used to change the orientation of the MK3MAG vertical to the horizontal plane
                         break;
                 case 4:
                         // find Min and Max of the Z-Sensor
-                        if(MagRawVector.Z < Zmin) Zmin = MagRawVector.Z;
+                        if(Z < Zmin) Zmin = Z;
-                        if(MagRawVector.Z > Zmax) Zmax = MagRawVector.Z;
+                        else if(Z > Zmax) Zmax = Z;
                         break;
                                 Calibration.MagX.Offset = (Xmin + Xmax) / 2;
                                 Calibration.MagY.Range = Ymax - Ymin;
                                 Calibration.MagY.Offset = (Ymin + Ymax) / 2;
                                 Calibration.MagZ.Range = Zmax - Zmin;
                                 Calibration.MagZ.Offset = (Zmin + Zmax) / 2;
-                                if(1)//if((Calibration.MagX.Range > 150) && (Calibration.MagY.Range > 150) && (Calibration.MagZ.Range > 150))
+                                if((Calibration.MagX.Range > 512) && (Calibration.MagY.Range > 512) && (Calibration.MagZ.Range > 512))
+                                {
                                         NCMAG_IsCalibrated = NCMag_CalibrationWrite();
+                                }
                                 else
+                                {
 // rx data handler for magnetic sensor raw data
 void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)
 {       // if number of bytes are matching
         if(RxBufferSize == sizeof(MagRawVector) )
         {       // byte order from big to little endian
+                s16 raw;
-                MagRawVector.X = pRxBuffer[0]<<8;
+                raw = pRxBuffer[0]<<8;
-                MagRawVector.X+= pRxBuffer[1];
+                raw+= pRxBuffer[1];
+                if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.X = raw;
-                MagRawVector.Y = pRxBuffer[2]<<8;
+                raw = pRxBuffer[2]<<8;
-                MagRawVector.Y+= pRxBuffer[3];
+                raw+= pRxBuffer[3];
+                if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.Y = raw;
-                MagRawVector.Z = pRxBuffer[4]<<8;
+                raw = pRxBuffer[4]<<8;
-                MagRawVector.Z+= pRxBuffer[5];
+                raw+= pRxBuffer[5];
+                if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.Z = raw;
+        }
         if(Compass_CalState || !NCMAG_IsCalibrated)
         {       // direct output the raw data
                 memcpy((u8*)&MagVector,(u8*)&MagRawVector, sizeof(MagVector));
                 Compass_Heading = -1;
+        {
                 // update MagVector from MagRaw Vector by Scaling
                 MagVector.X = (s16)((1024L*(s32)(MagRawVector.X - Calibration.MagX.Offset))/Calibration.MagX.Range);
                 MagVector.Y = (s16)((1024L*(s32)(MagRawVector.Y - Calibration.MagY.Offset))/Calibration.MagY.Range);
                 MagVector.Z = (s16)((1024L*(s32)(MagRawVector.Z - Calibration.MagZ.Offset))/Calibration.MagZ.Range);
+                if(UART_VersionInfo.HardwareError[0] & NC_ERROR0_SPI_RX)
+                {
+                        Compass_Heading = -1;
+                }
+                else // fc attitude is avialable
+                {
-                // calculate attitude correction
+                        // calculate attitude correction
+                        // a float implementation takes too long for an ISR call!
+                        s16 tmp, Hx, Hy;
-                double Hx, Hy, Cx, Cy, Cz, nick_rad, roll_rad;
+                        s32 sinnick, cosnick, sinroll, cosroll;
-                Cx = (double)MagVector.X;
+                        tmp = FromFlightCtrl.AngleNick/10; // in deg
-                Cy = (double)MagVector.Y;
+                        sinnick = (s32)c_sin_8192(tmp);
-                Cz = (double)MagVector.Z;
+                        cosnick = (s32)c_cos_8192(tmp);
-                nick_rad = ((double)FromFlightCtrl.AngleNick * M_PI) / 1800.0;
+                        tmp = FromFlightCtrl.AngleRoll/10; // in deg
+                        sinroll = (s32)c_sin_8192(tmp);
+                        cosroll = (s32)c_cos_8192(tmp);
-                roll_rad = ((double)FromFlightCtrl.AngleRoll * M_PI) / 1800.0;
+                        // tbd. compensation signs and oriantation has to be fixed
-                Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad);
+                        Hx = (s16)((MagVector.X * cosnick - MagVector.Z * sinnick)/8192L);
-                Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad);
+                        Hy = (s16)((MagVector.Y * cosroll + MagVector.Z * sinroll)/8192L);
-                //DebugOut.Analog[23] = (s16)Hx;
+                        DebugOut.Analog[23] = (s16)Hx;
-                //DebugOut.Analog[24] = (s16)Hy;
+                        DebugOut.Analog[24] = (s16)Hy;
-                // calculate heading
+                        // calculate heading
-                Compass_Heading = (s16)((180.0 * atan2(Hy, Hx)) / M_PI);
+                        tmp = (s16)(c_tan2_546(Hy, Hx)/546L);
+                        if (tmp <= 0) tmp = -tmp;
+                        else tmp = 360 - tmp;
+                        Compass_Heading = tmp;
+                }
+        }
+}
 // rx data handler  for acceleration raw data
 void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize)
 {       // if number of byte are matching
+                        }
+                }
                 else
+                {
                         UART1_PutString("\n\r Not compatible!");
+                        UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE;
                         LED_RED_ON;
+                }
+        }
         else // nothing found
+        {

Subversion Repositories NaviCtrl

(root)/tags/V2.16a/ncmag.c @ 811 - Rev 254 → 256