Rev 28 | Rev 30 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 28 | Rev 29 | ||
|---|---|---|---|
| Line 66... | Line 66... | ||
| 66 | #include "analog.h" |
66 | #include "analog.h" |
| 67 | #include "uart.h" |
67 | #include "uart.h" |
| Line 68... | Line 68... | ||
| 68 | 68 | ||
| - | 69 | ||
| Line 69... | Line 70... | ||
| 69 | 70 | AttitudeSource_t AttitudeSource = ATTITUDE_SOURCE_ACC; |
|
| Line 70... | Line 71... | ||
| 70 | AttitudeSource_t AttitudeSource = ATTITUDE_SOURCE_ACC; |
71 | Orientation_t Orientation = ORIENTATION_FC; |
| 71 | 72 | ||
| Line 83... | Line 84... | ||
| 83 | struct Scaling_t MagY; |
84 | struct Scaling_t MagY; |
| 84 | struct Scaling_t MagZ; |
85 | struct Scaling_t MagZ; |
| 85 | struct Scaling_t AccX; |
86 | struct Scaling_t AccX; |
| 86 | struct Scaling_t AccY; |
87 | struct Scaling_t AccY; |
| 87 | struct Scaling_t AccZ; |
88 | struct Scaling_t AccZ; |
| 88 | } ; |
89 | }; |
| Line 89... | Line 90... | ||
| 89 | 90 | ||
| 90 | struct Calibration_t eeCalibration EEMEM; // calibration data in EEProm |
91 | struct Calibration_t eeCalibration EEMEM; // calibration data in EEProm |
| Line 91... | Line 92... | ||
| 91 | struct Calibration_t Calibration; // calibration data in RAM |
92 | struct Calibration_t Calibration; // calibration data in RAM |
| 92 | 93 | ||
| 93 | // magnet sensor variable |
94 | // magnet sensor variable |
| 94 | int16_t RawMagnet1a, RawMagnet1b; // raw magnet sensor data |
95 | int16_t RawMagnet1a, RawMagnet1b; // raw magnet sensor data |
| 95 | int16_t RawMagnet2a, RawMagnet2b; |
96 | int16_t RawMagnet2a, RawMagnet2b; |
| 96 | int16_t RawMagnet3a, RawMagnet3b; |
97 | int16_t RawMagnet3a, RawMagnet3b; |
| - | 98 | int16_t UncalMagX, UncalMagY, UncalMagZ; // sensor signal difference without Scaling |
|
| 97 | int16_t UncalMagX, UncalMagY, UncalMagZ; // sensor signal difference without Scaling |
99 | int16_t MagX = 0, MagY = 0, MagZ = 0; // rescaled magnetic field readings |
| 98 | int16_t MagX, MagY, MagZ; // rescaled magnetic field readings |
100 | |
| 99 | // acc sensor variables |
101 | // acceleration sensor variables |
| 100 | int16_t RawAccX, RawAccY, RawAccZ; // raw acceleration readings |
- | |
| 101 | int16_t AccX, AccY, AccZ; // rescaled acceleration readings |
102 | int16_t RawAccX = 0, RawAccY = 0, RawAccZ = 0; // raw acceleration readings |
| Line 102... | Line 103... | ||
| 102 | uint8_t AccPresent = 0; |
103 | int16_t AccX = 0, AccY = 0, AccZ = 0; // rescaled acceleration readings |
| Line 103... | Line 104... | ||
| 103 | uint8_t PC_Connected = 0; |
104 | int16_t AccAttitudeNick = 0, AccAttitudeRoll = 0; // nick and roll angle from acc |
| 104 | 105 | ||
| 105 | int16_t Heading = -1; |
106 | int16_t Heading = -1; // the current compass heading in deg |
| Line 115... | Line 116... | ||
| 115 | else MagX = 0; |
116 | else MagX = 0; |
| 116 | if(Calibration.MagY.Range != 0) MagY = (1024L * (int32_t)(UncalMagY - Calibration.MagY.Offset)) / (Calibration.MagY.Range); |
117 | if(Calibration.MagY.Range != 0) MagY = (1024L * (int32_t)(UncalMagY - Calibration.MagY.Offset)) / (Calibration.MagY.Range); |
| 117 | else MagY = 0; |
118 | else MagY = 0; |
| 118 | if(Calibration.MagY.Range != 0) MagZ = (1024L * (int32_t)(UncalMagZ - Calibration.MagZ.Offset)) / (Calibration.MagZ.Range); |
119 | if(Calibration.MagY.Range != 0) MagZ = (1024L * (int32_t)(UncalMagZ - Calibration.MagZ.Offset)) / (Calibration.MagZ.Range); |
| 119 | else MagZ = 0; |
120 | else MagZ = 0; |
| - | 121 | ||
| - | 122 | if(AccPresent) |
|
| - | 123 | { |
|
| - | 124 | AccX = (RawAccX - Calibration.AccX.Offset); |
|
| - | 125 | AccY = (RawAccY - Calibration.AccY.Offset); |
|
| - | 126 | AccZ = (Calibration.AccZ.Offset - RawAccZ); |
|
| - | 127 | ||
| - | 128 | #if (BOARD == 10) // the hardware 1.0 has the LIS3L02AL |
|
| - | 129 | // acc mode assumes orientation like FC |
|
| - | 130 | if(AccX > 136) AccAttitudeNick = -800; |
|
| - | 131 | else |
|
| - | 132 | if(AccX < -136) AccAttitudeNick = 800; |
|
| - | 133 | else AccAttitudeNick = (int16_t)(-1800.0 * asin((double) AccX / 138.0) / M_PI); |
|
| - | 134 | ||
| - | 135 | ||
| - | 136 | if(AccY > 136) AccAttitudeRoll = 800; |
|
| - | 137 | else |
|
| - | 138 | if(AccY < -136) AccAttitudeRoll = -800; |
|
| - | 139 | else AccAttitudeRoll = (int16_t)( 1800.0 * asin((double) AccY / 138.0) / M_PI); |
|
| - | 140 | ||
| - | 141 | #else // the hardware 1.1 has the LIS344ALH with a different axis definition (X -> -Y, Y -> X, Z -> Z) |
|
| - | 142 | // acc mode assumes orientation like FC |
|
| - | 143 | if(AccY > 136) AccAttitudeNick = 800; |
|
| - | 144 | else |
|
| - | 145 | if(AccY < -136) AccAttitudeNick = -800; |
|
| - | 146 | else AccAttitudeNick = (int16_t)( 1800.0 * asin((double) AccY / 138.0) / M_PI); |
|
| - | 147 | ||
| - | 148 | ||
| - | 149 | if(AccX > 136) AccAttitudeRoll = 800; |
|
| - | 150 | else |
|
| - | 151 | if(AccX < -136) AccAttitudeRoll = -800; |
|
| - | 152 | else AccAttitudeRoll = (int16_t)( 1800.0 * asin((double) AccX / 138.0) / M_PI); |
|
| - | 153 | #endif |
|
| - | 154 | } |
|
| 120 | } |
155 | } |
| Line 121... | Line 156... | ||
| 121 | 156 | ||
| 122 | 157 | ||
| 123 | void CalcHeading(void) |
158 | void CalcHeading(void) |
| 124 | { |
159 | { |
| Line 125... | Line 160... | ||
| 125 | double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz; |
160 | double nick_rad, roll_rad, Hx, Hy, Cx = 0.0, Cy = 0.0, Cz = 0.0; |
| 126 | int16_t heading = -1; |
161 | int16_t heading = -1; |
| 127 | 162 | ||
| 128 | // blink code for normal operation |
163 | // blink code for normal operation |
| 129 | if(CheckDelay(Led_Timer)) |
164 | if(CheckDelay(Led_Timer)) |
| 130 | { |
165 | { |
| Line -... | Line 166... | ||
| - | 166 | LED_GRN_TOGGLE; |
|
| - | 167 | Led_Timer = SetDelay(500); |
|
| - | 168 | } |
|
| 131 | LED_GRN_TOGGLE; |
169 | |
| 132 | Led_Timer = SetDelay(500); |
170 | switch(Orientation) |
| 133 | } |
171 | { |
| - | 172 | case ORIENTATION_NC: |
|
| 134 | 173 | Cx = MagX; |
|
| - | 174 | Cy = MagY; |
|
| 135 | Cx = MagX; |
175 | Cz = MagZ; |
| 136 | Cy = MagY; |
- | |
| 137 | Cz = MagZ; |
176 | break; |
| 138 | 177 | ||
| 139 | if(ExternData.Orientation == 1) |
178 | case ORIENTATION_FC: |
| - | 179 | // rotation of 90 deg compared to NC setup |
|
| 140 | { |
180 | Cx = MagY; |
| Line 141... | Line 181... | ||
| 141 | Cx = MagX; |
181 | Cy = -MagX; |
| 142 | Cy = -MagY; |
182 | Cz = MagZ; |
| 143 | Cz = MagZ; |
183 | break; |
| Line 155... | Line 195... | ||
| 155 | nick_rad = ((double)ExternData.Attitude[NICK]) * M_PI / (double)(1800.0); |
195 | nick_rad = ((double)ExternData.Attitude[NICK]) * M_PI / (double)(1800.0); |
| 156 | roll_rad = ((double)ExternData.Attitude[ROLL]) * M_PI / (double)(1800.0); |
196 | roll_rad = ((double)ExternData.Attitude[ROLL]) * M_PI / (double)(1800.0); |
| 157 | break; |
197 | break; |
| Line 158... | Line 198... | ||
| 158 | 198 | ||
| 159 | case ATTITUDE_SOURCE_ACC: |
- | |
| 160 | if(AccX > 125) nick_rad = M_PI / 2; |
- | |
| 161 | else |
- | |
| 162 | if(AccX < -125) nick_rad = -M_PI / 2; |
- | |
| 163 | else |
- | |
| 164 | { |
199 | case ATTITUDE_SOURCE_ACC: |
| 165 | nick_rad = asin((double) AccX / 125.0); |
- | |
| 166 | } |
- | |
| 167 | - | ||
| 168 | if(AccY > 125) roll_rad = M_PI / 2; |
- | |
| 169 | else |
- | |
| 170 | if(AccY < -125) roll_rad = -M_PI / 2; |
- | |
| 171 | else |
- | |
| 172 | { |
200 | nick_rad = ((double)AccAttitudeNick) * M_PI / (double)(1800.0); |
| 173 | roll_rad = asin((double) AccY / 125.0); |
- | |
| 174 | } |
201 | roll_rad = ((double)AccAttitudeRoll) * M_PI / (double)(1800.0); |
| - | 202 | break; |
|
| - | 203 | ||
| - | 204 | default: |
|
| - | 205 | nick_rad = 0; |
|
| - | 206 | roll_rad = 0; |
|
| 175 | break; |
207 | break; |
| Line 176... | Line 208... | ||
| 176 | } |
208 | } |
| 177 | 209 | ||
| 178 | // calculate attitude correction |
210 | // calculate attitude correction |
| Line -... | Line 211... | ||
| - | 211 | Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad); |
|
| - | 212 | Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad); |
|
| - | 213 | ||
| - | 214 | DebugOut.Analog[27] = (int16_t)Hx; |
|
| 179 | Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad); |
215 | DebugOut.Analog[28] = (int16_t)Hy; |
| 180 | Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad); |
216 | |
| 181 | 217 | ||
| 182 | // calculate Heading |
218 | // calculate Heading |
| 183 | heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI); |
219 | heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI); |
| Line 267... | Line 303... | ||
| 267 | 303 | ||
| 268 | case 5: |
304 | case 5: |
| 269 | // Save values |
305 | // Save values |
| 270 | if(cal != calold) // avoid continously writing of eeprom! |
306 | if(cal != calold) // avoid continously writing of eeprom! |
| 271 | { |
307 | { |
| 272 | Calibration.MagY.Range = Xmax - Xmin; |
308 | Calibration.MagX.Range = Xmax - Xmin; |
| 273 | Calibration.MagX.Offset = (Xmin + Xmax) / 2; |
309 | Calibration.MagX.Offset = (Xmin + Xmax) / 2; |
| 274 | Calibration.MagY.Range = Ymax - Ymin; |
310 | Calibration.MagY.Range = Ymax - Ymin; |
| 275 | Calibration.MagY.Offset = (Ymin + Ymax) / 2; |
311 | Calibration.MagY.Offset = (Ymin + Ymax) / 2; |
| 276 | Calibration.MagZ.Range = Zmax - Zmin; |
312 | Calibration.MagZ.Range = Zmax - Zmin; |
| 277 | Calibration.MagZ.Offset = (Zmin + Zmax) / 2; |
313 | Calibration.MagZ.Offset = (Zmin + Zmax) / 2; |
| 278 | if((Calibration.MagX.Range > 150) && (Calibration.MagY.Range > 150) && (Calibration.MagZ.Range > 150)) |
314 | if((Calibration.MagX.Range > 150) && (Calibration.MagY.Range > 150) && (Calibration.MagZ.Range > 150)) |
| 279 | { |
315 | { |
| 280 | // indicate write process by setting the led |
316 | // indicate write process by setting the led |
| 281 | LED_GRN_ON; |
317 | LED_GRN_ON; |
| 282 | eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration)); |
318 | eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration)); |
| 283 | Delay_ms(2000); |
- | |
| 284 | // reset led state |
- | |
| 285 | LED_GRN_OFF; |
319 | Led_Timer = SetDelay(2000); |
| 286 | // reset blinkcode |
320 | // reset blinkcode |
| 287 | blinkcount = 0; |
- | |
| 288 | Led_Timer = SetDelay(1000); |
321 | blinkcount = 0; |
| 289 | } |
322 | } |
| 290 | } |
323 | } |
| Line 291... | Line 324... | ||
| 291 | break; |
324 | break; |
| Line 306... | Line 339... | ||
| 306 | DebugOut.Analog[4] = UncalMagY; |
339 | DebugOut.Analog[4] = UncalMagY; |
| 307 | DebugOut.Analog[5] = UncalMagZ; |
340 | DebugOut.Analog[5] = UncalMagZ; |
| 308 | switch(AttitudeSource) |
341 | switch(AttitudeSource) |
| 309 | { |
342 | { |
| 310 | case ATTITUDE_SOURCE_ACC: |
343 | case ATTITUDE_SOURCE_ACC: |
| 311 | - | ||
| - | 344 | DebugOut.Analog[6] = AccAttitudeNick; |
|
| - | 345 | DebugOut.Analog[7] = AccAttitudeRoll; |
|
| 312 | break; |
346 | break; |
| Line 313... | Line 347... | ||
| 313 | 347 | ||
| 314 | case ATTITUDE_SOURCE_UART: |
348 | case ATTITUDE_SOURCE_UART: |
| 315 | DebugOut.Analog[6] = ExternData.Attitude[NICK]; |
349 | DebugOut.Analog[6] = ExternData.Attitude[NICK]; |
| Line 339... | Line 373... | ||
| 339 | DebugOut.Analog[22] = RawAccY; |
373 | DebugOut.Analog[22] = RawAccY; |
| 340 | DebugOut.Analog[23] = RawAccZ; |
374 | DebugOut.Analog[23] = RawAccZ; |
| 341 | DebugOut.Analog[24] = Calibration.AccX.Offset; |
375 | DebugOut.Analog[24] = Calibration.AccX.Offset; |
| 342 | DebugOut.Analog[25] = Calibration.AccY.Offset; |
376 | DebugOut.Analog[25] = Calibration.AccY.Offset; |
| 343 | DebugOut.Analog[26] = Calibration.AccZ.Offset; |
377 | DebugOut.Analog[26] = Calibration.AccZ.Offset; |
| - | 378 | DebugOut.Analog[29] = AttitudeSource; |
|
| 344 | } |
379 | } |
| Line 345... | Line 380... | ||
| 345 | 380 | ||
| 346 | void AccMeasurement(void) |
381 | void AccMeasurement(void) |
| 347 | { |
382 | { |
| 348 | if(AccPresent) |
383 | if(AccPresent) |
| 349 | { |
384 | { |
| 350 | RawAccX = (RawAccX + (int16_t)ADC_GetValue(ADC2))/2; |
385 | RawAccX = (RawAccX + (int16_t)ADC_GetValue(ACC_X))/2; |
| 351 | RawAccY = (RawAccY + (int16_t)ADC_GetValue(ADC3))/2; |
386 | RawAccY = (RawAccY + (int16_t)ADC_GetValue(ACC_Y))/2; |
| 352 | RawAccZ = (RawAccZ + (int16_t)ADC_GetValue(ADC6))/2; |
387 | RawAccZ = (RawAccZ + (int16_t)ADC_GetValue(ACC_Z))/2; |
| 353 | } |
388 | } |
| 354 | else |
389 | else |
| 355 | { |
390 | { |
| 356 | RawAccX = 0; |
391 | RawAccX = 0; |
| 357 | RawAccY = 0; |
392 | RawAccY = 0; |
| 358 | RawAccZ = 0; |
393 | RawAccZ = 0; |
| 359 | } |
- | |
| 360 | AccX = ((RawAccX - Calibration.AccX.Offset) + AccX * 7) / 8; |
- | |
| 361 | AccY = ((RawAccY - Calibration.AccY.Offset) + AccY * 7) / 8; |
- | |
| 362 | AccZ = ((Calibration.AccZ.Offset - RawAccZ) + AccZ * 7) / 8; |
394 | } |
| Line 363... | Line -... | ||
| 363 | } |
- | |
| 364 | - | ||
| 365 | 395 | } |
|
| 366 | 396 | ||
| 367 | int main (void) |
397 | int main (void) |
| 368 | { |
398 | { |
| 369 | // reset input pullup |
399 | // reset input pullup |
| Line 376... | Line 406... | ||
| 376 | ADC_Init(); |
406 | ADC_Init(); |
| 377 | I2C_Init(); |
407 | I2C_Init(); |
| Line 378... | Line 408... | ||
| 378 | 408 | ||
| Line -... | Line 409... | ||
| - | 409 | sei(); // enable globale interrupts |
|
| - | 410 | ||
| - | 411 | if(AccPresent) |
|
| - | 412 | { |
|
| - | 413 | USART0_Print("ACC present\n"); |
|
| 379 | sei(); // enable globale interrupts |
414 | } |
| Line 380... | Line 415... | ||
| 380 | 415 | ||
| 381 | LED_GRN_ON; |
416 | LED_GRN_ON; |
| Line 382... | Line 417... | ||
| 382 | 417 | ||
| 383 | Debug_Timer = SetDelay(200); |
418 | Debug_Timer = SetDelay(200); |
| Line 384... | Line -... | ||
| 384 | Led_Timer = SetDelay(200); |
- | |
| 385 | 419 | Led_Timer = SetDelay(200); |
|
| 386 | // read calibration info from eeprom |
420 | |
| Line 387... | Line 421... | ||
| 387 | eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration)); |
421 | // read calibration info from eeprom |
| 388 | 422 | eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration)); |
|
| 389 | ExternData.Orientation = 0; |
423 | |
| 390 | ExternData.CalState = 0; |
424 | ExternData.CalState = 0; |
| 391 | I2C_WriteCal.CalByte = 0; |
425 | I2C_WriteCal.CalByte = 0; |
| 392 | 426 | ||
| 393 | 427 | ||
| 394 | // main loop |
428 | // main loop |
| 395 | while (1) |
429 | while (1) |
| 396 | { |
430 | { |
| Line 397... | Line 431... | ||
| 397 | FLIP_LOW; |
431 | FLIP_LOW; |
| 398 | Delay_ms(2); |
432 | Delay_ms(2); |
| 399 | RawMagnet1a = ADC_GetValue(ADC0); |
433 | RawMagnet1a = ADC_GetValue(MAG_X); |
| 400 | RawMagnet2a = -ADC_GetValue(ADC1); |
434 | RawMagnet2a = -ADC_GetValue(MAG_Y); |
| 401 | RawMagnet3a = ADC_GetValue(ADC7); |
435 | RawMagnet3a = ADC_GetValue(MAG_Z); |
| 402 | AccMeasurement(); |
436 | AccMeasurement(); |
| 403 | Delay_ms(1); |
437 | Delay_ms(1); |
| Line 404... | Line 438... | ||
| 404 | 438 | ||
| Line 405... | Line 439... | ||
| 405 | FLIP_HIGH; |
439 | FLIP_HIGH; |
| 406 | Delay_ms(2); |
440 | Delay_ms(2); |
| Line 407... | Line 441... | ||
| 407 | RawMagnet1b = ADC_GetValue(ADC0); |
441 | RawMagnet1b = ADC_GetValue(MAG_X); |
| 408 | RawMagnet2b = -ADC_GetValue(ADC1); |
442 | RawMagnet2b = -ADC_GetValue(MAG_Y); |
| - | 443 | RawMagnet3b = ADC_GetValue(MAG_Z); |
|
| - | 444 | AccMeasurement(); |
|
| - | 445 | Delay_ms(1); |
|
| - | 446 | ||
| - | 447 | CalcFields(); |
|
| - | 448 | ||
| - | 449 | if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate(); |
|
| 409 | RawMagnet3b = ADC_GetValue(ADC7); |
450 | else CalcHeading(); |
| - | 451 | ||
| Line 410... | Line 452... | ||
| 410 | AccMeasurement(); |
452 | // check data from USART |
| 411 | Delay_ms(1); |
453 | USART0_ProcessRxData(); |
| 412 | 454 | ||
| 413 | CalcFields(); |
455 | if(NC_Connected) NC_Connected--; |