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 | } |