Rev 338 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 338 | Rev 339 | ||
|---|---|---|---|
| Line 75... | Line 75... | ||
| 75 | #define MAG_TYPE_LSM303DLH 2 |
75 | #define MAG_TYPE_LSM303DLH 2 |
| 76 | u8 NCMAG_MagType = MAG_TYPE_NONE; |
76 | u8 NCMAG_MagType = MAG_TYPE_NONE; |
| Line 77... | Line 77... | ||
| 77 | 77 | ||
| 78 | #define CALIBRATION_VERSION 1 |
78 | #define CALIBRATION_VERSION 1 |
| 79 | #define EEPROM_ADR_MAG_CALIBRATION 50 |
79 | #define EEPROM_ADR_MAG_CALIBRATION 50 |
| Line 80... | Line 80... | ||
| 80 | #define MAG_CALIBRATION_COMPATIBEL 0xA1 |
80 | #define MAG_CALIBRATION_COMPATIBEL 0xA2 |
| 81 | 81 | ||
| 82 | #define NCMAG_MIN_RAWVALUE -2047 |
82 | #define NCMAG_MIN_RAWVALUE -2047 |
| Line 97... | Line 97... | ||
| 97 | u8 Version; |
97 | u8 Version; |
| 98 | u8 crc; |
98 | u8 crc; |
| 99 | } __attribute__((packed)) Calibration_t; |
99 | } __attribute__((packed)) Calibration_t; |
| Line 100... | Line 100... | ||
| 100 | 100 | ||
| - | 101 | Calibration_t Calibration; // calibration data in RAM |
|
| - | 102 | volatile s16vec_t AccRawVector; |
|
| Line 101... | Line 103... | ||
| 101 | Calibration_t Calibration; // calibration data in RAM |
103 | volatile s16vec_t MagRawVector; |
| 102 | 104 | ||
| Line 103... | Line 105... | ||
| 103 | // i2c MAG interface |
105 | // i2c MAG interface |
| Line 155... | Line 157... | ||
| 155 | #define HMC5843_CRA_RATE_20HZ 0x14 |
157 | #define HMC5843_CRA_RATE_20HZ 0x14 |
| 156 | #define HMC5843_CRA_RATE_50HZ 0x18 |
158 | #define HMC5843_CRA_RATE_50HZ 0x18 |
| 157 | // bit mask for gain |
159 | // bit mask for gain |
| 158 | #define HMC5843_CRB_GAIN_07GA 0x00 |
160 | #define HMC5843_CRB_GAIN_07GA 0x00 |
| 159 | #define HMC5843_CRB_GAIN_10GA 0x20 //default |
161 | #define HMC5843_CRB_GAIN_10GA 0x20 //default |
| 160 | #define HMC5843_CRB_GAIN_15GA 0x40 |
162 | #define HMC5843_CRB_GAIN_15GA 0x40 // <--- we use this |
| 161 | #define HMC5843_CRB_GAIN_20GA 0x60 |
163 | #define HMC5843_CRB_GAIN_20GA 0x60 |
| 162 | #define HMC5843_CRB_GAIN_32GA 0x80 |
164 | #define HMC5843_CRB_GAIN_32GA 0x80 |
| 163 | #define HMC5843_CRB_GAIN_38GA 0xA0 |
165 | #define HMC5843_CRB_GAIN_38GA 0xA0 |
| 164 | #define HMC5843_CRB_GAIN_45GA 0xC0 |
166 | #define HMC5843_CRB_GAIN_45GA 0xC0 |
| 165 | #define HMC5843_CRB_GAIN_65GA 0xE0 |
167 | #define HMC5843_CRB_GAIN_65GA 0xE0 |
| 166 | // self test value |
168 | // self test value |
| 167 | #define HMC5843_TEST_XSCALE 715 |
169 | #define HMC5843_TEST_XSCALE 555 |
| 168 | #define HMC5843_TEST_YSCALE 715 |
170 | #define HMC5843_TEST_YSCALE 555 |
| 169 | #define HMC5843_TEST_ZSCALE 715 |
171 | #define HMC5843_TEST_ZSCALE 555 |
| 170 | - | ||
| - | 172 | // clibration range |
|
| - | 173 | #define HMC5843_CALIBRATION_RANGE 550 |
|
| Line 171... | Line 174... | ||
| 171 | 174 | ||
| 172 | // the special LSM302DLH interface |
175 | // the special LSM302DLH interface |
| 173 | // bit mask for rate |
176 | // bit mask for rate |
| 174 | #define LSM303DLH_CRA_RATE_0_75HZ 0x00 |
177 | #define LSM303DLH_CRA_RATE_0_75HZ 0x00 |
| Line 180... | Line 183... | ||
| 180 | #define LSM303DLH_CRA_RATE_75HZ 0x18 |
183 | #define LSM303DLH_CRA_RATE_75HZ 0x18 |
| Line 181... | Line 184... | ||
| 181 | 184 | ||
| 182 | // bit mask for gain |
185 | // bit mask for gain |
| 183 | #define LSM303DLH_CRB_GAIN_XXGA 0x00 |
186 | #define LSM303DLH_CRB_GAIN_XXGA 0x00 |
| 184 | #define LSM303DLH_CRB_GAIN_13GA 0x20 //default |
187 | #define LSM303DLH_CRB_GAIN_13GA 0x20 //default |
| 185 | #define LSM303DLH_CRB_GAIN_19GA 0x40 |
188 | #define LSM303DLH_CRB_GAIN_19GA 0x40 // <--- we use this |
| 186 | #define LSM303DLH_CRB_GAIN_25GA 0x60 |
189 | #define LSM303DLH_CRB_GAIN_25GA 0x60 |
| 187 | #define LSM303DLH_CRB_GAIN_40GA 0x80 |
190 | #define LSM303DLH_CRB_GAIN_40GA 0x80 |
| 188 | #define LSM303DLH_CRB_GAIN_47GA 0xA0 |
191 | #define LSM303DLH_CRB_GAIN_47GA 0xA0 |
| 189 | #define LSM303DLH_CRB_GAIN_56GA 0xC0 |
192 | #define LSM303DLH_CRB_GAIN_56GA 0xC0 |
| 190 | #define LSM303DLH_CRB_GAIN_81GA 0xE0 |
193 | #define LSM303DLH_CRB_GAIN_81GA 0xE0 |
| 191 | // self test value |
194 | // self test value |
| 192 | #define LSM303DLH_TEST_XSCALE 495 |
195 | #define LSM303DLH_TEST_XSCALE 495 |
| 193 | #define LSM303DLH_TEST_YSCALE 495 |
196 | #define LSM303DLH_TEST_YSCALE 495 |
| - | 197 | #define LSM303DLH_TEST_ZSCALE 470 |
|
| - | 198 | // clibration range |
|
| Line 194... | Line 199... | ||
| 194 | #define LSM303DLH_TEST_ZSCALE 470 |
199 | #define LSM303_CALIBRATION_RANGE 500 |
| 195 | 200 | ||
| 196 | // the i2c ACC interface |
201 | // the i2c ACC interface |
| 197 | #define ACC_SLAVE_ADDRESS 0x30 // i2c slave for acc. sensor registers |
202 | #define ACC_SLAVE_ADDRESS 0x30 // i2c slave for acc. sensor registers |
| Line 245... | Line 250... | ||
| 245 | u8 ctrl_5; |
250 | u8 ctrl_5; |
| 246 | } __attribute__((packed)) AccConfig_t; |
251 | } __attribute__((packed)) AccConfig_t; |
| Line 247... | Line 252... | ||
| 247 | 252 | ||
| Line 248... | Line -... | ||
| 248 | volatile AccConfig_t AccConfig; |
- | |
| 249 | - | ||
| 250 | volatile s16vec_t AccRawVector; |
- | |
| 251 | volatile s16vec_t MagRawVector; |
- | |
| 252 | 253 | volatile AccConfig_t AccConfig; |
|
| 253 | 254 | ||
| 254 | u8 NCMag_CalibrationWrite(void) |
255 | u8 NCMag_CalibrationWrite(void) |
| 255 | { |
256 | { |
| 256 | u8 i, crc = MAG_CALIBRATION_COMPATIBEL; |
257 | u8 i, crc = MAG_CALIBRATION_COMPATIBEL; |
| Line 292... | Line 293... | ||
| 292 | { |
293 | { |
| 293 | u8 msg[64]; |
294 | u8 msg[64]; |
| 294 | static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0; |
295 | static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0; |
| 295 | static s16 X = 0, Y = 0, Z = 0; |
296 | static s16 X = 0, Y = 0, Z = 0; |
| 296 | static u8 OldCalState = 0; |
297 | static u8 OldCalState = 0; |
| - | 298 | s16 MinCaclibration = 450; |
|
| Line 297... | Line 299... | ||
| 297 | 299 | ||
| 298 | X = (4*X + MagRawVector.X + 3)/5; |
300 | X = (4*X + MagRawVector.X + 3)/5; |
| 299 | Y = (4*Y + MagRawVector.Y + 3)/5; |
301 | Y = (4*Y + MagRawVector.Y + 3)/5; |
| Line 334... | Line 336... | ||
| 334 | case 5: |
336 | case 5: |
| 335 | // Save values |
337 | // Save values |
| 336 | if(Compass_CalState != OldCalState) // avoid continously writing of eeprom! |
338 | if(Compass_CalState != OldCalState) // avoid continously writing of eeprom! |
| 337 | { |
339 | { |
| 338 | // #define MIN_CALIBRATION 256 |
340 | // #define MIN_CALIBRATION 256 |
| - | 341 | if(NCMAG_MagType == MAG_TYPE_HMC5843) |
|
| - | 342 | { |
|
| - | 343 | UART1_PutString("\r\nHMC5843 calibration\n\r"); |
|
| - | 344 | MinCaclibration = HMC5843_CALIBRATION_RANGE; |
|
| - | 345 | } |
|
| - | 346 | if(NCMAG_MagType == MAG_TYPE_LSM303DLH) |
|
| - | 347 | { |
|
| - | 348 | UART1_PutString("\r\n\r\nLSM303 calibration\n\r"); |
|
| 339 | #define MIN_CALIBRATION 450 |
349 | MinCaclibration =LSM303_CALIBRATION_RANGE; |
| - | 350 | } |
|
| - | 351 | ||
| 340 | Calibration.MagX.Range = Xmax - Xmin; |
352 | Calibration.MagX.Range = Xmax - Xmin; |
| 341 | Calibration.MagX.Offset = (Xmin + Xmax) / 2; |
353 | Calibration.MagX.Offset = (Xmin + Xmax) / 2; |
| 342 | Calibration.MagY.Range = Ymax - Ymin; |
354 | Calibration.MagY.Range = Ymax - Ymin; |
| 343 | Calibration.MagY.Offset = (Ymin + Ymax) / 2; |
355 | Calibration.MagY.Offset = (Ymin + Ymax) / 2; |
| 344 | Calibration.MagZ.Range = Zmax - Zmin; |
356 | Calibration.MagZ.Range = Zmax - Zmin; |
| 345 | Calibration.MagZ.Offset = (Zmin + Zmax) / 2; |
357 | Calibration.MagZ.Offset = (Zmin + Zmax) / 2; |
| 346 | if((Calibration.MagX.Range > MIN_CALIBRATION) && (Calibration.MagY.Range > MIN_CALIBRATION) && (Calibration.MagZ.Range > MIN_CALIBRATION)) |
358 | if((Calibration.MagX.Range > MinCaclibration) && (Calibration.MagY.Range > MinCaclibration) && (Calibration.MagZ.Range > MinCaclibration)) |
| 347 | { |
359 | { |
| 348 | NCMAG_IsCalibrated = NCMag_CalibrationWrite(); |
360 | NCMAG_IsCalibrated = NCMag_CalibrationWrite(); |
| 349 | BeepTime = 2500; |
361 | BeepTime = 2500; |
| 350 | UART1_PutString("\r\n Calibration okay\n\r"); |
362 | UART1_PutString("\r\n Calibration okay\n\r"); |
| 351 | } |
363 | } |
| 352 | else |
364 | else |
| 353 | { |
365 | { |
| 354 | UART1_PutString("\r\n Calibration FAILED - Values too low"); |
366 | UART1_PutString("\r\nCalibration FAILED - Values too low: "); |
| 355 | if(Calibration.MagX.Range < MIN_CALIBRATION) UART1_PutString("X! "); |
367 | if(Calibration.MagX.Range < MinCaclibration) UART1_PutString("X! "); |
| 356 | if(Calibration.MagY.Range < MIN_CALIBRATION) UART1_PutString("y! "); |
368 | if(Calibration.MagY.Range < MinCaclibration) UART1_PutString("y! "); |
| 357 | if(Calibration.MagZ.Range < MIN_CALIBRATION) UART1_PutString("Z! "); |
369 | if(Calibration.MagZ.Range < MinCaclibration) UART1_PutString("Z! "); |
| 358 | UART1_PutString("\r\n"); |
370 | UART1_PutString("\r\n"); |
| - | 371 | sprintf(msg, "Minimum is: %i \r\n",MinCaclibration); |
|
| - | 372 | UART1_PutString(msg); |
|
| - | 373 | ||
| 359 | // restore old calibration data from eeprom |
374 | // restore old calibration data from eeprom |
| 360 | NCMAG_IsCalibrated = NCMag_CalibrationRead(); |
375 | NCMAG_IsCalibrated = NCMag_CalibrationRead(); |
| 361 | } |
376 | } |
| 362 | sprintf(msg, "X: (%i - %i = %i)\r\n",Xmax,Xmin,Xmax - Xmin); |
377 | sprintf(msg, "X: (%i - %i = %i)\r\n",Xmax,Xmin,Xmax - Xmin); |
| 363 | UART1_PutString(msg); |
378 | UART1_PutString(msg); |
| Line 628... | Line 643... | ||
| 628 | // u8 retval = 1; |
643 | // u8 retval = 1; |
| Line 629... | Line 644... | ||
| 629 | 644 | ||
| 630 | switch(NCMAG_MagType) |
645 | switch(NCMAG_MagType) |
| 631 | { |
646 | { |
| 632 | case MAG_TYPE_HMC5843: |
647 | case MAG_TYPE_HMC5843: |
| 633 | crb_gain = HMC5843_CRB_GAIN_10GA; |
648 | crb_gain = HMC5843_CRB_GAIN_15GA; |
| 634 | cra_rate = HMC5843_CRA_RATE_50HZ; |
649 | cra_rate = HMC5843_CRA_RATE_50HZ; |
| 635 | xscale = HMC5843_TEST_XSCALE; |
650 | xscale = HMC5843_TEST_XSCALE; |
| 636 | yscale = HMC5843_TEST_YSCALE; |
651 | yscale = HMC5843_TEST_YSCALE; |
| 637 | zscale = HMC5843_TEST_ZSCALE; |
652 | zscale = HMC5843_TEST_ZSCALE; |
| Line 706... | Line 721... | ||
| 706 | u8 i = 0, retval = 1; |
721 | u8 i = 0, retval = 1; |
| Line 707... | Line 722... | ||
| 707 | 722 | ||
| 708 | switch(NCMAG_MagType) |
723 | switch(NCMAG_MagType) |
| 709 | { |
724 | { |
| 710 | case MAG_TYPE_HMC5843: |
725 | case MAG_TYPE_HMC5843: |
| 711 | crb_gain = HMC5843_CRB_GAIN_10GA; |
726 | crb_gain = HMC5843_CRB_GAIN_15GA; |
| 712 | cra_rate = HMC5843_CRA_RATE_50HZ; |
727 | cra_rate = HMC5843_CRA_RATE_50HZ; |
| 713 | xscale = HMC5843_TEST_XSCALE; |
728 | xscale = HMC5843_TEST_XSCALE; |
| 714 | yscale = HMC5843_TEST_YSCALE; |
729 | yscale = HMC5843_TEST_YSCALE; |
| 715 | zscale = HMC5843_TEST_ZSCALE; |
730 | zscale = HMC5843_TEST_ZSCALE; |