Rev 256 | Rev 288 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 256 | Rev 287 | ||
|---|---|---|---|
| Line 259... | Line 259... | ||
| 259 | 259 | ||
| 260 | //---------------------------------------------------------------- |
260 | //---------------------------------------------------------------- |
| 261 | void MK3MAG_UpdateCompass(void) |
261 | void MK3MAG_UpdateCompass(void) |
| 262 | { |
262 | { |
| - | 263 | static u32 TimerCompassUpdate = 0; |
|
| Line 263... | Line 264... | ||
| 263 | static u32 TimerCompassUpdate = 0; |
264 | static s32 x_max,y_max,z_max,x_min,y_min,z_min; |
| Line 264... | Line 265... | ||
| 264 | 265 | ||
| 265 | if( (I2C_State == I2C_STATE_OFF) || !MK3MAG_Present ) return; |
266 | if( (I2C_State == I2C_STATE_OFF) || !MK3MAG_Present ) return; |
| Line 271... | Line 272... | ||
| 271 | MK3MAG_WriteCal.CalByte = Compass_CalState; |
272 | MK3MAG_WriteCal.CalByte = Compass_CalState; |
| 272 | // send new calstate |
273 | // send new calstate |
| 273 | if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte) |
274 | if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte) |
| 274 | { |
275 | { |
| 275 | MK3MAG_SendCommand(MK3MAG_CMD_WRITE_CAL); |
276 | MK3MAG_SendCommand(MK3MAG_CMD_WRITE_CAL); |
| - | 277 | x_max = -30000; y_max = -30000; z_max = -30000; |
|
| - | 278 | x_min = 30000; y_min = 30000; z_min = 30000; |
|
| - | 279 | } |
|
| - | 280 | if(MK3MAG_WriteCal.CalByte == 2) |
|
| - | 281 | { |
|
| - | 282 | Compass_UpdateMagVector(); |
|
| - | 283 | if(MagVector.X > x_max) { x_max = MagVector.X; BeepTime = 30; }; |
|
| - | 284 | if(MagVector.Y > y_max) { y_max = MagVector.Y; BeepTime = 30; }; |
|
| - | 285 | if(MagVector.X < x_min) { x_min = MagVector.X; BeepTime = 30; }; |
|
| - | 286 | if(MagVector.Y < y_min) { y_min = MagVector.Y; BeepTime = 30; }; |
|
| - | 287 | } |
|
| - | 288 | if(MK3MAG_WriteCal.CalByte == 4) |
|
| - | 289 | { |
|
| - | 290 | Compass_UpdateMagVector(); |
|
| - | 291 | if(MagVector.Z > z_max) { z_max = MagVector.Z; BeepTime = 30; }; |
|
| - | 292 | if(MagVector.Z < z_min) { z_min = MagVector.Z; BeepTime = 30; }; |
|
| 276 | } |
293 | } |
| 277 | else // request current heading |
294 | else // request current heading |
| 278 | { |
295 | { |
| 279 | MK3MAG_SendCommand(MK3MAG_CMD_READ_HEADING); |
296 | MK3MAG_SendCommand(MK3MAG_CMD_READ_HEADING); |
| 280 | } |
297 | } |