Rev 21 | Rev 23 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 21 | Rev 22 | ||
|---|---|---|---|
| Line 89... | Line 89... | ||
| 89 | Calibration_t eeCalibration EEMEM; // calibration data in EEProm |
89 | Calibration_t eeCalibration EEMEM; // calibration data in EEProm |
| 90 | Calibration_t Calibration; // calibration data in RAM |
90 | Calibration_t Calibration; // calibration data in RAM |
| 91 | 91 | ||
| 92 | 92 | ||
| 93 | int16_t UncalMagnetX, UncalMagnetY, UncalMagnetZ; // sensor signal difference without Scaling |
93 | int16_t UncalMagnetX, UncalMagnetY, UncalMagnetZ; // sensor signal difference without Scaling |
| 94 | int16_t MagnetX, MagnetY, MagnetZ; // rescaled magnetic field readings |
94 | int16_t MagnetX, MagnetY, MagnetZ; // rescaled magnetic field readings |
| 95 | 95 | ||
| 96 | uint8_t PC_Connected = 0; |
96 | uint8_t PC_Connected = 0; |
| 97 | 97 | ||
| 98 | int16_t Heading = -1; |
98 | int16_t Heading = -1; |
| 99 | 99 | ||
| Line 102... | Line 102... | ||
| 102 | { |
102 | { |
| 103 | UncalMagnetX = (1 * UncalMagnetX + (RawMagnet1a - RawMagnet1b)) / 2; |
103 | UncalMagnetX = (1 * UncalMagnetX + (RawMagnet1a - RawMagnet1b)) / 2; |
| 104 | UncalMagnetY = (1 * UncalMagnetY + (RawMagnet3a - RawMagnet3b)) / 2; |
104 | UncalMagnetY = (1 * UncalMagnetY + (RawMagnet3a - RawMagnet3b)) / 2; |
| 105 | UncalMagnetZ = (1 * UncalMagnetZ + (RawMagnet2a - RawMagnet2b)) / 2; |
105 | UncalMagnetZ = (1 * UncalMagnetZ + (RawMagnet2a - RawMagnet2b)) / 2; |
| 106 | 106 | ||
| 107 | MagnetX = (1024L * (int32_t)(UncalMagnetX - Calibration.X.Offset)) / (Calibration.X.Range); |
107 | if(Calibration.X.Range != 0) MagnetX = (1024L * (int32_t)(UncalMagnetX - Calibration.X.Offset)) / (Calibration.X.Range); |
| - | 108 | else MagnetX = 0; |
|
| 108 | MagnetY = (1024L * (int32_t)(UncalMagnetY - Calibration.Y.Offset)) / (Calibration.Y.Range); |
109 | if(Calibration.Y.Range != 0) MagnetY = (1024L * (int32_t)(UncalMagnetY - Calibration.Y.Offset)) / (Calibration.Y.Range); |
| - | 110 | else MagnetY = 0; |
|
| 109 | MagnetZ = (1024L * (int32_t)(UncalMagnetZ - Calibration.Z.Offset)) / (Calibration.Z.Range); |
111 | if(Calibration.Y.Range != 0) MagnetZ = (1024L * (int32_t)(UncalMagnetZ - Calibration.Z.Offset)) / (Calibration.Z.Range); |
| - | 112 | else MagnetZ = 0; |
|
| 110 | } |
113 | } |
| 111 | 114 | ||
| 112 | 115 | ||
| 113 | void CalcHeading(void) |
116 | void CalcHeading(void) |
| 114 | { |
117 | { |
| Line 157... | Line 160... | ||
| 157 | { |
160 | { |
| 158 | uint8_t cal; |
161 | uint8_t cal; |
| 159 | static uint8_t calold = 0; |
162 | static uint8_t calold = 0; |
| 160 | static int16_t Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0; |
163 | static int16_t Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0; |
| 161 | static uint8_t blinkcount = 0; |
164 | static uint8_t blinkcount = 0; |
| 162 | /* |
165 | |
| 163 | // check both sources of communication for calibration request |
166 | // check both sources of communication for calibration request |
| 164 | if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte; |
167 | if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte; |
| 165 | else cal = ExternData.CalState;*/ |
168 | else cal = ExternData.CalState; |
| 166 | - | ||
| 167 | cal = ExternData.CalState; |
- | |
| 168 | 169 | ||
| - | 170 | DebugOut.Analog[30] = cal; |
|
| 169 | // blink code for current calibration state |
171 | // blink code for current calibration state |
| 170 | if(cal) |
172 | if(cal) |
| 171 | { |
173 | { |
| 172 | if(CheckDelay(Led_Timer) || (cal != calold)) |
174 | if(CheckDelay(Led_Timer) || (cal != calold)) |
| 173 | { |
175 | { |
| Line 278... | Line 280... | ||
| 278 | DebugOut.Analog[13] = Calibration.Z.Range; |
280 | DebugOut.Analog[13] = Calibration.Z.Range; |
| 279 | DebugOut.Analog[14] = ExternData.CalState; |
281 | DebugOut.Analog[14] = ExternData.CalState; |
| 280 | DebugOut.Analog[15] = Heading; |
282 | DebugOut.Analog[15] = Heading; |
| 281 | DebugOut.Analog[16] = ExternData.UserParam[0]; |
283 | DebugOut.Analog[16] = ExternData.UserParam[0]; |
| 282 | DebugOut.Analog[17] = ExternData.UserParam[1]; |
284 | DebugOut.Analog[17] = ExternData.UserParam[1]; |
| - | 285 | DebugOut.Analog[30] = I2C_WriteCal.CalByte; |
|
| 283 | DebugOut.Analog[31] = PC_Connected; |
286 | DebugOut.Analog[31] = PC_Connected; |
| 284 | } |
287 | } |
| 285 | 288 | ||
| 286 | 289 | ||
| 287 | int main (void) |
290 | int main (void) |
| 288 | { |
291 | { |
| - | 292 | // reset input pullup |
|
| - | 293 | DDRC &=~((1<<DDC6)); |
|
| 289 | Led_Init(); |
294 | PORTC |= (1<<PORTC6); |
| - | 295 | ||
| 290 | LED_GRN_ON; |
296 | LED_Init(); |
| 291 | TIMER0_Init(); |
297 | TIMER0_Init(); |
| 292 | USART0_Init(); |
298 | USART0_Init(); |
| 293 | ADC_Init(); |
299 | ADC_Init(); |
| 294 | I2C_Init(); |
300 | I2C_Init(); |
| 295 | 301 | ||
| - | 302 | sei(); // enable globale interrupts |
|
| 296 | 303 | ||
| 297 | sei(); //Globale Interrupts Einschalten |
304 | LED_GRN_ON; |
| 298 | - | ||
| 299 | - | ||
| 300 | 305 | ||
| 301 | Debug_Timer = SetDelay(100); // Sendeintervall |
306 | Debug_Timer = SetDelay(200); |
| 302 | Led_Timer = SetDelay(100); |
307 | Led_Timer = SetDelay(200); |
| 303 | 308 | ||
| 304 | // read calibration info from eeprom |
309 | // read calibration info from eeprom |
| 305 | eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration_t)); |
310 | eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration_t)); |
| 306 | 311 | ||
| 307 | ExternData.Orientation = 0; |
312 | ExternData.Orientation = 0; |
| 308 | ExternData.CalState = 0; |
313 | ExternData.CalState = 0; |
| 309 | I2C_WriteCal.CalByte = 0; |
314 | I2C_WriteCal.CalByte = 0; |
| 310 | 315 | ||
| - | 316 | Heading = 101; |
|
| - | 317 | ||
| 311 | // main loop |
318 | // main loop |
| 312 | while (1) |
319 | while (1) |
| 313 | { |
320 | { |
| - | 321 | ||
| - | 322 | ||
| 314 | FLIP_LOW; |
323 | FLIP_LOW; |
| 315 | Delay_ms(2); |
324 | Delay_ms(2); |
| 316 | RawMagnet1a = ADC_GetValue(ADC0); |
325 | RawMagnet1a = ADC_GetValue(ADC0); |
| 317 | RawMagnet2a = -ADC_GetValue(ADC1); |
326 | RawMagnet2a = -ADC_GetValue(ADC1); |
| 318 | RawMagnet3a = ADC_GetValue(ADC7); |
327 | RawMagnet3a = ADC_GetValue(ADC7); |
| Line 326... | Line 335... | ||
| 326 | RawMagnet3b = ADC_GetValue(ADC7); |
335 | RawMagnet3b = ADC_GetValue(ADC7); |
| 327 | Delay_ms(1); |
336 | Delay_ms(1); |
| 328 | 337 | ||
| 329 | CalcFields(); |
338 | CalcFields(); |
| 330 | 339 | ||
| 331 | //if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate(); |
340 | if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate(); |
| 332 | if(ExternData.CalState) Calibrate(); |
- | |
| 333 | else CalcHeading(); |
341 | else CalcHeading(); |
| 334 | 342 | ||
| 335 | // check data from USART |
343 | // check data from USART |
| 336 | USART0_ProcessRxData(); |
344 | USART0_ProcessRxData(); |
| 337 | 345 | ||