Rev 392 | Rev 399 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 392 | Rev 397 | ||
---|---|---|---|
Line 76... | Line 76... | ||
76 | #define SPI_TXSYNCBYTE2 0x55 |
76 | #define SPI_TXSYNCBYTE2 0x55 |
Line 77... | Line 77... | ||
77 | 77 | ||
78 | //communication packets |
78 | //communication packets |
79 | FromFlightCtrl_t FromFlightCtrl; |
79 | FromFlightCtrl_t FromFlightCtrl; |
80 | ToFlightCtrl_t ToFlightCtrl; |
80 | ToFlightCtrl_t ToFlightCtrl; |
81 | #define SPI0_TIMEOUT 500 // 500ms |
81 | #define SPI0_TIMEOUT 2500 //ms |
82 | volatile u32 SPI0_Timeout = 0; |
82 | volatile u32 SPI0_Timeout = 0; |
- | 83 | u8 Logging_FCStatusFlags1 = 0,Logging_FCStatusFlags2 = 0; |
|
Line 83... | Line 84... | ||
83 | u8 Logging_FCStatusFlags1 = 0,Logging_FCStatusFlags2 = 0; |
84 | u8 SpeakHoTT = 0; |
84 | 85 | ||
85 | // tx packet buffer |
86 | // tx packet buffer |
86 | #define SPI_TXBUFFER_LEN (2 + sizeof(ToFlightCtrl)) // 2 bytes at start are for synchronization |
87 | #define SPI_TXBUFFER_LEN (2 + sizeof(ToFlightCtrl)) // 2 bytes at start are for synchronization |
Line 360... | Line 361... | ||
360 | break; |
361 | break; |
361 | case SPI_MISC: |
362 | case SPI_MISC: |
362 | ToFlightCtrl.Param.Byte[0] = EarthMagneticFieldFiltered/5; |
363 | ToFlightCtrl.Param.Byte[0] = EarthMagneticFieldFiltered/5; |
363 | ToFlightCtrl.Param.Byte[1] = EarthMagneticInclination; |
364 | ToFlightCtrl.Param.Byte[1] = EarthMagneticInclination; |
364 | ToFlightCtrl.Param.Byte[2] = EarthMagneticInclinationTheoretic; |
365 | ToFlightCtrl.Param.Byte[2] = EarthMagneticInclinationTheoretic; |
365 | ToFlightCtrl.Param.Byte[3] = 0; |
366 | ToFlightCtrl.Param.Byte[3] = SpeakHoTT; |
366 | ToFlightCtrl.Param.Byte[4] = 0; |
367 | ToFlightCtrl.Param.Byte[4] = 0; |
367 | ToFlightCtrl.Param.Byte[5] = 0; |
368 | ToFlightCtrl.Param.Byte[5] = 0; |
368 | ToFlightCtrl.Param.Byte[6] = 0; |
369 | ToFlightCtrl.Param.Byte[6] = 0; |
369 | ToFlightCtrl.Param.Byte[7] = 0; |
370 | ToFlightCtrl.Param.Byte[7] = 0; |
370 | ToFlightCtrl.Param.Byte[8] = 0; |
371 | ToFlightCtrl.Param.Byte[8] = 0; |
371 | ToFlightCtrl.Param.Byte[9] = 0; |
372 | ToFlightCtrl.Param.Byte[9] = 0; |
372 | ToFlightCtrl.Param.Byte[10] = 0; |
373 | ToFlightCtrl.Param.Byte[10] = 0; |
373 | ToFlightCtrl.Param.Byte[11] = 0; |
374 | ToFlightCtrl.Param.Byte[11] = 0; |
- | 375 | //DebugOut.Analog[16] = SpeakHoTT; |
|
- | 376 | SpeakHoTT = 0; |
|
374 | break; |
377 | break; |
Line 375... | Line 378... | ||
375 | 378 | ||
376 | case SPI_NCCMD_GPSINFO: |
379 | case SPI_NCCMD_GPSINFO: |
377 | ToFlightCtrl.Param.Byte[0] = GPSData.Flags; |
380 | ToFlightCtrl.Param.Byte[0] = GPSData.Flags; |
Line 404... | Line 407... | ||
404 | { |
407 | { |
405 | case 0: |
408 | case 0: |
406 | //Dezimalgrad --> Grad mit Dezimalminuten --> Grad, Minuten, Sekunden |
409 | //Dezimalgrad --> Grad mit Dezimalminuten --> Grad, Minuten, Sekunden |
407 | //53.285788 7.4847269 --> N53° 17.14728 E7° 29.08362 --> N53° 17' 8.837" E7° 29' 5.017" |
410 | //53.285788 7.4847269 --> N53° 17.14728 E7° 29.08362 --> N53° 17' 8.837" E7° 29' 5.017" |
408 | ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID; |
411 | ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID; |
409 | ToFlightCtrl.Param.Byte[0] = 3; // index |
412 | ToFlightCtrl.Param.Byte[0] = 3+3; // index +3, weil bei HoTT V4 3 Bytes eingeschoben wurden |
410 | ToFlightCtrl.Param.Byte[1] = 9-1; // how many |
413 | ToFlightCtrl.Param.Byte[1] = 9-1; // how many |
411 | //----------------------------- |
414 | //----------------------------- |
412 | ToFlightCtrl.Param.Byte[2] = NaviData.HomePositionDeviation.Bearing / 2; |
415 | ToFlightCtrl.Param.Byte[2] = FromFlightCtrl.GyroHeading / 20;//NaviData.HomePositionDeviation.Bearing / 2; |
413 | i1 = GPSData.Speed_Ground; // in cm/sec |
416 | i1 = GPSData.Speed_Ground; // in cm/sec |
414 | i1 *= 36; |
417 | i1 *= 36; |
415 | i1 /= 1000; |
418 | i1 /= 1000; |
416 | ToFlightCtrl.Param.Byte[3] = i1 % 256; |
419 | ToFlightCtrl.Param.Byte[3] = i1 % 256; |
417 | ToFlightCtrl.Param.Byte[4] = i1 / 256; |
420 | ToFlightCtrl.Param.Byte[4] = i1 / 256; |
418 | //----------------------------- |
421 | //----------------------------- |
419 | if(GPSData.Position.Latitude < 0) ToFlightCtrl.Param.Byte[2] = 1; // 1 = S |
422 | if(GPSData.Position.Latitude < 0) ToFlightCtrl.Param.Byte[2] = 1; // 1 = S |
420 | else ToFlightCtrl.Param.Byte[5] = 0; // 1 = S |
423 | else ToFlightCtrl.Param.Byte[5] = 0; // 1 = S |
421 | i1 = abs(GPSData.Position.Latitude)/10000000L; |
424 | i1 = abs(GPSData.Position.Latitude)/10000000L; |
422 | i2 = abs(GPSData.Position.Latitude)%10000000L; |
425 | i2 = abs(GPSData.Position.Latitude)%10000000L; |
- | 426 | if(!(NCFlags & NC_FLAG_GPS_OK)) {i1 = 0; i2 = 0;} |
|
423 | i1 *= 100; |
427 | i1 *= 100; |
424 | i1 += i2 / 100000; |
428 | i1 += i2 / 100000; |
425 | i2 = i2 % 100000; |
429 | i2 = i2 % 100000; |
426 | i2 /= 10; |
430 | i2 /= 10; |
427 | ToFlightCtrl.Param.Byte[6] = i1 % 256; |
431 | ToFlightCtrl.Param.Byte[6] = i1 % 256; |
Line 429... | Line 433... | ||
429 | ToFlightCtrl.Param.Byte[8] = i2 % 256; |
433 | ToFlightCtrl.Param.Byte[8] = i2 % 256; |
430 | ToFlightCtrl.Param.Byte[9] = i2 / 256; |
434 | ToFlightCtrl.Param.Byte[9] = i2 / 256; |
431 | break; |
435 | break; |
432 | case 1: |
436 | case 1: |
433 | ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID; |
437 | ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID; |
434 | ToFlightCtrl.Param.Byte[0] = 11; // index |
438 | ToFlightCtrl.Param.Byte[0] = 11+3; // index +3, weil bei HoTT V4 3 Bytes eingeschoben wurden |
435 | ToFlightCtrl.Param.Byte[1] = 8-1; // how many |
439 | ToFlightCtrl.Param.Byte[1] = 8-1; // how many |
436 | //----------------------------- |
440 | //----------------------------- |
437 | if(GPSData.Position.Longitude < 0) ToFlightCtrl.Param.Byte[2] = 1; // 1 = E |
441 | if(GPSData.Position.Longitude < 0) ToFlightCtrl.Param.Byte[2] = 1; // 1 = E |
438 | else ToFlightCtrl.Param.Byte[2] = 0; // 1 = S |
442 | else ToFlightCtrl.Param.Byte[2] = 0; // 1 = S |
439 | i1 = abs(GPSData.Position.Longitude)/10000000L; |
443 | i1 = abs(GPSData.Position.Longitude)/10000000L; |
440 | i2 = abs(GPSData.Position.Longitude)%10000000L; |
444 | i2 = abs(GPSData.Position.Longitude)%10000000L; |
- | 445 | if(!(NCFlags & NC_FLAG_GPS_OK)) {i1 = 0; i2 = 0;} |
|
441 | i1 *= 100; |
446 | i1 *= 100; |
442 | i1 += i2 / 100000; |
447 | i1 += i2 / 100000; |
443 | i2 = i2 % 100000; |
448 | i2 = i2 % 100000; |
444 | i2 /= 10; |
449 | i2 /= 10; |
445 | ToFlightCtrl.Param.Byte[3] = i1 % 256; |
450 | ToFlightCtrl.Param.Byte[3] = i1 % 256; |
Line 451... | Line 456... | ||
451 | ToFlightCtrl.Param.Byte[7] = i1 % 256; |
456 | ToFlightCtrl.Param.Byte[7] = i1 % 256; |
452 | ToFlightCtrl.Param.Byte[8] = i1 / 256; |
457 | ToFlightCtrl.Param.Byte[8] = i1 / 256; |
453 | break; |
458 | break; |
454 | case 2: |
459 | case 2: |
455 | ToFlightCtrl.Param.Byte[11] = HOTT_GENERAL_PACKET_ID; |
460 | ToFlightCtrl.Param.Byte[11] = HOTT_GENERAL_PACKET_ID; |
456 | ToFlightCtrl.Param.Byte[0] = 5; // index |
461 | ToFlightCtrl.Param.Byte[0] = 5+3; // index // +3, weil bei HoTT V4 3 Bytes eingeschoben wurden |
457 | ToFlightCtrl.Param.Byte[1] = 2; // how many |
462 | ToFlightCtrl.Param.Byte[1] = 2; // how many |
458 | ToFlightCtrl.Param.Byte[2] = EarthMagneticField / (5 * 2); |
463 | ToFlightCtrl.Param.Byte[2] = EarthMagneticField / (5 * 2); |
459 | ToFlightCtrl.Param.Byte[3] = EarthMagneticInclination / 2; |
464 | ToFlightCtrl.Param.Byte[3] = EarthMagneticInclination / 2; |
460 | hott_index = 0; |
465 | hott_index = 0; |
461 | break; |
466 | break; |
Line 667... | Line 672... | ||
667 | 672 | ||
668 | 673 | ||
669 | /* |
674 | /* |
- | 675 | //+++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 676 | */ |
|
670 | //+++++++++++++++++++++++++++++++++++++++++++++++++++ |
677 | |
671 | */ |
678 | |
672 | GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); |
679 | GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); |
673 | ClearFCStatusFlags = 1; |
680 | ClearFCStatusFlags = 1; |
674 | if(counter) |
681 | if(counter) |