WebSVN - NaviCtrl - Diff - Rev 253 and 254

 // +  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 <math.h>
 #include <string.h>
 #include "91x_lib.h"
 #include "ncmag.h"
 #include "i2c.h"
 #include "timer1.h"
 #include "led.h"
 #include "spi_slave.h"
 #include "uart1.h"
+#include "eeprom.h"
+u8 NCMAG_Present = 0;
-u8 NCMAG_Present = 0;
+u8 NCMAG_IsCalibrated = 0;
 #define MAG_TYPE_NONE           0
 #define MAG_TYPE_HMC5843        1
+#define MAG_TYPE_LSM303DLH      2
+u8 NCMAG_MagType = MAG_TYPE_NONE;
+#define CALIBRATION_VERSION 1
+#define EEPROM_ADR_MAG_CALIBRATION 50
+typedef struct
+{
+        s16 Range;
+        s16 Offset;
+}  Scaling_t;
+typedef struct
+{
+        Scaling_t MagX;
+        Scaling_t MagY;
+        Scaling_t MagZ;
+        u8 Version;
+        u8 crc;
+}  Calibration_t;
 #define MAG_TYPE_LSM303DLH      2
-u8 NCMAG_MagType = MAG_TYPE_NONE;
+Calibration_t Calibration;              // calibration data in RAM
         u8 ctrl_5;
 } __attribute__((packed)) AccConfig_t;
 volatile AccConfig_t AccConfig;
+volatile s16vec_t AccRawVector;
+volatile s16vec_t MagRawVector;
+u8 NCMag_CalibrationWrite(void)
+{
+        u8 i, crc = 0xAA;
+        EEPROM_Result_t eres;
+        u8 *pBuff = (u8*)&Calibration;
+        Calibration.Version = CALIBRATION_VERSION;
+        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;
+        if(EEPROM_SUCCESS == eres) i = 1;
+        else i = 0;
+        return(i);
+}
+u8 NCMag_CalibrationRead(void)
+{
+        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++)
+                {
+                        crc += pBuff[i];
+                }
+                crc = ~crc;
+                if(Calibration.crc != crc) return(0); // crc mismatch
+                if(Calibration.Version == CALIBRATION_VERSION) return(1);
+        }
+        return(0);
+}
+void NCMAG_Calibrate(void)
+{
+        static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
+        static u8 OldCalState = 0;
+        switch(Compass_CalState)
+        {
+                case 1:
+                        // 1st step of calibration
+                        // initialize ranges
+                        // used to change the orientation of the NC in the horizontal plane
+                        Xmin =  10000;
+                        Xmax = -10000;
+                        Ymin =  10000;
+                        Ymax = -10000;
+                        Zmin =  10000;
+                        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(MagRawVector.X > Xmax) Xmax = MagRawVector.X;
+                        if(MagRawVector.Y < Ymin) Ymin = MagRawVector.Y;
+                        if(MagRawVector.Y > Ymax) Ymax = MagRawVector.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(MagRawVector.Z > Zmax) Zmax = MagRawVector.Z;
+                        break;
+                case 5:
+                        // Save values
+                        if(Compass_CalState != OldCalState) // avoid continously writing of eeprom!
+                        {
+                                Calibration.MagX.Range = Xmax - Xmin;
+                                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))
+                                {
+                                        NCMAG_IsCalibrated = NCMag_CalibrationWrite();
+                                }
+                                else
+                                {
+                                        // restore old calibration data from eeprom
+                                        NCMAG_IsCalibrated = NCMag_CalibrationRead();
+                                }
+                        }
+                        break;
+                default:
+                        break;
+        }
-volatile s16vec_t AccVector;
+        OldCalState = Compass_CalState;
+}
 // ---------- call back handlers -----------------------------------------
 // rx data handler for id info request
 void NCMAG_UpdateIdentification(u8* pRxBuffer, u8 RxBufferSize)
