Subversion Repositories NaviCtrl

Rev

Rev 487 | Rev 489 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 487 Rev 488
Line 341... Line 341...
341 
        static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0, Zmin2 = 0, Zmax2 = 0;
 341 
        static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0, Zmin2 = 0, Zmax2 = 0;
342 
        static s16 X = 0, Y = 0, Z = 0;
 342 
        static s16 X = 0, Y = 0, Z = 0;
343 
        static u8 OldCalState = 0;     
 343 
        static u8 OldCalState = 0;     
344 
        s16 MinCalibration = 450;
 344 
        s16 MinCalibration = 450;
Line 345... Line 345...
345 
 
 345 
 
346 
        X = (4*X + MagRawVector.X + 3)/5;
 346 
        X = (X + MagRawVector.X)/2;
347 
        Y = (4*Y + MagRawVector.Y + 3)/5;
 347 
        Y = (Y + MagRawVector.Y)/2;
Line 348... Line 348...
348 
        Z = (4*Z + MagRawVector.Z + 3)/5;
 348 
        Z = (Z + MagRawVector.Z)/2;
349 
 
 349 
 
350 
        switch(Compass_CalState)
 350 
        switch(Compass_CalState)
351 
        {
 351 
        {
Line 358... Line 358...
358 
                        Ymin =  10000;
 358 
                        Ymin =  10000;
359 
                        Ymax = -10000;
 359 
                        Ymax = -10000;
360 
                        Zmin =  10000;
 360 
                        Zmin =  10000;
361 
                        Zmax = -10000;
 361 
                        Zmax = -10000;
362 
                        speak = 1;
 362 
                        speak = 1;
- 
 
 363 
                        CompassValueErrorCount = 0;
363 
                        if(Compass_CalState != OldCalState) // avoid continously writing of eeprom!
 364 
                        if(Compass_CalState != OldCalState) // avoid continously writing of eeprom!
364 
                        {
 365 
                        {
365 
                         UART1_PutString("\r\nStarting compass calibration");
 366 
                         UART1_PutString("\r\nStarting compass calibration");
366 
                         if(I2C_CompassPort == I2C_EXTERN_0)
 367 
                         if(I2C_CompassPort == I2C_EXTERN_0)
367 
                         {
 368 
                         {
Line 435... Line 436...
435 
                                Calibration.MagX.Offset = (Xmin + Xmax) / 2;
 436 
                                Calibration.MagX.Offset = (Xmin + Xmax) / 2;
436 
                                Calibration.MagY.Range = Ymax - Ymin;
 437 
                                Calibration.MagY.Range = Ymax - Ymin;
437 
                                Calibration.MagY.Offset = (Ymin + Ymax) / 2;
 438 
                                Calibration.MagY.Offset = (Ymin + Ymax) / 2;
438 
                                Calibration.MagZ.Range = Zmax - Zmin;
 439 
                                Calibration.MagZ.Range = Zmax - Zmin;
439 
                                Calibration.MagZ.Offset = (Zmin + Zmax) / 2;
 440 
                                Calibration.MagZ.Offset = (Zmin + Zmax) / 2;
- 
 
 441 
                                if(CompassValueErrorCount)
- 
 
 442 
                                 {
- 
 
 443 
                                        SpeakHoTT = SPEAK_ERR_CALIBARTION;
- 
 
 444 
                                        UART1_PutString("\r\nCalibration FAILED - Compass sensor error !!!!\r\n ");
- 
 
 445 
 
- 
 
 446 
                                }
- 
 
 447 
                                else
440 
                                if((Calibration.MagX.Range > MinCalibration) && (Calibration.MagY.Range > MinCalibration) && (Calibration.MagZ.Range > MinCalibration))
 448 
                                if((Calibration.MagX.Range > MinCalibration) && (Calibration.MagY.Range > MinCalibration) && (Calibration.MagZ.Range > MinCalibration))
441 
                                {
 449 
                                {
442 
                                        NCMAG_IsCalibrated = NCMag_CalibrationWrite(I2C_CompassPort);
 450 
                                        NCMAG_IsCalibrated = NCMag_CalibrationWrite(I2C_CompassPort);
443 
                                        BeepTime = 2500;
 451 
                                        BeepTime = 2500;
444 
                                        UART1_PutString("\r\n-> Calibration okay <-\n\r");
 452 
                                        UART1_PutString("\r\n-> Calibration okay <-\n\r");