Rev 329 | Rev 331 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 329 | Rev 330 | ||
---|---|---|---|
Line 94... | Line 94... | ||
94 | #define SPI_COMMAND_INDEX 0 |
94 | #define SPI_COMMAND_INDEX 0 |
Line 95... | Line 95... | ||
95 | 95 | ||
96 | s32 Kalman_K = 32; |
96 | s32 Kalman_K = 32; |
97 | s32 Kalman_MaxDrift = 5 * 16; |
97 | s32 Kalman_MaxDrift = 5 * 16; |
- | 98 | s32 Kalman_MaxFusion = 64; |
|
98 | s32 Kalman_MaxFusion = 64; |
99 | s32 Kalman_Kompass = 32; |
Line 99... | Line 100... | ||
99 | s32 ToFcGpsZ = 0; |
100 | s32 ToFcGpsZ = 0; |
100 | 101 | ||
101 | u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_HOTT_INFO, SPI_NCCMD_KALMAN}; |
102 | u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_HOTT_INFO, SPI_NCCMD_KALMAN}; |
Line 107... | Line 108... | ||
107 | s32 ToFC_AltitudeSetpoint = 0; |
108 | s32 ToFC_AltitudeSetpoint = 0; |
108 | u8 FromFC_VarioCharacter = ' '; |
109 | u8 FromFC_VarioCharacter = ' '; |
109 | u8 DisableFC_Sticks = 0; |
110 | u8 DisableFC_Sticks = 0; |
110 | u8 NC_GPS_ModeCharacter = ' '; |
111 | u8 NC_GPS_ModeCharacter = ' '; |
111 | u8 FCCalibActive = 0; |
112 | u8 FCCalibActive = 0; |
- | 113 | u8 FC_is_Calibrated = 0; |
|
Line 112... | Line 114... | ||
112 | 114 | ||
Line 113... | Line 115... | ||
113 | SPI_Version_t FC_Version; |
115 | SPI_Version_t FC_Version; |
114 | 116 | ||
Line 301... | Line 303... | ||
301 | case SPI_NCCMD_KALMAN: // wird am häufigsten betätigt |
303 | case SPI_NCCMD_KALMAN: // wird am häufigsten betätigt |
302 | CalcHeadFree(); |
304 | CalcHeadFree(); |
303 | ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K; |
305 | ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K; |
304 | ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion; |
306 | ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion; |
305 | ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift; |
307 | ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift; |
306 | ToFlightCtrl.Param.Byte[3] = (u8) SerialLinkOkay; |
308 | ToFlightCtrl.Param.Byte[3] = (u8) Kalman_Kompass; |
307 | ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ; |
309 | ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ; |
308 | ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C; |
310 | ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C; |
309 | ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S; |
311 | ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S; |
310 | if(DisableFC_Sticks) ToFlightCtrl.Param.Byte[7] = FLAG_GPS_AID; else ToFlightCtrl.Param.Byte[7] = 0x00; |
312 | if(DisableFC_Sticks) ToFlightCtrl.Param.Byte[7] = FLAG_GPS_AID; else ToFlightCtrl.Param.Byte[7] = 0x00; |
311 | if(CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH) |
313 | if(CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH) |
Line 337... | Line 339... | ||
337 | ToFlightCtrl.Param.Byte[4] = Version_HW; |
339 | ToFlightCtrl.Param.Byte[4] = Version_HW; |
338 | ToFlightCtrl.Param.Byte[5] = DebugOut.Status[0]; |
340 | ToFlightCtrl.Param.Byte[5] = DebugOut.Status[0]; |
339 | ToFlightCtrl.Param.Byte[6] = DebugOut.Status[1]; |
341 | ToFlightCtrl.Param.Byte[6] = DebugOut.Status[1]; |
340 | ToFlightCtrl.Param.Byte[7] = ErrorCode; |
342 | ToFlightCtrl.Param.Byte[7] = ErrorCode; |
341 | ToFlightCtrl.Param.Byte[8] = NC_GPS_ModeCharacter; |
343 | ToFlightCtrl.Param.Byte[8] = NC_GPS_ModeCharacter; |
- | 344 | ToFlightCtrl.Param.Byte[9] = SerialLinkOkay; |
|
342 | break; |
345 | break; |
Line 343... | Line 346... | ||
343 | 346 | ||
344 | case SPI_NCCMD_GPSINFO: |
347 | case SPI_NCCMD_GPSINFO: |
345 | ToFlightCtrl.Param.Byte[0] = GPSData.Flags; |
348 | ToFlightCtrl.Param.Byte[0] = GPSData.Flags; |
Line 364... | Line 367... | ||
364 | } |
367 | } |
365 | else |
368 | else |
366 | { |
369 | { |
367 | ToFlightCtrl.Param.sInt[5] = (s16)ToFC_AltitudeSetpoint; |
370 | ToFlightCtrl.Param.sInt[5] = (s16)ToFC_AltitudeSetpoint; |
368 | } |
371 | } |
369 | // DebugOut.Analog[25] = (s16)ToFlightCtrl.Param.Byte[9]; |
- | |
370 | // DebugOut.Analog[20] = ToFlightCtrl.Param.sInt[5]; |
- | |
371 | break; |
372 | break; |
372 | case SPI_NCCMD_HOTT_INFO: |
- | |
373 | switch(hott_index++) |
- | |
374 | { |
- | |
375 | /* |
373 | /* |
376 | typedef struct |
374 | typedef struct |
377 | { |
375 | { |
378 | unsigned char StartByte; //0 // 0x7C |
376 | unsigned char StartByte; //0 // 0x7C |
379 | unsigned char Packet_ID; //1 // 0x89 - Vario ID |
377 | unsigned char Packet_ID; //1 // 0x89 - Vario ID |
Line 398... | Line 396... | ||
398 | unsigned char NullByte; // 0x00 |
396 | unsigned char NullByte; // 0x00 |
399 | unsigned char NullByte1; // 0x00 |
397 | unsigned char NullByte1; // 0x00 |
400 | unsigned char EndByte; // 0x7D |
398 | unsigned char EndByte; // 0x7D |
401 | } GPSPacket_t; |
399 | } GPSPacket_t; |
402 | */ |
400 | */ |
- | 401 | /* |
|
- | 402 | typedef struct |
|
- | 403 | { |
|
- | 404 | unsigned char StartByte; //0 0x7C |
|
- | 405 | unsigned char Packet_ID; //1 HOTT_GENERAL_PACKET_ID |
|
- | 406 | unsigned char WarnBeep; //2 Anzahl der Töne 0..36 |
|
- | 407 | unsigned char VoltageCell1; //3 208 = 4,16V (Voltage * 50 = Wert) |
|
- | 408 | unsigned char VoltageCell2; //4 |
|
- | 409 | unsigned char VoltageCell3; //5 |
|
- | 410 | unsigned char VoltageCell4; //6 |
|
- | 411 | unsigned char VoltageCell5; //7 |
|
- | 412 | unsigned char VoltageCell6; //8 |
|
- | 413 | unsigned int Battery1; //9 51 = 5,1V |
|
- | 414 | unsigned int Battery2; //11 51 = 5,1V |
|
- | 415 | unsigned char Temperature1; //13 44 = 24°C, 0 = -20°C |
|
- | 416 | unsigned char Temperature2; //14 44 = 24°C, 0 = -20°C |
|
- | 417 | unsigned char FuelPercent; //15 |
|
- | 418 | signed int FuelCapacity; //16 |
|
- | 419 | unsigned int Rpm; |
|
- | 420 | unsigned int Altitude; |
|
- | 421 | unsigned int m_sec; // 3000 = 0 |
|
- | 422 | unsigned char m_3sec; // 120 = 0 |
|
- | 423 | unsigned int Current; // 1 = 0.1A |
|
- | 424 | unsigned int InputVoltage; // 66 = 6,6V |
|
- | 425 | unsigned int Capacity; // 1 = 10mAh |
|
- | 426 | unsigned char NullByte1; // 0x00 |
|
- | 427 | unsigned char NullByte2; // 0x00 |
|
- | 428 | unsigned char EndByte; // 0x7D |
|
- | 429 | } HoTTGeneral_t; |
|
- | 430 | */ |
|
- | 431 | case SPI_NCCMD_HOTT_INFO: |
|
- | 432 | switch(hott_index++) |
|
- | 433 | { |
|
403 | case 0: |
434 | case 0: |
404 | //Dezimalgrad --> Grad mit Dezimalminuten --> Grad, Minuten, Sekunden |
435 | //Dezimalgrad --> Grad mit Dezimalminuten --> Grad, Minuten, Sekunden |
405 | //53.285788 7.4847269 --> N53° 17.14728 E7° 29.08362 --> N53° 17' 8.837" E7° 29' 5.017" |
436 | //53.285788 7.4847269 --> N53° 17.14728 E7° 29.08362 --> N53° 17' 8.837" E7° 29' 5.017" |
406 | ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID; |
437 | ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID; |
407 | ToFlightCtrl.Param.Byte[0] = 3; // index |
438 | ToFlightCtrl.Param.Byte[0] = 3; // index |
Line 424... | Line 455... | ||
424 | i2 /= 10; |
455 | i2 /= 10; |
425 | ToFlightCtrl.Param.Byte[6] = i1 % 256; |
456 | ToFlightCtrl.Param.Byte[6] = i1 % 256; |
426 | ToFlightCtrl.Param.Byte[7] = i1 / 256; |
457 | ToFlightCtrl.Param.Byte[7] = i1 / 256; |
427 | ToFlightCtrl.Param.Byte[8] = i2 % 256; |
458 | ToFlightCtrl.Param.Byte[8] = i2 % 256; |
428 | ToFlightCtrl.Param.Byte[9] = i2 / 256; |
459 | ToFlightCtrl.Param.Byte[9] = i2 / 256; |
429 | - | ||
430 | break; |
460 | break; |
431 | case 1: |
461 | case 1: |
432 | ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID; |
462 | ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID; |
433 | ToFlightCtrl.Param.Byte[0] = 11; // index |
463 | ToFlightCtrl.Param.Byte[0] = 11; // index |
434 | ToFlightCtrl.Param.Byte[1] = 8-1; // how many |
464 | ToFlightCtrl.Param.Byte[1] = 8-1; // how many |
Line 447... | Line 477... | ||
447 | ToFlightCtrl.Param.Byte[6] = i2 / 256; |
477 | ToFlightCtrl.Param.Byte[6] = i2 / 256; |
448 | //----------------------------- |
478 | //----------------------------- |
449 | i1 = NaviData.HomePositionDeviation.Distance / 10; // dann in m |
479 | i1 = NaviData.HomePositionDeviation.Distance / 10; // dann in m |
450 | ToFlightCtrl.Param.Byte[7] = i1 % 256; |
480 | ToFlightCtrl.Param.Byte[7] = i1 % 256; |
451 | ToFlightCtrl.Param.Byte[8] = i1 / 256; |
481 | ToFlightCtrl.Param.Byte[8] = i1 / 256; |
- | 482 | break; |
|
- | 483 | case 2: |
|
- | 484 | ToFlightCtrl.Param.Byte[11] = HOTT_GENERAL_PACKET_ID; |
|
- | 485 | ToFlightCtrl.Param.Byte[0] = 5; // index |
|
- | 486 | ToFlightCtrl.Param.Byte[1] = 2; // how many |
|
- | 487 | ToFlightCtrl.Param.Byte[2] = EarthMagneticField / (5 * 2); |
|
- | 488 | ToFlightCtrl.Param.Byte[3] = EarthMagneticInclination / 2; |
|
452 | hott_index = 0; |
489 | hott_index = 0; |
453 | break; |
490 | break; |
454 | default: |
491 | default: |
455 | ToFlightCtrl.Param.Byte[0] = 255; |
492 | ToFlightCtrl.Param.Byte[0] = 255; |
456 | hott_index = 0; |
493 | hott_index = 0; |
457 | break; |
494 | break; |
458 | } |
495 | } |
459 | break; |
496 | break; |
460 | default: |
497 | default: |
461 | break; |
498 | break; |
462 | // 0 = 0,1 |
499 | // 0 = 0,1 |
Line 487... | Line 524... | ||
487 | FC.StatusFlags |= FromFlightCtrl.Param.Byte[8]; |
524 | FC.StatusFlags |= FromFlightCtrl.Param.Byte[8]; |
488 | if(FC.StatusFlags & FC_STATUS_CALIBRATE && !FCCalibActive) |
525 | if(FC.StatusFlags & FC_STATUS_CALIBRATE && !FCCalibActive) |
489 | { |
526 | { |
490 | HeadFreeStartAngle = Compass_Heading * 10; |
527 | HeadFreeStartAngle = Compass_Heading * 10; |
491 | Compass_Init(); |
528 | Compass_Init(); |
492 | FCCalibActive = 1; |
529 | FCCalibActive = 10; |
493 | } |
530 | } |
494 | else |
531 | else |
495 | { |
532 | { |
496 | FCCalibActive = 0; |
533 | if(FCCalibActive) if(--FCCalibActive == 0) FC_is_Calibrated = 1; |
497 | } |
534 | } |
498 | if(FC.StatusFlags & FC_STATUS_START) |
535 | if(FC.StatusFlags & FC_STATUS_START) |
499 | { |
536 | { |
500 | if(Compass_Heading != -1) HeadFreeStartAngle = Compass_Heading * 10; else |
537 | if(Compass_Heading != -1) HeadFreeStartAngle = Compass_Heading * 10; else |
501 | HeadFreeStartAngle = FromFlightCtrl.GyroHeading; |
538 | HeadFreeStartAngle = FromFlightCtrl.GyroHeading; |