-{       // if number of byte are matching
+{       // if number of bytes are matching
-        if(RxBufferSize == sizeof(NCMAG_Identification) )
-        {
+        if(RxBufferSize == sizeof(NCMAG_Identification) )
                 memcpy((u8 *)&NCMAG_Identification, pRxBuffer, sizeof(NCMAG_Identification));
-        }
+                memcpy((u8 *)&NCMAG_Identification, pRxBuffer, sizeof(NCMAG_Identification));
-}
+        }
+}
-// rx data handler for mag vector request
+// rx data handler for magnetic sensor raw data
 void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)
-{       // if number of byte are matching
+{       // if number of bytes are matching
-        if(RxBufferSize == sizeof(MagVector) )
+        if(RxBufferSize == sizeof(MagRawVector) )
         {       // byte order from big to little endian
-                MagVector.X = pRxBuffer[0]<<8;
+                MagRawVector.X = pRxBuffer[0]<<8;
+                MagRawVector.X+= pRxBuffer[1];
+                MagRawVector.Y = pRxBuffer[2]<<8;
+                MagRawVector.Y+= pRxBuffer[3];
+                MagRawVector.Z = pRxBuffer[4]<<8;
+                MagRawVector.Z+= pRxBuffer[5];
+        }
+        if(Compass_CalState || !NCMAG_IsCalibrated)
+        {       // direct output the raw data
+                memcpy((u8*)&MagVector,(u8*)&MagRawVector, sizeof(MagVector));
+                Compass_Heading = -1;
+        }
+        else
+        {
+                // 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);
+                // calculate attitude correction
+                double Hx, Hy, Cx, Cy, Cz, nick_rad, roll_rad;
+                Cx = (double)MagVector.X;
+                Cy = (double)MagVector.Y;
+                Cz = (double)MagVector.Z;
+                nick_rad = ((double)FromFlightCtrl.AngleNick * M_PI) / 1800.0;
+                roll_rad = ((double)FromFlightCtrl.AngleRoll * M_PI) / 1800.0;
+                Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad);
+                Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad);
-                MagVector.X+= pRxBuffer[1];
+                //DebugOut.Analog[23] = (s16)Hx;
-                MagVector.Y = pRxBuffer[2]<<8;
-                MagVector.Y+= pRxBuffer[3];
-                MagVector.Z = pRxBuffer[4]<<8;
+                //DebugOut.Analog[24] = (s16)Hy;
-                MagVector.Z+= pRxBuffer[5];
-        }
-        // tbd. calculate heading from mag vector and attitude
+                // calculate heading
-        CompassHeading = 50; // tbd.
+                Compass_Heading = (s16)((180.0 * atan2(Hy, Hx)) / M_PI);
-}
+        }
+}
-// rx data handler for acc vector request
+// rx data handler  for acceleration raw data
 void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize)
 {       // if number of byte are matching
-        if(RxBufferSize == sizeof(AccVector) )
+        if(RxBufferSize == sizeof(AccRawVector) )
+        {
-                memcpy((u8*)&AccVector, pRxBuffer,sizeof(AccVector));
+                memcpy((u8*)&AccRawVector, pRxBuffer,sizeof(AccRawVector));
+        }
+}
 // rx data handler for reading magnetic sensor configuration
 void NCMAG_UpdateMagConfig(u8* pRxBuffer, u8 RxBufferSize)
 {       // if number of byte are matching
-        if(RxBufferSize == sizeof(MagConfig) )
+        if(RxBufferSize == sizeof(MagConfig) )
+        {
                 memcpy((u8*)(&MagConfig), pRxBuffer, sizeof(MagConfig));
+        }
+}
 // rx data handler for reading acceleration sensor configuration
 void NCMAG_UpdateAccConfig(u8* pRxBuffer, u8 RxBufferSize)
 {       // if number of byte are matching
+        if(RxBufferSize == sizeof(AccConfig) )
+        {
-        if(RxBufferSize == sizeof(AccConfig) )
+                memcpy((u8*)&AccConfig, pRxBuffer, sizeof(AccConfig));
+        }
                 memcpy((u8*)&AccConfig, pRxBuffer, sizeof(AccConfig));
 //----------------------------------------------------------------------
+        {
                 u16 TxBytes = 0;
                 // set register pointer
                 I2C_Buffer[TxBytes++] = REG_ACC_X_LSB;
                 // initiate transmission
-                I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccVector));
+                I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector));
+        }
+}
 // --------------------------------------------------------
 void NCMAG_UpdateCompass(void)
