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"); |