Rev 292 | Rev 342 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 292 | Rev 339 | ||
|---|---|---|---|
| Line 263... | Line 263... | ||
| 263 | //---------------------------------------------------------------- |
263 | //---------------------------------------------------------------- |
| 264 | void MK3MAG_Update(void) |
264 | void MK3MAG_Update(void) |
| 265 | { |
265 | { |
| 266 | static u32 TimerUpdate = 0; |
266 | static u32 TimerUpdate = 0; |
| 267 | static s16 x_max,y_max,z_max,x_min,y_min,z_min; |
267 | static s16 x_max,y_max,z_max,x_min,y_min,z_min; |
| - | 268 | static u8 last_state; |
|
| - | 269 | u8 msg[64]; |
|
| - | 270 | u16 MinCaclibration = 500; |
|
| Line 268... | Line 271... | ||
| 268 | 271 | ||
| Line 269... | Line 272... | ||
| 269 | if( (I2C_State == I2C_STATE_OFF) || !MK3MAG_Present ) return; |
272 | if( (I2C_State == I2C_STATE_OFF) || !MK3MAG_Present ) return; |
| 270 | 273 | ||
| Line 283... | Line 286... | ||
| 283 | MK3MAG_SendCommand(MK3MAG_CMD_READ_MAGVECT); // initiate magvector transfer |
286 | MK3MAG_SendCommand(MK3MAG_CMD_READ_MAGVECT); // initiate magvector transfer |
| Line 284... | Line 287... | ||
| 284 | 287 | ||
| 285 | switch(Compass_CalState) |
288 | switch(Compass_CalState) |
| 286 | { |
289 | { |
| - | 290 | case 1: |
|
| - | 291 | if(last_state != Compass_CalState) |
|
| - | 292 | { |
|
| - | 293 | UART1_PutString("\r\nMK3Mag calibration\n\r"); |
|
| - | 294 | } |
|
| 287 | case 1: |
295 | |
| 288 | x_max = -30000; y_max = -30000; z_max = -30000; |
296 | x_max = -30000; y_max = -30000; z_max = -30000; |
| 289 | x_min = 30000; y_min = 30000; z_min = 30000; |
297 | x_min = 30000; y_min = 30000; z_min = 30000; |
| Line 290... | Line 298... | ||
| 290 | break; |
298 | break; |
| Line 298... | Line 306... | ||
| 298 | 306 | ||
| 299 | case 4: |
307 | case 4: |
| 300 | if(MagVector.Z > z_max) { z_max = MagVector.Z; BeepTime = 80; } |
308 | if(MagVector.Z > z_max) { z_max = MagVector.Z; BeepTime = 80; } |
| 301 | else if(MagVector.Z < z_min) { z_min = MagVector.Z; BeepTime = 80; } |
309 | else if(MagVector.Z < z_min) { z_min = MagVector.Z; BeepTime = 80; } |
| - | 310 | break; |
|
| - | 311 | case 5: |
|
| - | 312 | if(last_state == Compass_CalState) break; |
|
| - | 313 | if(((x_max - x_min) > MinCaclibration) && ((y_max - y_min) > MinCaclibration) && ((z_max - z_min) > MinCaclibration)) |
|
| - | 314 | { |
|
| - | 315 | BeepTime = 2500; |
|
| - | 316 | UART1_PutString("\r\n Calibration okay\n\r"); |
|
| - | 317 | } |
|
| - | 318 | else |
|
| - | 319 | { |
|
| - | 320 | UART1_PutString("\r\nCalibration FAILED - Values too low: "); |
|
| - | 321 | if((x_max - x_min) < MinCaclibration) UART1_PutString("X! "); |
|
| - | 322 | if((y_max - y_min) < MinCaclibration) UART1_PutString("y! "); |
|
| - | 323 | if((z_max - z_min) < MinCaclibration) UART1_PutString("Z! "); |
|
| - | 324 | UART1_PutString("\r\n"); |
|
| - | 325 | sprintf(msg, "Minimum is: %i \r\n",MinCaclibration); |
|
| - | 326 | } |
|
| - | 327 | UART1_PutString(msg); |
|
| - | 328 | sprintf(msg, "\r\nX: (%i - %i = %i)\r\n",x_max,x_min,x_max - x_min); |
|
| - | 329 | UART1_PutString(msg); |
|
| - | 330 | sprintf(msg, "Y: (%i - %i = %i)\r\n",y_max,y_min,y_max - y_min); |
|
| - | 331 | UART1_PutString(msg); |
|
| - | 332 | sprintf(msg, "Z: (%i - %i = %i)\r\n",z_max,z_min,z_max - z_min); |
|
| Line -... | Line 333... | ||
| - | 333 | UART1_PutString(msg); |
|
| 302 | break; |
334 | |
| 303 | 335 | break; |
|
| 304 | default: |
336 | default: |
| - | 337 | break; |
|
| 305 | break; |
338 | } |
| 306 | } |
339 | last_state = Compass_CalState; |
| 307 | } |
340 | } |
| 308 | TimerUpdate = SetDelay(20); // every 20 ms are 50 Hz |
341 | TimerUpdate = SetDelay(20); // every 20 ms are 50 Hz |