+{
         static u32 TimerCompassUpdate = 0;
+        if( (I2C_State == I2C_STATE_OFF) || !NCMAG_Present )
+        {
+                Compass_Heading = -1;
                 return;
         if( (I2C_State == I2C_STATE_OFF) || !NCMAG_Present ) return;
-        if(CheckDelay(TimerCompassUpdate))
-        {
-                // check for incomming compass calibration request
+        if(CheckDelay(TimerCompassUpdate))
-                // update CalByte from spi input queue
+        {
-                /*fifo_get(&CompassCalcStateFiFo, (u8 *)&(MK3MAG_WriteCal.CalByte));
-                // send new calstate
-                if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte)
-                {
-                        do the calibration here
-                }
+                // check for new calibration state
-                else // request current heading */
                 Compass_UpdateCalState();
-                        NCMAG_GetMagVector();
+                if(Compass_CalState) NCMAG_Calibrate();
                 NCMAG_GetMagVector(); //Get new data;
+                TimerCompassUpdate = SetDelay(20);    // every 20 ms are 50 Hz
                 TimerCompassUpdate = SetDelay(20);    // every 20 ms are 50 Hz
+}
 // --------------------------------------------------------
 u8 NCMAG_SelfTest(void)
         for(i = 0; i<AVERAGE; i++)
+        {
                 NCMAG_GetMagVector();
                 time = SetDelay(20);
         while(!CheckDelay(time));
-                XMax += MagVector.X;
+                XMax += MagRawVector.X;
-                YMax += MagVector.Y;
+                YMax += MagRawVector.Y;
-                ZMax += MagVector.Z;
+                ZMax += MagRawVector.Z;
+        }
         MagConfig.cra = cra_rate|CRA_MODE_NEGBIAS;
         // activate positive bias field
         NCMAG_SetMagConfig();
     // wait for stable readings
         for(i = 0; i < AVERAGE; i++)
+        {
                 NCMAG_GetMagVector();
                 time = SetDelay(20);
         while(!CheckDelay(time));
-                XMin += MagVector.X;
+                XMin += MagRawVector.X;
-                YMin += MagVector.Y;
+                YMin += MagRawVector.Y;
-                ZMin += MagVector.Z;
+                ZMin += MagRawVector.Z;
+        }
         // setup final configuration
         MagConfig.cra = cra_rate|CRA_MODE_NORMAL;
         // activate positive bias field
         NCMAG_SetMagConfig();
                         if(!NCMAG_SelfTest())
+                        {
                                 UART1_PutString(" Selftest failed!");
                                 LED_RED_ON;
+                        }
+                        else
+                        {
-                        else NCMAG_Present = 1;
+                                NCMAG_Present = 1;
+                                NCMAG_IsCalibrated = NCMag_CalibrationRead();
+                                if(!NCMAG_IsCalibrated) UART1_PutString("\r\n Not calibrated!");
+                        }
+                }
                 else
+                {
-                        UART1_PutString("\n\r not compatible!");
+                        UART1_PutString("\n\r Not compatible!");
                         LED_RED_ON;
+                }
+        }
         else // nothing found
+        {

Subversion Repositories NaviCtrl

(root)/tags/V0.26f/ncmag.c @ 347 - Rev 253 → 254