Rev 292 | 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 |