Rev 338 | Rev 342 | 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; |