Rev 242 | Rev 248 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 242 | Rev 243 | ||
---|---|---|---|
Line 146... | Line 146... | ||
146 | 146 | ||
147 | // rx data handler for mag vector request |
147 | // rx data handler for mag vector request |
148 | void HMC5843_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize) |
148 | void HMC5843_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize) |
149 | { // if number of byte are matching |
149 | { // if number of byte are matching |
150 | if(RxBufferSize == sizeof(MagVector) ) |
- | |
- | 150 | if(RxBufferSize == sizeof(MagVector) ) |
|
151 | { |
151 | { // byte order from big to little endian |
152 | MagVector.X = pRxBuffer[0]<<8; |
152 | MagVector.X = pRxBuffer[0]<<8; |
153 | MagVector.X+= pRxBuffer[1]; |
153 | MagVector.X+= pRxBuffer[1]; |
154 | MagVector.Y = pRxBuffer[2]<<8; |
154 | MagVector.Y = pRxBuffer[2]<<8; |
155 | MagVector.Y+= pRxBuffer[3]; |
155 | MagVector.Y+= pRxBuffer[3]; |
Line 193... | Line 193... | ||
193 | I2C_Transmission(HMC5843_SLAVE_ADDRESS, NULL, 0); |
193 | I2C_Transmission(HMC5843_SLAVE_ADDRESS, NULL, 0); |
194 | return 1; |
194 | return 1; |
195 | } |
195 | } |
Line 196... | Line 196... | ||
196 | 196 | ||
- | 197 | //---------------------------------------------------------------- |
|
- | 198 | void HMC5843_GetMagVector(void) |
|
- | 199 | { |
|
- | 200 | if(I2C_State == I2C_STATE_IDLE) |
|
- | 201 | { |
|
- | 202 | I2C_TxBufferSize = 0; |
|
- | 203 | // set register pointer |
|
- | 204 | I2C_TxBuffer[I2C_TxBufferSize++] = REGISTER_DATAX_MSB; |
|
- | 205 | // initiate transmission |
|
- | 206 | I2C_Transmission(HMC5843_SLAVE_ADDRESS, &HMC5843_UpdateMagVector, sizeof(MagVector)); |
|
- | 207 | } |
|
- | 208 | } |
|
- | 209 | ||
- | 210 | void HMC5843_UpdateCompass(void) |
|
- | 211 | { |
|
- | 212 | static u32 TimerCompassUpdate = 0; |
|
- | 213 | ||
- | 214 | if( (I2C_State == I2C_STATE_OFF) || !HMC5843_Present ) return; |
|
- | 215 | ||
- | 216 | if(CheckDelay(TimerCompassUpdate)) |
|
- | 217 | { |
|
- | 218 | // check for incomming compass calibration request |
|
- | 219 | // update CalByte from spi input queue |
|
- | 220 | /*fifo_get(&CompassCalcStateFiFo, (u8 *)&(MK3MAG_WriteCal.CalByte)); |
|
- | 221 | // send new calstate |
|
- | 222 | if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte) |
|
- | 223 | { |
|
- | 224 | do the calibration here |
|
- | 225 | } |
|
- | 226 | else // request current heading */ |
|
- | 227 | { |
|
- | 228 | HMC5843_GetMagVector(); |
|
- | 229 | } |
|
- | 230 | TimerCompassUpdate = SetDelay(20); // every 20 ms are 50 Hz |
|
- | 231 | } |
|
- | 232 | } |
|
- | 233 | ||
- | 234 | u8 HMC5843_SelfTest(void) |
|
- | 235 | { |
|
- | 236 | u32 time; |
|
- | 237 | s32 XMin = 0, XMax = 0, YMin = 0, YMax = 0, ZMin = 0, ZMax = 0; |
|
- | 238 | s32 scale, scale_min, scale_max; |
|
- | 239 | u8 i = 0; |
|
- | 240 | ||
- | 241 | // activate positive bias field of 0.55 gauss |
|
- | 242 | HMC5843_SetConfiguration(HMC5843_CRA_RATE_50HZ|HMC5843_CRA_MODE_POSBIAS, HMC5843_CRB_GAIN_10GA, HMC5843_MODE_CONTINUOUS); |
|
- | 243 | // wait a while |
|
- | 244 | time = SetDelay(55); |
|
- | 245 | while(!CheckDelay(time)); |
|
- | 246 | // averaging |
|
- | 247 | for(i = 0; i<20; i++) |
|
- | 248 | { |
|
- | 249 | HMC5843_GetMagVector(); |
|
- | 250 | time = SetDelay(20); |
|
- | 251 | while(!CheckDelay(time)); |
|
- | 252 | XMax += MagVector.X; |
|
- | 253 | YMax += MagVector.Y; |
|
- | 254 | ZMax += MagVector.Z; |
|
- | 255 | } |
|
- | 256 | // activate negative bias field of 0.55 gauss |
|
- | 257 | HMC5843_SetConfiguration(HMC5843_CRA_RATE_50HZ|HMC5843_CRA_MODE_NEGBIAS, HMC5843_CRB_GAIN_10GA, HMC5843_MODE_CONTINUOUS); |
|
- | 258 | // wait a while |
|
- | 259 | time = SetDelay(55); |
|
- | 260 | while(!CheckDelay(time)); |
|
- | 261 | // averaging |
|
- | 262 | for(i = 0; i < 20; i++) |
|
- | 263 | { |
|
- | 264 | HMC5843_GetMagVector(); |
|
- | 265 | time = SetDelay(20); |
|
- | 266 | while(!CheckDelay(time)); |
|
- | 267 | XMin += MagVector.X; |
|
- | 268 | YMin += MagVector.Y; |
|
- | 269 | ZMin += MagVector.Z; |
|
- | 270 | } |
|
- | 271 | // setup final configuration |
|
- | 272 | HMC5843_SetConfiguration(HMC5843_CRA_RATE_50HZ|HMC5843_CRA_MODE_NORMAL, HMC5843_CRB_GAIN_10GA, HMC5843_MODE_CONTINUOUS); |
|
- | 273 | // prepare scale limits |
|
- | 274 | scale = 715; // 1300 counts/Gauss * 0.55 Gauss = 715 counts |
|
- | 275 | scale_min = (scale * 90)/100; |
|
- | 276 | scale_max = (scale * 110)/100; |
|
- | 277 | // check scale for all axes |
|
- | 278 | scale = (XMax - XMin)/40; |
|
- | 279 | if((scale > scale_max) && (scale < scale_min)) return 0; |
|
- | 280 | scale = (YMax + YMin)/40; |
|
- | 281 | if((scale > scale_max) && (scale < scale_min)) return 0; |
|
- | 282 | scale = (ZMax + ZMin)/40; |
|
- | 283 | if((scale > scale_max) && (scale < scale_min)) return 0; |
|
- | 284 | return 1; |
|
- | 285 | } |
|
- | 286 | ||
- | 287 | ||
197 | //---------------------------------------------------------------- |
288 | //---------------------------------------------------------------- |
198 | u8 HMC5843_Init(void) |
289 | u8 HMC5843_Init(void) |
199 | { |
290 | { |
200 | u8 msg[64]; |
291 | u8 msg[64]; |
201 | u8 repeat; |
292 | u8 repeat; |
Line 221... | Line 312... | ||
221 | repeat++; |
312 | repeat++; |
222 | }while ((HMC5843_Identification.A == 0xFF) && (repeat < 12)); // 12*250ms=3s |
313 | }while ((HMC5843_Identification.A == 0xFF) && (repeat < 12)); // 12*250ms=3s |
223 | // if we got it |
314 | // if we got it |
224 | if(HMC5843_Identification.A != 0xFF) |
315 | if(HMC5843_Identification.A != 0xFF) |
225 | { |
316 | { |
226 | sprintf(msg, " HMC5843 ID %d/%d/%d.", HMC5843_Identification.A, HMC5843_Identification.B, HMC5843_Identification.C); |
317 | sprintf(msg, " HMC5843 ID %d/%d/%d", HMC5843_Identification.A, HMC5843_Identification.B, HMC5843_Identification.C); |
227 | UART1_PutString(msg); |
318 | UART1_PutString(msg); |
Line 228... | Line 319... | ||
228 | 319 | ||
229 | if ( (HMC5843_Identification.A == HMC5843_IDA) |
320 | if ( (HMC5843_Identification.A == HMC5843_IDA) |
230 | && (HMC5843_Identification.B == HMC5843_IDB) |
321 | && (HMC5843_Identification.B == HMC5843_IDB) |
231 | && (HMC5843_Identification.C == HMC5843_IDC)) |
322 | && (HMC5843_Identification.C == HMC5843_IDC)) |
232 | { |
323 | { |
- | 324 | //HMC5843_SetConfiguration(HMC5843_CRA_RATE_50HZ|HMC5843_CRA_MODE_NORMAL, HMC5843_CRB_GAIN_10GA, HMC5843_MODE_CONTINUOUS); |
|
- | 325 | if(!HMC5843_SelfTest()) |
|
- | 326 | { |
|
- | 327 | UART1_PutString("\n\r HMC5843 selftest failed!"); |
|
- | 328 | LED_RED_ON; |
|
233 | HMC5843_SetConfiguration(HMC5843_CRA_RATE_50HZ|HMC5843_CRA_MODE_NORMAL, HMC5843_CRB_GAIN_10GA, HMC5843_MODE_CONTINUOUS); |
329 | } |
234 | HMC5843_Present = 1; |
330 | else HMC5843_Present = 1; |
235 | } |
331 | } |
236 | else |
332 | else |
237 | { |
333 | { |
238 | UART1_PutString("\n\r HMC5843 not compatible!"); |
334 | UART1_PutString("\n\r HMC5843 not compatible!"); |
239 | LED_RED_ON; |
335 | LED_RED_ON; |
240 | } |
336 | } |
241 | } |
337 | } |
242 | return(HMC5843_Present); |
338 | return(HMC5843_Present); |
Line 243... | Line -... | ||
243 | } |
- | |
244 | - | ||
245 | - | ||
246 | //---------------------------------------------------------------- |
- | |
247 | void HMC5843_GetMagVector(void) |
- | |
248 | { |
- | |
249 | if(I2C_State == I2C_STATE_IDLE) |
- | |
250 | { |
- | |
251 | I2C_TxBufferSize = 0; |
- | |
252 | // set register pointer |
- | |
253 | I2C_TxBuffer[I2C_TxBufferSize++] = REGISTER_DATAX_MSB; |
- | |
254 | // initiate transmission |
- | |
255 | I2C_Transmission(HMC5843_SLAVE_ADDRESS, &HMC5843_UpdateMagVector, sizeof(MagVector)); |
- | |
256 | } |
- | |
257 | } |
- | |
258 | - | ||
259 | void HMC5843_UpdateCompass(void) |
- | |
260 | { |
- | |
261 | static u32 TimerCompassUpdate = 0; |
- | |
262 | - | ||
263 | if( (I2C_State == I2C_STATE_OFF) || !HMC5843_Present ) return; |
- | |
264 | - | ||
265 | if(CheckDelay(TimerCompassUpdate)) |
- | |
266 | { |
- | |
267 | // check for incomming compass calibration request |
- | |
268 | // update CalByte from spi input queue |
- | |
269 | /*fifo_get(&CompassCalcStateFiFo, (u8 *)&(MK3MAG_WriteCal.CalByte)); |
- | |
270 | // send new calstate |
- | |
271 | if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte) |
- | |
272 | { |
- | |
273 | do the calibration here |
- | |
274 | } |
- | |
275 | else // request current heading */ |
- | |
276 | { |
- | |
277 | HMC5843_GetMagVector(); |
- | |
278 | } |
- | |
279 | TimerCompassUpdate = SetDelay(20); // every 20 ms are 50 Hz |
- |