Rev 482 | Rev 486 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 482 | Rev 483 | ||
|---|---|---|---|
| Line 61... | Line 61... | ||
| 61 | #include "91x_lib.h" |
61 | #include "91x_lib.h" |
| 62 | #include "main.h" |
62 | #include "main.h" |
| 63 | #include "config.h" |
63 | #include "config.h" |
| 64 | #include "menu.h" |
64 | #include "menu.h" |
| 65 | #include "GPS.h" |
65 | #include "GPS.h" |
| - | 66 | #include "i2c0.h" |
|
| 66 | #include "i2c.h" |
67 | #include "i2c1.h" |
| 67 | #include "uart0.h" |
68 | #include "uart0.h" |
| 68 | #include "uart1.h" |
69 | #include "uart1.h" |
| 69 | #include "uart2.h" |
70 | #include "uart2.h" |
| 70 | #include "timer1.h" |
71 | #include "timer1.h" |
| 71 | #include "timer2.h" |
72 | #include "timer2.h" |
| Line 245... | Line 246... | ||
| 245 | UART_StructInit(&UART_InitStructure); |
246 | UART_StructInit(&UART_InitStructure); |
| 246 | UART_InitStructure.UART_WordLength = UART_WordLength_8D; |
247 | UART_InitStructure.UART_WordLength = UART_WordLength_8D; |
| 247 | UART_InitStructure.UART_StopBits = UART_StopBits_1; |
248 | UART_InitStructure.UART_StopBits = UART_StopBits_1; |
| 248 | UART_InitStructure.UART_Parity = UART_Parity_No ; |
249 | UART_InitStructure.UART_Parity = UART_Parity_No ; |
| 249 | UART_InitStructure.UART_BaudRate = UART1_BAUD_RATE; |
250 | UART_InitStructure.UART_BaudRate = UART1_BAUD_RATE; |
| 250 | UART_InitStructure. UART_HardwareFlowControl = UART_HardwareFlowControl_None; |
251 | UART_InitStructure.UART_HardwareFlowControl = UART_HardwareFlowControl_None; |
| 251 | UART_InitStructure.UART_Mode = UART_Mode_Tx_Rx; |
252 | UART_InitStructure.UART_Mode = UART_Mode_Tx_Rx; |
| 252 | UART_InitStructure.UART_FIFO = UART_FIFO_Enable; |
253 | // UART_InitStructure.UART_FIFO = UART_FIFO_Enable; |
| 253 | UART_InitStructure.UART_TxFIFOLevel = UART_FIFOLevel_1_2; |
254 | UART_InitStructure.UART_FIFO = UART_FIFO_Disable; |
| 254 | UART_InitStructure.UART_RxFIFOLevel = UART_FIFOLevel_1_2; |
255 | // UART_InitStructure.UART_TxFIFOLevel = UART_FIFOLevel_1_2; |
| - | 256 | UART_InitStructure.UART_TxFIFOLevel = UART_FIFOLevel_1_8; // FIFO size 16 bytes, FIFO level 2 bytes |
|
| - | 257 | UART_InitStructure.UART_RxFIFOLevel = UART_FIFOLevel_1_2; // FIFO size 16 bytes, FIFO level 8 bytes |
|
| Line 255... | Line 258... | ||
| 255 | 258 | ||
| 256 | UART_DeInit(UART1); // reset uart 1 to default |
259 | UART_DeInit(UART1); // reset uart 1 to default |
| 257 | UART_Init(UART1, &UART_InitStructure); // initialize uart 1 |
260 | UART_Init(UART1, &UART_InitStructure); // initialize uart 1 |
| 258 | // enable uart 1 interrupts selective |
261 | // enable uart 1 interrupts selective |
| Line 650... | Line 653... | ||
| 650 | /**************************************************************/ |
653 | /**************************************************************/ |
| 651 | void UART1_Transmit(void) |
654 | void UART1_Transmit(void) |
| 652 | { |
655 | { |
| 653 | u8 tmp_tx; |
656 | u8 tmp_tx; |
| 654 | if(DebugUART != UART1) return; |
657 | if(DebugUART != UART1) return; |
| - | 658 | if(I2C0_State != I2C_STATE_IDLE) return; |
|
| 655 | // if something has to be send and the txd fifo is not full |
659 | // if something has to be send and the txd fifo is not full |
| 656 | if(UART1_tx_buffer.Locked == TRUE) |
660 | if(UART1_tx_buffer.Locked == TRUE) |
| 657 | { |
661 | { |
| 658 | // while there is some space in the tx fifo |
662 | // while there is some space in the tx fifo |
| 659 | while(UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) != SET) |
663 | while(UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) != SET) |
| Line 683... | Line 687... | ||
| 683 | //$GPGGA,191410,4735.5634,N,00739.3538,E,1,04,4.4,351.5,M,48.0,M,,*45 |
687 | //$GPGGA,191410,4735.5634,N,00739.3538,E,1,04,4.4,351.5,M,48.0,M,,*45 |
| 684 | //$GPGGA,092120.20,,,,,0,00,99.99,,,,,,*6C |
688 | //$GPGGA,092120.20,,,,,0,00,99.99,,,,,,*6C |
| 685 | //http://www.kowoma.de/gps/zusatzerklaerungen/NMEA.htm |
689 | //http://www.kowoma.de/gps/zusatzerklaerungen/NMEA.htm |
| 686 | void CreateNmeaGGA(void) |
690 | void CreateNmeaGGA(void) |
| 687 | { |
691 | { |
| 688 | u8 array[200], i = 0, crc = 0, x; |
692 | unsigned char array[200], i = 0, crc = 0, x; |
| 689 | s32 tmp1, tmp2; |
693 | long tmp1, tmp2; |
| 690 | - | ||
| 691 | i += sprintf(array, "$GPGGA,"); |
694 | i += sprintf(array, "$GPGGA,"); |
| 692 | // +++++++++++++++++++++++++++++++++++++++++++ |
695 | // +++++++++++++++++++++++++++++++++++++++++++ |
| 693 | if(SystemTime.Valid) |
696 | if(SystemTime.Valid) |
| 694 | { |
697 | { |
| 695 | i += sprintf(&array[i], "%02d%02d%02d.%02d,", SystemTime.Hour, SystemTime.Min, SystemTime.Sec, SystemTime.mSec/10); |
698 | i += sprintf(&array[i], "%02d%02d%02d.%02d,",SystemTime.Hour,SystemTime.Min,SystemTime.Sec,SystemTime.mSec/10); |
| 696 | } |
699 | } |
| 697 | else |
700 | else |
| 698 | { |
701 | { |
| 699 | i += sprintf(&array[i], ","); |
702 | i += sprintf(&array[i], ","); |
| 700 | } |
703 | } |
| 701 | // +++++++++++++++++++++++++++++++++++++++++++ |
704 | // +++++++++++++++++++++++++++++++++++++++++++ |
| 702 | if(GPSData.Flags & FLAG_GPSFIXOK) |
705 | if(GPSData.Flags & FLAG_GPSFIXOK) |
| 703 | { |
706 | { |
| 704 | tmp1 = abs(GPSData.Position.Latitude)/10000000L; |
707 | tmp1 = abs(GPSData.Position.Latitude)/10000000L; |
| 705 | i += sprintf(&array[i],"%02d",(s16)tmp1); |
708 | i += sprintf(&array[i],"%02d",(int)tmp1); |
| 706 | 709 | ||
| 707 | tmp1 = abs(GPSData.Position.Latitude)%10000000L; |
710 | tmp1 = abs(GPSData.Position.Latitude)%10000000L; |
| 708 | tmp1 *= 6; // in Minuten |
711 | tmp1 *= 6; // in Minuten |
| 709 | tmp2 = tmp1 / 1000000L; |
712 | tmp2 = tmp1 / 1000000L; |
| 710 | i += sprintf(&array[i],"%02d", (s16)tmp2); |
713 | i += sprintf(&array[i],"%02d",(int)tmp2); |
| 711 | tmp2 = tmp1 % 1000000L; |
714 | tmp2 = tmp1 % 1000000L; |
| 712 | tmp2 /= 10; // eine Stelle zu viel |
715 | tmp2 /= 10; // eine Stelle zu viel |
| 713 | i += sprintf(&array[i],".%05d,", (s16)tmp2); |
716 | i += sprintf(&array[i],".%05d,",(int)tmp2); |
| 714 | 717 | ||
| 715 | if(GPSData.Position.Latitude >= 0) i += sprintf(&array[i],"N,"); |
718 | if(GPSData.Position.Latitude >= 0) i += sprintf(&array[i],"N,"); |
| 716 | else i += sprintf(&array[i],"S,"); |
719 | else i += sprintf(&array[i],"S,"); |
| 717 | // +++++++++++++++++++++++++++++++++++++++++++ |
720 | // +++++++++++++++++++++++++++++++++++++++++++ |
| 718 | 721 | ||
| 719 | tmp1 = abs(GPSData.Position.Longitude)/10000000L; |
722 | tmp1 = abs(GPSData.Position.Longitude)/10000000L; |
| 720 | i += sprintf(&array[i],"%03d", (s16)tmp1); |
723 | i += sprintf(&array[i],"%03d",(int)tmp1); |
| 721 | 724 | ||
| 722 | tmp1 = abs(GPSData.Position.Longitude)%10000000L; |
725 | tmp1 = abs(GPSData.Position.Longitude)%10000000L; |
| 723 | tmp1 *= 6; // in Minuten |
726 | tmp1 *= 6; // in Minuten |
| 724 | tmp2 = tmp1 / 1000000L; |
727 | tmp2 = tmp1 / 1000000L; |
| 725 | i += sprintf(&array[i],"%02d", (s16)tmp2); |
728 | i += sprintf(&array[i],"%02d",(int)tmp2); |
| 726 | tmp2 = tmp1 % 1000000L; |
729 | tmp2 = tmp1 % 1000000L; |
| 727 | tmp2 /= 10; // eine Stelle zu viel |
730 | tmp2 /= 10; // eine Stelle zu viel |
| 728 | i += sprintf(&array[i],".%05d,", (s16)tmp2); |
731 | i += sprintf(&array[i],".%05d,",(int)tmp2); |
| 729 | 732 | ||
| 730 | 733 | ||
| 731 | if(GPSData.Position.Longitude >= 0) i += sprintf(&array[i],"E,"); |
734 | if(GPSData.Position.Longitude >= 0) i += sprintf(&array[i],"E,"); |
| 732 | else i += sprintf(&array[i],"W,"); |
735 | else i += sprintf(&array[i],"W,"); |
| 733 | i += sprintf(&array[i],"%d,",GPSData.SatFix); |
736 | i += sprintf(&array[i],"%d,",GPSData.SatFix); |
| 734 | i += sprintf(&array[i],"%d,",GPSData.NumOfSats); |
737 | i += sprintf(&array[i],"%d,",GPSData.NumOfSats); |
| 735 | i += sprintf(&array[i],"%d.%d,",(s16)(GPSData.Position_Accuracy/100),abs(GPSData.Position_Accuracy%100)); |
738 | i += sprintf(&array[i],"%d.%d,",GPSData.Position_Accuracy/100,abs(GPSData.Position_Accuracy%100)); |
| 736 | // i += sprintf(&array[i],"%d.%d,M,",GPSData.Position.Altitude/1000,abs(GPSData.Position.Altitude%1000)/100); |
739 | // i += sprintf(&array[i],"%d.%d,M,",GPSData.Position.Altitude/1000,abs(GPSData.Position.Altitude%1000)/100); |
| 737 | tmp1 = NaviData.Altimeter / 2; // in dm |
740 | tmp1 = NaviData.Altimeter / 2; // in dm |
| 738 | i += sprintf(&array[i],"%d.%d,M,",(s16)tmp1 / 10,abs((s16)tmp1 % 10)); |
741 | i += sprintf(&array[i],"%d.%d,M,",tmp1 / 10,abs(tmp1 % 10)); |
| 739 | i += sprintf(&array[i],",,,*"); |
742 | i += sprintf(&array[i],",,,*"); |
| 740 | } |
743 | } |
| 741 | else |
744 | else |
| 742 | { |
745 | { |
| 743 | i += sprintf(&array[i], ",,,,%d,00,99.99,,,,,,*",GPSData.NumOfSats); |
746 | i += sprintf(&array[i], ",,,,%d,00,99.99,,,,,,*",GPSData.NumOfSats); |
| 744 | } |
747 | } |
| 745 | for(x = 1; x < i-1; x++) |
748 | for(x=1; x<i-1; x++) |
| 746 | { |
749 | { |
| 747 | crc ^= array[x]; |
750 | crc ^= array[x]; |
| 748 | } |
751 | } |
| 749 | i += sprintf(&array[i], "%02x%c%c",crc,0x0d,0x0a); |
752 | i += sprintf(&array[i], "%02x%c%c",crc,0x0d,0x0a); |
| 750 | AddSerialData(&UART1_tx_buffer,array,i); |
753 | AddSerialData(&UART1_tx_buffer,array,i); |
| Line 751... | Line 754... | ||
| 751 | 754 | ||
| 752 | // +++++++++++++++++++++++++++++++++++++++++++ |
755 | // +++++++++++++++++++++++++++++++++++++++++++ |
| Line 753... | Line 756... | ||
| 753 | } |
756 | } |
| 754 | 757 | ||
| Line 755... | Line 758... | ||
| 755 | //$GPRMC,162614.123,A,5230.5900,N,01322.3900,E,10.0,90.0,131006,1.2,E,A*13 |
758 | //$GPRMC,162614.123,A,5230.5900,N,01322.3900,E,10.0,90.0,131006,1.2,E,A*13 |
| 756 | //$GPRMC,HHMMSS.sss,A,BBBB.BBBB,b,LLLLL.LLLL,l,GG.G,RR.R,DDMMYY,M.M,m,F*PP |
759 | //$GPRMC,HHMMSS.sss,A,BBBB.BBBB,b,LLLLL.LLLL,l,GG.G,RR.R,DDMMYY,M.M,m,F*PP |
| 757 | 760 | ||
| 758 | void CreateNmeaRMC(void) |
761 | void CreateNmeaRMC(void) |
| 759 | { |
762 | { |
| 760 | u8 array[200], i = 0, crc = 0, x; |
763 | unsigned char array[200], i = 0, crc = 0, x; |
| 761 | s16 tmp_int; |
764 | int tmp_int; |
| 762 | s32 tmp1, tmp2; |
765 | long tmp1, tmp2; |
| 763 | // +++++++++++++++++++++++++++++++++++++++++++ |
766 | // +++++++++++++++++++++++++++++++++++++++++++ |
| 764 | i += sprintf(array, "$GPRMC,"); |
767 | i += sprintf(array, "$GPRMC,"); |
| 765 | // +++++++++++++++++++++++++++++++++++++++++++ |
768 | // +++++++++++++++++++++++++++++++++++++++++++ |
| 766 | if(SystemTime.Valid) |
769 | if(SystemTime.Valid) |
| 767 | { |
770 | { |
| 768 | i += sprintf(&array[i], "%02d%02d%02d.%03d,", SystemTime.Hour, SystemTime.Min, SystemTime.Sec, SystemTime.mSec); |
771 | i += sprintf(&array[i], "%02d%02d%02d.%03d,",SystemTime.Hour,SystemTime.Min,SystemTime.Sec,SystemTime.mSec); |
| 769 | } |
772 | } |
| 770 | else |
773 | else |
| 771 | { |
774 | { |
| 772 | i += sprintf(&array[i], ","); |
775 | i += sprintf(&array[i], ","); |
| 773 | } |
776 | } |
| 774 | if(GPSData.Flags & FLAG_GPSFIXOK) |
777 | if(GPSData.Flags & FLAG_GPSFIXOK) |
| 775 | { |
778 | { |
| 776 | // +++++++++++++++++++++++++++++++++++++++++++ |
779 | // +++++++++++++++++++++++++++++++++++++++++++ |
| 777 | tmp1 = abs(GPSData.Position.Latitude)/10000000L; |
780 | tmp1 = abs(GPSData.Position.Latitude)/10000000L; |
| 778 | i += sprintf(&array[i],"A,%02d", (s16)tmp1); // Status: A = Okay V = Warnung |
781 | i += sprintf(&array[i],"A,%02d",(int)tmp1); // Status: A = Okay V = Warnung |
| 779 | 782 | ||
| 780 | tmp1 = abs(GPSData.Position.Latitude)%10000000L; |
783 | tmp1 = abs(GPSData.Position.Latitude)%10000000L; |
| 781 | tmp1 *= 6; // in Minuten |
784 | tmp1 *= 6; // in Minuten |
| 782 | tmp2 = tmp1 / 1000000L; |
785 | tmp2 = tmp1 / 1000000L; |
| 783 | i += sprintf(&array[i],"%02d", (s16)tmp2); |
786 | i += sprintf(&array[i],"%02d",(int)tmp2); |
| 784 | tmp2 = tmp1 % 1000000L; |
787 | tmp2 = tmp1 % 1000000L; |
| 785 | tmp2 /= 10; // eine Stelle zu viel |
788 | tmp2 /= 10; // eine Stelle zu viel |
| 786 | i += sprintf(&array[i],".%05d,", (s16)tmp2); |
789 | i += sprintf(&array[i],".%05d,",(int)tmp2); |
| 787 | if(GPSData.Position.Latitude >= 0) i += sprintf(&array[i],"N,"); |
790 | if(GPSData.Position.Latitude >= 0) i += sprintf(&array[i],"N,"); |
| 788 | else i += sprintf(&array[i],"S,"); |
791 | else i += sprintf(&array[i],"S,"); |
| 789 | // +++++++++++++++++++++++++++++++++++++++++++ |
792 | // +++++++++++++++++++++++++++++++++++++++++++ |
| 790 | tmp1 = abs(GPSData.Position.Longitude)/10000000L; |
793 | tmp1 = abs(GPSData.Position.Longitude)/10000000L; |
| 791 | i += sprintf(&array[i],"%03d", (s16)tmp1); |
794 | i += sprintf(&array[i],"%03d",(int)tmp1); |
| 792 | 795 | ||
| 793 | tmp1 = abs(GPSData.Position.Longitude)%10000000L; |
796 | tmp1 = abs(GPSData.Position.Longitude)%10000000L; |
| 794 | tmp1 *= 6; // in Minuten |
797 | tmp1 *= 6; // in Minuten |
| 795 | tmp2 = tmp1 / 1000000L; |
798 | tmp2 = tmp1 / 1000000L; |
| 796 | i += sprintf(&array[i],"%02d", (s16)tmp2); |
799 | i += sprintf(&array[i],"%02d",(int)tmp2); |
| 797 | tmp2 = tmp1 % 1000000L; |
800 | tmp2 = tmp1 % 1000000L; |
| 798 | tmp2 /= 10; // eine Stelle zu viel |
801 | tmp2 /= 10; // eine Stelle zu viel |
| 799 | i += sprintf(&array[i],".%05d,", (s16)tmp2); |
802 | i += sprintf(&array[i],".%05d,",(int)tmp2); |
| 800 | if(GPSData.Position.Longitude >= 0) i += sprintf(&array[i],"E,"); |
803 | if(GPSData.Position.Longitude >= 0) i += sprintf(&array[i],"E,"); |
| 801 | else i += sprintf(&array[i],"W,"); |
804 | else i += sprintf(&array[i],"W,"); |
| 802 | // +++++++++++++++++++++++++++++++++++++++++++ |
805 | // +++++++++++++++++++++++++++++++++++++++++++ |
| 803 | tmp_int = GPSData.Speed_Ground; // in cm/sek |
806 | tmp_int = GPSData.Speed_Ground; // in cm/sek |
| 804 | tmp_int *= 90; |
807 | tmp_int *= 90; |
| 805 | tmp_int /= 463; |
808 | tmp_int /= 463; |
| 806 | i += sprintf(&array[i],"%02d.%d,",tmp_int/10,tmp_int%10); // in Knoten |
809 | i += sprintf(&array[i],"%02d.%d,",tmp_int/10,tmp_int%10); // in Knoten |
| 807 | // +++++++++++++++++++++++++++++++++++++++++++ |
810 | // +++++++++++++++++++++++++++++++++++++++++++ |
| 808 | i += sprintf(&array[i],"%03d.%d,",GyroCompassCorrected/10,GyroCompassCorrected%10); |
811 | i += sprintf(&array[i],"%03d.%d,",GyroCompassCorrected/10,GyroCompassCorrected%10); |
| 809 | // +++++++++++++++++++++++++++++++++++++++++++ |
812 | // +++++++++++++++++++++++++++++++++++++++++++ |
| 810 | if(SystemTime.Valid) |
813 | if(SystemTime.Valid) |
| 811 | { |
814 | { |
| 812 | i += sprintf(&array[i], "%02d%02d%02d,",SystemTime.Day,SystemTime.Month,SystemTime.Year); |
815 | i += sprintf(&array[i], "%02d%02d%02d,",SystemTime.Day,SystemTime.Month,SystemTime.Year); |
| 813 | } |
816 | } |
| 814 | else |
817 | else |
| 815 | { |
818 | { |
| 816 | i += sprintf(&array[i], ","); |
819 | i += sprintf(&array[i], ","); |
| 817 | } |
820 | } |
| 818 | // +++++++++++++++++++++++++++++++++++++++++++ |
821 | // +++++++++++++++++++++++++++++++++++++++++++ |
| 819 | i += sprintf(&array[i],"%d.%1d,", abs(GeoMagDec)/10,abs(GeoMagDec)%10); |
822 | i += sprintf(&array[i],"%d.%1d,", abs(GeoMagDec)/10,abs(GeoMagDec)%10); |
| 820 | if(GeoMagDec < 0) i += sprintf(&array[i], "W,"); else i += sprintf(&array[i], "E,"); |
823 | if(GeoMagDec < 0) i += sprintf(&array[i], "W,"); else i += sprintf(&array[i], "E,"); |
| 821 | // +++++++++++++++++++++++++++++++++++++++++++ |
824 | // +++++++++++++++++++++++++++++++++++++++++++ |
| 822 | if(GPSData.Flags & FLAG_DIFFSOLN) i += sprintf(&array[i], "D,"); |
825 | if(GPSData.Flags & FLAG_DIFFSOLN) i += sprintf(&array[i], "D,"); |
| 823 | else i += sprintf(&array[i], "A,"); |
826 | else i += sprintf(&array[i], "A,"); |
| 824 | } |
827 | } |
| 825 | else // kein Satfix |
828 | else // kein Satfix |
| 826 | { |
829 | { |
| 827 | i += sprintf(&array[i], "V,,,,,,,,,,N*"); |
830 | i += sprintf(&array[i], "V,,,,,,,,,,N*"); |
| 828 | } |
831 | } |
| 829 | // +++++++++++++++++++++++++++++++++++++++++++ |
832 | // +++++++++++++++++++++++++++++++++++++++++++ |
| 830 | // CRC |
833 | // CRC |
| 831 | // +++++++++++++++++++++++++++++++++++++++++++ |
834 | // +++++++++++++++++++++++++++++++++++++++++++ |
| 832 | for(x=1; x<i-1; x++) |
835 | for(x=1; x<i-1; x++) |
| 833 | { |
836 | { |
| 834 | crc ^= array[x]; |
837 | crc ^= array[x]; |
| 835 | } |
838 | } |
| 836 | i += sprintf(&array[i], "%02x%c%c",crc,0x0d,0x0a); |
839 | i += sprintf(&array[i], "%02x%c%c",crc,0x0d,0x0a); |
| 837 | // +++++++++++++++++++++++++++++++++++++++++++ |
840 | // +++++++++++++++++++++++++++++++++++++++++++ |
| Line 838... | Line 841... | ||
| 838 | AddSerialData(&UART1_tx_buffer,array,i); |
841 | AddSerialData(&UART1_tx_buffer,array,i); |
| 839 | // +++++++++++++++++++++++++++++++++++++++++++ |
842 | // +++++++++++++++++++++++++++++++++++++++++++ |
| 840 | /* |
843 | /* |
| 841 | |
844 | |
| 842 | |
845 | |
| 843 | 846 | ||
| 844 | GPSData.Flags = (GPSData.Flags & 0xf0) | (UbxSol.Flags & 0x0f); // we take only the lower bits |
847 | GPSData.Flags = (GPSData.Flags & 0xf0) | (UbxSol.Flags & 0x0f); // we take only the lower bits |
| 845 | GPSData.NumOfSats = UbxSol.numSV; |
848 | GPSData.NumOfSats = UbxSol.numSV; |
| 846 | GPSData.SatFix = UbxSol.GPSfix; |
849 | GPSData.SatFix = UbxSol.GPSfix; |
| 847 | GPSData.Position_Accuracy = UbxSol.PAcc; |
850 | GPSData.Position_Accuracy = UbxSol.PAcc; |
| 848 | GPSData.Speed_Accuracy = UbxSol.SAcc; |
851 | GPSData.Speed_Accuracy = UbxSol.SAcc; |
| 849 | SetGPSTime(&SystemTime); // update system time |
852 | SetGPSTime(&SystemTime); // update system time |
| 850 | // NAV POSLLH |
853 | // NAV POSLLH |
| 851 | GPSData.Position.Status = INVALID; |
854 | GPSData.Position.Status = INVALID; |
| 852 | GPSData.Position.Longitude = UbxPosLlh.LON; |
855 | GPSData.Position.Longitude = UbxPosLlh.LON; |
| 853 | GPSData.Position.Latitude = UbxPosLlh.LAT; |
856 | GPSData.Position.Latitude = UbxPosLlh.LAT; |
| 854 | GPSData.Position.Altitude = UbxPosLlh.HMSL; |
857 | GPSData.Position.Altitude = UbxPosLlh.HMSL; |
| 855 | GPSData.Position.Status = NEWDATA; |
858 | GPSData.Position.Status = NEWDATA; |
| 856 | // NAV VELNED |
859 | // NAV VELNED |
| 857 | GPSData.Speed_East = UbxVelNed.VEL_E; |
860 | GPSData.Speed_East = UbxVelNed.VEL_E; |
| 858 | GPSData.Speed_North = UbxVelNed.VEL_N; |
861 | GPSData.Speed_North = UbxVelNed.VEL_N; |
| 859 | GPSData.Speed_Top = -UbxVelNed.VEL_D; |
862 | GPSData.Speed_Top = -UbxVelNed.VEL_D; |
| 860 | GPSData.Speed_Ground = UbxVelNed.GSpeed; |
863 | GPSData.Speed_Ground = UbxVelNed.GSpeed; |
| 861 | GPSData.Heading = UbxVelNed.Heading; |
864 | GPSData.Heading = UbxVelNed.Heading; |
| 862 | SystemTime.Year = 0; |
865 | SystemTime.Year = 0; |
| 863 | SystemTime.Month = 0; |
866 | SystemTime.Month = 0; |
| Line 864... | Line 867... | ||
| 864 | SystemTime.Day = 0; |
867 | SystemTime.Day = 0; |
| 865 | SystemTime.Hour = 0; |
868 | SystemTime.Hour = 0; |
| 866 | SystemTime.Min = 0; |
869 | SystemTime.Min = 0; |
| 867 | SystemTime.Sec = 0; |
870 | SystemTime.Sec = 0; |
| 868 | SystemTime.mSec = 0; |
871 | SystemTime.mSec = 0; |
| Line 869... | Line 872... | ||
| 869 | SystemTime.Valid = 0; |
872 | SystemTime.Valid = 0; |
| 870 | |
873 | |
| 871 | FromFlightCtrl.GyroHeading / 10;//NaviData.HomePositionDeviation.Bearing / 2; |
874 | FromFlightCtrl.GyroHeading / 10;//NaviData.HomePositionDeviation.Bearing / 2; |
| 872 | if(GPSData.Position.Latitude < 0) ToFlightCtrl.Param.Byte[5] = 1; // 1 = S |
875 | if(GPSData.Position.Latitude < 0) ToFlightCtrl.Param.Byte[5] = 1; // 1 = S |
| 873 | else ToFlightCtrl.Param.Byte[5] = 0; // 1 = S |
876 | else ToFlightCtrl.Param.Byte[5] = 0; // 1 = S |
| 874 | i1 = abs(GPSData.Position.Latitude)/10000000L; |
877 | i1 = abs(GPSData.Position.Latitude)/10000000L; |
| 875 | i2 = abs(GPSData.Position.Latitude)%10000000L; |
878 | i2 = abs(GPSData.Position.Latitude)%10000000L; |
| 876 | 879 | ||
| 877 | 880 | ||
| 878 | 881 | ||
| 879 | if(!(NCFlags & NC_FLAG_GPS_OK)) {i1 = 0; i2 = 0;} |
882 | if(!(NCFlags & NC_FLAG_GPS_OK)) {i1 = 0; i2 = 0;} |
| 880 | i1 *= 100; |
883 | i1 *= 100; |
| 881 | i1 += i2 / 100000; |
884 | i1 += i2 / 100000; |
| 882 | i2 = i2 % 100000; |
885 | i2 = i2 % 100000; |
| 883 | i2 /= 10; |
886 | i2 /= 10; |
| 884 | ToFlightCtrl.Param.Byte[6] = i1 % 256; |
887 | ToFlightCtrl.Param.Byte[6] = i1 % 256; |
| 885 | ToFlightCtrl.Param.Byte[7] = i1 / 256; |
888 | ToFlightCtrl.Param.Byte[7] = i1 / 256; |
| 886 | ToFlightCtrl.Param.Byte[8] = i2 % 256; |
889 | ToFlightCtrl.Param.Byte[8] = i2 % 256; |
| 887 | ToFlightCtrl.Param.Byte[9] = i2 / 256; |
890 | ToFlightCtrl.Param.Byte[9] = i2 / 256; |
| Line 888... | Line 891... | ||
| 888 | break; |
891 | break; |
| 889 | case 1: |
892 | case 1: |