Rev 253 | Rev 256 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 253 | Rev 254 | ||
---|---|---|---|
Line 52... | Line 52... | ||
52 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
52 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
53 | // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
53 | // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
54 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
54 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
55 | // + POSSIBILITY OF SUCH DAMAGE. |
55 | // + POSSIBILITY OF SUCH DAMAGE. |
56 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
56 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | 57 | #include <math.h> |
|
57 | #include <string.h> |
58 | #include <string.h> |
58 | #include "91x_lib.h" |
59 | #include "91x_lib.h" |
59 | #include "ncmag.h" |
60 | #include "ncmag.h" |
60 | #include "i2c.h" |
61 | #include "i2c.h" |
61 | #include "timer1.h" |
62 | #include "timer1.h" |
62 | #include "led.h" |
63 | #include "led.h" |
63 | #include "spi_slave.h" |
64 | #include "spi_slave.h" |
64 | #include "uart1.h" |
65 | #include "uart1.h" |
- | 66 | #include "eeprom.h" |
|
Line 65... | Line 67... | ||
65 | 67 | ||
- | 68 | u8 NCMAG_Present = 0; |
|
Line 66... | Line 69... | ||
66 | u8 NCMAG_Present = 0; |
69 | u8 NCMAG_IsCalibrated = 0; |
67 | 70 | ||
68 | #define MAG_TYPE_NONE 0 |
71 | #define MAG_TYPE_NONE 0 |
69 | #define MAG_TYPE_HMC5843 1 |
72 | #define MAG_TYPE_HMC5843 1 |
- | 73 | #define MAG_TYPE_LSM303DLH 2 |
|
- | 74 | u8 NCMAG_MagType = MAG_TYPE_NONE; |
|
- | 75 | ||
- | 76 | #define CALIBRATION_VERSION 1 |
|
- | 77 | #define EEPROM_ADR_MAG_CALIBRATION 50 |
|
- | 78 | ||
- | 79 | typedef struct |
|
- | 80 | { |
|
- | 81 | s16 Range; |
|
- | 82 | s16 Offset; |
|
- | 83 | } Scaling_t; |
|
- | 84 | ||
- | 85 | typedef struct |
|
- | 86 | { |
|
- | 87 | Scaling_t MagX; |
|
- | 88 | Scaling_t MagY; |
|
- | 89 | Scaling_t MagZ; |
|
- | 90 | u8 Version; |
|
- | 91 | u8 crc; |
|
- | 92 | } Calibration_t; |
|
Line 70... | Line 93... | ||
70 | #define MAG_TYPE_LSM303DLH 2 |
93 | |
71 | u8 NCMAG_MagType = MAG_TYPE_NONE; |
94 | Calibration_t Calibration; // calibration data in RAM |
Line 72... | Line 95... | ||
72 | 95 | ||
Line 207... | Line 230... | ||
207 | u8 ctrl_5; |
230 | u8 ctrl_5; |
208 | } __attribute__((packed)) AccConfig_t; |
231 | } __attribute__((packed)) AccConfig_t; |
Line 209... | Line 232... | ||
209 | 232 | ||
Line 210... | Line 233... | ||
210 | volatile AccConfig_t AccConfig; |
233 | volatile AccConfig_t AccConfig; |
- | 234 | ||
- | 235 | volatile s16vec_t AccRawVector; |
|
- | 236 | volatile s16vec_t MagRawVector; |
|
- | 237 | ||
- | 238 | ||
- | 239 | u8 NCMag_CalibrationWrite(void) |
|
- | 240 | { |
|
- | 241 | u8 i, crc = 0xAA; |
|
- | 242 | EEPROM_Result_t eres; |
|
- | 243 | u8 *pBuff = (u8*)&Calibration; |
|
- | 244 | ||
- | 245 | Calibration.Version = CALIBRATION_VERSION; |
|
- | 246 | for(i = 0; i<sizeof(Calibration)-1; i++) |
|
- | 247 | { |
|
- | 248 | crc += pBuff[i]; |
|
- | 249 | } |
|
- | 250 | Calibration.crc = ~crc; |
|
- | 251 | eres = EEPROM_WriteBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration)); |
|
- | 252 | DebugOut.Analog[25] = eres; |
|
- | 253 | if(EEPROM_SUCCESS == eres) i = 1; |
|
- | 254 | else i = 0; |
|
- | 255 | return(i); |
|
- | 256 | } |
|
- | 257 | ||
- | 258 | u8 NCMag_CalibrationRead(void) |
|
- | 259 | { |
|
- | 260 | u8 i, crc = 0xAA; |
|
- | 261 | u8 *pBuff = (u8*)&Calibration; |
|
- | 262 | ||
- | 263 | if(EEPROM_SUCCESS == EEPROM_ReadBlock(EEPROM_ADR_MAG_CALIBRATION, pBuff, sizeof(Calibration))) |
|
- | 264 | { |
|
- | 265 | for(i = 0; i<sizeof(Calibration)-1; i++) |
|
- | 266 | { |
|
- | 267 | crc += pBuff[i]; |
|
- | 268 | } |
|
- | 269 | crc = ~crc; |
|
- | 270 | if(Calibration.crc != crc) return(0); // crc mismatch |
|
- | 271 | if(Calibration.Version == CALIBRATION_VERSION) return(1); |
|
- | 272 | } |
|
- | 273 | return(0); |
|
- | 274 | } |
|
- | 275 | ||
- | 276 | ||
- | 277 | void NCMAG_Calibrate(void) |
|
- | 278 | { |
|
- | 279 | static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0; |
|
- | 280 | static u8 OldCalState = 0; |
|
- | 281 | ||
- | 282 | switch(Compass_CalState) |
|
- | 283 | { |
|
- | 284 | case 1: |
|
- | 285 | // 1st step of calibration |
|
- | 286 | // initialize ranges |
|
- | 287 | // used to change the orientation of the NC in the horizontal plane |
|
- | 288 | Xmin = 10000; |
|
- | 289 | Xmax = -10000; |
|
- | 290 | Ymin = 10000; |
|
- | 291 | Ymax = -10000; |
|
- | 292 | Zmin = 10000; |
|
- | 293 | Zmax = -10000; |
|
- | 294 | break; |
|
- | 295 | ||
- | 296 | case 2: // 2nd step of calibration |
|
- | 297 | // find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane |
|
- | 298 | if(MagRawVector.X < Xmin) Xmin = MagRawVector.X; |
|
- | 299 | if(MagRawVector.X > Xmax) Xmax = MagRawVector.X; |
|
- | 300 | if(MagRawVector.Y < Ymin) Ymin = MagRawVector.Y; |
|
- | 301 | if(MagRawVector.Y > Ymax) Ymax = MagRawVector.Y; |
|
- | 302 | break; |
|
- | 303 | ||
- | 304 | case 3: // 3rd step of calibration |
|
Line -... | Line 305... | ||
- | 305 | // used to change the orientation of the MK3MAG vertical to the horizontal plane |
|
- | 306 | break; |
|
- | 307 | ||
- | 308 | case 4: |
|
- | 309 | // find Min and Max of the Z-Sensor |
|
- | 310 | if(MagRawVector.Z < Zmin) Zmin = MagRawVector.Z; |
|
- | 311 | if(MagRawVector.Z > Zmax) Zmax = MagRawVector.Z; |
|
- | 312 | break; |
|
- | 313 | ||
- | 314 | case 5: |
|
- | 315 | // Save values |
|
- | 316 | if(Compass_CalState != OldCalState) // avoid continously writing of eeprom! |
|
- | 317 | { |
|
- | 318 | Calibration.MagX.Range = Xmax - Xmin; |
|
- | 319 | Calibration.MagX.Offset = (Xmin + Xmax) / 2; |
|
- | 320 | Calibration.MagY.Range = Ymax - Ymin; |
|
- | 321 | Calibration.MagY.Offset = (Ymin + Ymax) / 2; |
|
- | 322 | Calibration.MagZ.Range = Zmax - Zmin; |
|
- | 323 | Calibration.MagZ.Offset = (Zmin + Zmax) / 2; |
|
- | 324 | if(1)//if((Calibration.MagX.Range > 150) && (Calibration.MagY.Range > 150) && (Calibration.MagZ.Range > 150)) |
|
- | 325 | { |
|
- | 326 | NCMAG_IsCalibrated = NCMag_CalibrationWrite(); |
|
- | 327 | } |
|
- | 328 | else |
|
- | 329 | { |
|
- | 330 | // restore old calibration data from eeprom |
|
- | 331 | NCMAG_IsCalibrated = NCMag_CalibrationRead(); |
|
- | 332 | } |
|
- | 333 | } |
|
- | 334 | break; |
|
- | 335 | ||
- | 336 | default: |
|
- | 337 | break; |
|
Line 211... | Line 338... | ||
211 | 338 | } |
|
Line 212... | Line 339... | ||
212 | volatile s16vec_t AccVector; |
339 | OldCalState = Compass_CalState; |
213 | 340 | } |
|
214 | 341 | ||
215 | // ---------- call back handlers ----------------------------------------- |
342 | // ---------- call back handlers ----------------------------------------- |
216 | 343 | ||
217 | // rx data handler for id info request |
344 | // rx data handler for id info request |
218 | void NCMAG_UpdateIdentification(u8* pRxBuffer, u8 RxBufferSize) |
345 | void NCMAG_UpdateIdentification(u8* pRxBuffer, u8 RxBufferSize) |
219 | { // if number of byte are matching |
346 | { // if number of bytes are matching |
220 | if(RxBufferSize == sizeof(NCMAG_Identification) ) |
- | |
221 | { |
347 | if(RxBufferSize == sizeof(NCMAG_Identification) ) |
222 | memcpy((u8 *)&NCMAG_Identification, pRxBuffer, sizeof(NCMAG_Identification)); |
348 | { |
223 | } |
349 | memcpy((u8 *)&NCMAG_Identification, pRxBuffer, sizeof(NCMAG_Identification)); |
224 | } |
350 | } |
225 | 351 | } |
|
226 | // rx data handler for mag vector request |
352 | // rx data handler for magnetic sensor raw data |
227 | void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize) |
353 | void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize) |
228 | { // if number of byte are matching |
354 | { // if number of bytes are matching |
229 | if(RxBufferSize == sizeof(MagVector) ) |
355 | if(RxBufferSize == sizeof(MagRawVector) ) |
230 | { // byte order from big to little endian |
356 | { // byte order from big to little endian |
231 | MagVector.X = pRxBuffer[0]<<8; |
357 | MagRawVector.X = pRxBuffer[0]<<8; |
- | 358 | MagRawVector.X+= pRxBuffer[1]; |
|
- | 359 | MagRawVector.Y = pRxBuffer[2]<<8; |
|
- | 360 | MagRawVector.Y+= pRxBuffer[3]; |
|
- | 361 | MagRawVector.Z = pRxBuffer[4]<<8; |
|
- | 362 | MagRawVector.Z+= pRxBuffer[5]; |
|
- | 363 | } |
|
- | 364 | if(Compass_CalState || !NCMAG_IsCalibrated) |
|
- | 365 | { // direct output the raw data |
|
- | 366 | memcpy((u8*)&MagVector,(u8*)&MagRawVector, sizeof(MagVector)); |
|
- | 367 | Compass_Heading = -1; |
|
- | 368 | } |
|
- | 369 | else |
|
- | 370 | { |
|
- | 371 | // update MagVector from MagRaw Vector by Scaling |
|
- | 372 | MagVector.X = (s16)((1024L*(s32)(MagRawVector.X - Calibration.MagX.Offset))/Calibration.MagX.Range); |
|
- | 373 | MagVector.Y = (s16)((1024L*(s32)(MagRawVector.Y - Calibration.MagY.Offset))/Calibration.MagY.Range); |
|
- | 374 | MagVector.Z = (s16)((1024L*(s32)(MagRawVector.Z - Calibration.MagZ.Offset))/Calibration.MagZ.Range); |
|
- | 375 | // calculate attitude correction |
|
- | 376 | double Hx, Hy, Cx, Cy, Cz, nick_rad, roll_rad; |
|
- | 377 | Cx = (double)MagVector.X; |
|
- | 378 | Cy = (double)MagVector.Y; |
|
- | 379 | Cz = (double)MagVector.Z; |
|
- | 380 | nick_rad = ((double)FromFlightCtrl.AngleNick * M_PI) / 1800.0; |
|
- | 381 | roll_rad = ((double)FromFlightCtrl.AngleRoll * M_PI) / 1800.0; |
|
- | 382 | ||
- | 383 | Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad); |
|
- | 384 | Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad); |
|
- | 385 | ||
232 | MagVector.X+= pRxBuffer[1]; |
386 | //DebugOut.Analog[23] = (s16)Hx; |
233 | MagVector.Y = pRxBuffer[2]<<8; |
- | |
234 | MagVector.Y+= pRxBuffer[3]; |
- | |
235 | MagVector.Z = pRxBuffer[4]<<8; |
387 | //DebugOut.Analog[24] = (s16)Hy; |
236 | MagVector.Z+= pRxBuffer[5]; |
- | |
237 | } |
388 | |
238 | // tbd. calculate heading from mag vector and attitude |
389 | // calculate heading |
239 | CompassHeading = 50; // tbd. |
390 | Compass_Heading = (s16)((180.0 * atan2(Hy, Hx)) / M_PI); |
240 | } |
391 | } |
241 | 392 | } |
|
242 | // rx data handler for acc vector request |
393 | // rx data handler for acceleration raw data |
243 | void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize) |
394 | void NCMAG_UpdateAccVector(u8* pRxBuffer, u8 RxBufferSize) |
244 | { // if number of byte are matching |
395 | { // if number of byte are matching |
245 | if(RxBufferSize == sizeof(AccVector) ) |
- | |
- | 396 | if(RxBufferSize == sizeof(AccRawVector) ) |
|
246 | { |
397 | { |
247 | memcpy((u8*)&AccVector, pRxBuffer,sizeof(AccVector)); |
398 | memcpy((u8*)&AccRawVector, pRxBuffer,sizeof(AccRawVector)); |
248 | } |
399 | } |
249 | } |
400 | } |
250 | 401 | // rx data handler for reading magnetic sensor configuration |
|
251 | void NCMAG_UpdateMagConfig(u8* pRxBuffer, u8 RxBufferSize) |
402 | void NCMAG_UpdateMagConfig(u8* pRxBuffer, u8 RxBufferSize) |
252 | { // if number of byte are matching |
403 | { // if number of byte are matching |
253 | if(RxBufferSize == sizeof(MagConfig) ) |
- | |
- | 404 | if(RxBufferSize == sizeof(MagConfig) ) |
|
254 | { |
405 | { |
255 | memcpy((u8*)(&MagConfig), pRxBuffer, sizeof(MagConfig)); |
406 | memcpy((u8*)(&MagConfig), pRxBuffer, sizeof(MagConfig)); |
256 | } |
407 | } |
257 | } |
408 | } |
258 | 409 | // rx data handler for reading acceleration sensor configuration |
|
259 | void NCMAG_UpdateAccConfig(u8* pRxBuffer, u8 RxBufferSize) |
410 | void NCMAG_UpdateAccConfig(u8* pRxBuffer, u8 RxBufferSize) |
260 | { // if number of byte are matching |
411 | { // if number of byte are matching |
- | 412 | if(RxBufferSize == sizeof(AccConfig) ) |
|
Line -... | Line 413... | ||
- | 413 | { |
|
261 | if(RxBufferSize == sizeof(AccConfig) ) |
414 | memcpy((u8*)&AccConfig, pRxBuffer, sizeof(AccConfig)); |
262 | { |
415 | } |
263 | memcpy((u8*)&AccConfig, pRxBuffer, sizeof(AccConfig)); |
416 | } |
264 | } |
417 | //---------------------------------------------------------------------- |
265 | } |
418 | |
266 | 419 | ||
Line 394... | Line 547... | ||
394 | { |
547 | { |
395 | u16 TxBytes = 0; |
548 | u16 TxBytes = 0; |
396 | // set register pointer |
549 | // set register pointer |
397 | I2C_Buffer[TxBytes++] = REG_ACC_X_LSB; |
550 | I2C_Buffer[TxBytes++] = REG_ACC_X_LSB; |
398 | // initiate transmission |
551 | // initiate transmission |
399 | I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccVector)); |
552 | I2C_Transmission(ACC_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateAccVector, sizeof(AccRawVector)); |
400 | } |
553 | } |
401 | } |
554 | } |
Line 402... | Line 555... | ||
402 | 555 | ||
403 | // -------------------------------------------------------- |
556 | // -------------------------------------------------------- |
404 | void NCMAG_UpdateCompass(void) |
557 | void NCMAG_UpdateCompass(void) |
405 | { |
558 | { |
Line 406... | Line 559... | ||
406 | static u32 TimerCompassUpdate = 0; |
559 | static u32 TimerCompassUpdate = 0; |
- | 560 | ||
- | 561 | if( (I2C_State == I2C_STATE_OFF) || !NCMAG_Present ) |
|
- | 562 | { |
|
- | 563 | Compass_Heading = -1; |
|
Line 407... | Line 564... | ||
407 | 564 | return; |
|
408 | if( (I2C_State == I2C_STATE_OFF) || !NCMAG_Present ) return; |
565 | } |
409 | 566 | ||
410 | if(CheckDelay(TimerCompassUpdate)) |
- | |
411 | { |
- | |
412 | // check for incomming compass calibration request |
567 | if(CheckDelay(TimerCompassUpdate)) |
413 | // update CalByte from spi input queue |
568 | { |
414 | /*fifo_get(&CompassCalcStateFiFo, (u8 *)&(MK3MAG_WriteCal.CalByte)); |
- | |
415 | // send new calstate |
- | |
416 | if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte) |
- | |
417 | { |
- | |
418 | do the calibration here |
- | |
419 | } |
569 | // check for new calibration state |
420 | else // request current heading */ |
- | |
421 | { |
570 | Compass_UpdateCalState(); |
422 | NCMAG_GetMagVector(); |
571 | if(Compass_CalState) NCMAG_Calibrate(); |
423 | } |
572 | NCMAG_GetMagVector(); //Get new data; |
Line -... | Line 573... | ||
- | 573 | TimerCompassUpdate = SetDelay(20); // every 20 ms are 50 Hz |
|
424 | TimerCompassUpdate = SetDelay(20); // every 20 ms are 50 Hz |
574 | } |
425 | } |
575 | } |
426 | } |
576 | |
427 | 577 | // -------------------------------------------------------- |
|
428 | u8 NCMAG_SelfTest(void) |
578 | u8 NCMAG_SelfTest(void) |
Line 469... | Line 619... | ||
469 | for(i = 0; i<AVERAGE; i++) |
619 | for(i = 0; i<AVERAGE; i++) |
470 | { |
620 | { |
471 | NCMAG_GetMagVector(); |
621 | NCMAG_GetMagVector(); |
472 | time = SetDelay(20); |
622 | time = SetDelay(20); |
473 | while(!CheckDelay(time)); |
623 | while(!CheckDelay(time)); |
474 | XMax += MagVector.X; |
624 | XMax += MagRawVector.X; |
475 | YMax += MagVector.Y; |
625 | YMax += MagRawVector.Y; |
476 | ZMax += MagVector.Z; |
626 | ZMax += MagRawVector.Z; |
477 | } |
627 | } |
478 | MagConfig.cra = cra_rate|CRA_MODE_NEGBIAS; |
628 | MagConfig.cra = cra_rate|CRA_MODE_NEGBIAS; |
479 | // activate positive bias field |
629 | // activate positive bias field |
480 | NCMAG_SetMagConfig(); |
630 | NCMAG_SetMagConfig(); |
481 | // wait for stable readings |
631 | // wait for stable readings |
Line 485... | Line 635... | ||
485 | for(i = 0; i < AVERAGE; i++) |
635 | for(i = 0; i < AVERAGE; i++) |
486 | { |
636 | { |
487 | NCMAG_GetMagVector(); |
637 | NCMAG_GetMagVector(); |
488 | time = SetDelay(20); |
638 | time = SetDelay(20); |
489 | while(!CheckDelay(time)); |
639 | while(!CheckDelay(time)); |
490 | XMin += MagVector.X; |
640 | XMin += MagRawVector.X; |
491 | YMin += MagVector.Y; |
641 | YMin += MagRawVector.Y; |
492 | ZMin += MagVector.Z; |
642 | ZMin += MagRawVector.Z; |
493 | } |
643 | } |
494 | // setup final configuration |
644 | // setup final configuration |
495 | MagConfig.cra = cra_rate|CRA_MODE_NORMAL; |
645 | MagConfig.cra = cra_rate|CRA_MODE_NORMAL; |
496 | // activate positive bias field |
646 | // activate positive bias field |
497 | NCMAG_SetMagConfig(); |
647 | NCMAG_SetMagConfig(); |
Line 555... | Line 705... | ||
555 | if(!NCMAG_SelfTest()) |
705 | if(!NCMAG_SelfTest()) |
556 | { |
706 | { |
557 | UART1_PutString(" Selftest failed!"); |
707 | UART1_PutString(" Selftest failed!"); |
558 | LED_RED_ON; |
708 | LED_RED_ON; |
559 | } |
709 | } |
- | 710 | else |
|
- | 711 | { |
|
560 | else NCMAG_Present = 1; |
712 | NCMAG_Present = 1; |
- | 713 | NCMAG_IsCalibrated = NCMag_CalibrationRead(); |
|
- | 714 | if(!NCMAG_IsCalibrated) UART1_PutString("\r\n Not calibrated!"); |
|
- | 715 | } |
|
561 | } |
716 | } |
562 | else |
717 | else |
563 | { |
718 | { |
564 | UART1_PutString("\n\r not compatible!"); |
719 | UART1_PutString("\n\r Not compatible!"); |
565 | LED_RED_ON; |
720 | LED_RED_ON; |
566 | } |
721 | } |
567 | } |
722 | } |
568 | else // nothing found |
723 | else // nothing found |
569 | { |
724 | { |