Rev 456 | Rev 461 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 456 | Rev 460 | ||
---|---|---|---|
Line 189... | Line 189... | ||
189 | u32 UART1_Data3D_Interval = 0; // in ms |
189 | u32 UART1_Data3D_Interval = 0; // in ms |
190 | u32 UART1_MotorData_Timer = 0; |
190 | u32 UART1_MotorData_Timer = 0; |
191 | u32 UART1_MotorData_Interval = 0; // in ms |
191 | u32 UART1_MotorData_Interval = 0; // in ms |
192 | u32 UART1_Display_Timer = 0; |
192 | u32 UART1_Display_Timer = 0; |
193 | u32 UART1_Display_Interval = 0; // in ms |
193 | u32 UART1_Display_Interval = 0; // in ms |
- | 194 | u32 NMEA_Timer = 0; |
|
- | 195 | u32 NMEA_Interval = 500;// in ms |
|
Line 194... | Line 196... | ||
194 | 196 | ||
195 | /********************************************************/ |
197 | /********************************************************/ |
196 | /* Initialization the UART1 */ |
198 | /* Initialization the UART1 */ |
197 | /********************************************************/ |
199 | /********************************************************/ |
Line 260... | Line 262... | ||
260 | VIC_ITCmd(UART1_ITLine, ENABLE); |
262 | VIC_ITCmd(UART1_ITLine, ENABLE); |
Line 261... | Line 263... | ||
261 | 263 | ||
262 | // initialize the debug timer |
264 | // initialize the debug timer |
263 | UART1_DebugData_Timer = SetDelay(UART1_DebugData_Interval); |
265 | UART1_DebugData_Timer = SetDelay(UART1_DebugData_Interval); |
- | 266 | UART1_NaviData_Timer = SetDelay(UART1_NaviData_Interval)+500; |
|
Line 264... | Line 267... | ||
264 | UART1_NaviData_Timer = SetDelay(UART1_NaviData_Interval)+500; |
267 | NMEA_Timer = SetDelay(9000); |
265 | 268 | ||
266 | // Fill Version Info Structure |
269 | // Fill Version Info Structure |
267 | UART_VersionInfo.SWMajor = VERSION_MAJOR; |
270 | UART_VersionInfo.SWMajor = VERSION_MAJOR; |
Line 672... | Line 675... | ||
672 | } |
675 | } |
673 | } |
676 | } |
674 | } |
677 | } |
675 | } |
678 | } |
Line -... | Line 679... | ||
- | 679 | ||
- | 680 | //$GPGGA,HHMMSS.ss,BBBB.BBBB,b,LLLLL.LLLL,l,Q,NN,D.D,H.H,h,G.G,g,A.A,RRRR*PP |
|
- | 681 | //$GPGGA,090527.40,5317.15615,N,00729.08295,E,1,04,2.26,-2.6,M,45.5,M,,* |
|
- | 682 | //$GPGGA,191410,4735.5634,N,00739.3538,E,1,04,4.4,351.5,M,48.0,M,,*45 |
|
- | 683 | //$GPGGA,092120.20,,,,,0,00,99.99,,,,,,*6C |
|
- | 684 | void CreateNmeaGGA(void) |
|
- | 685 | { |
|
- | 686 | unsigned char array[100], i = 0, crc = 0, x; |
|
- | 687 | long tmp1, tmp2; |
|
- | 688 | i += sprintf(array, "$GPGGA,"); |
|
- | 689 | // +++++++++++++++++++++++++++++++++++++++++++ |
|
- | 690 | if(SystemTime.Valid) |
|
- | 691 | { |
|
- | 692 | i += sprintf(&array[i], "%02d%02d%02d.%02d,",SystemTime.Hour,SystemTime.Min,SystemTime.Sec,SystemTime.mSec/10); |
|
- | 693 | } |
|
- | 694 | else |
|
- | 695 | { |
|
- | 696 | i += sprintf(&array[i], ","); |
|
- | 697 | } |
|
- | 698 | // +++++++++++++++++++++++++++++++++++++++++++ |
|
- | 699 | if(GPSData.Flags & FLAG_GPSFIXOK) |
|
- | 700 | { |
|
- | 701 | tmp1 = abs(GPSData.Position.Latitude)/10000000L; |
|
- | 702 | i += sprintf(&array[i],"%02d",tmp1); |
|
- | 703 | ||
- | 704 | tmp1 = abs(GPSData.Position.Latitude)%10000000L; |
|
- | 705 | tmp1 *= 6; // in Minuten |
|
- | 706 | tmp2 = tmp1 / 1000000L; |
|
- | 707 | i += sprintf(&array[i],"%02d",tmp2); |
|
- | 708 | tmp2 = tmp1 % 1000000L; |
|
- | 709 | tmp2 /= 10; // eine Stelle zu viel |
|
- | 710 | i += sprintf(&array[i],".%05d,",tmp2); |
|
- | 711 | ||
- | 712 | if(GPSData.Position.Latitude >= 0) i += sprintf(&array[i],"N,"); |
|
- | 713 | else i += sprintf(&array[i],"S,"); |
|
- | 714 | // +++++++++++++++++++++++++++++++++++++++++++ |
|
- | 715 | ||
- | 716 | tmp1 = abs(GPSData.Position.Longitude)/10000000L; |
|
- | 717 | i += sprintf(&array[i],"%03d",tmp1); |
|
- | 718 | ||
- | 719 | tmp1 = abs(GPSData.Position.Longitude)%10000000L; |
|
- | 720 | tmp1 *= 6; // in Minuten |
|
- | 721 | tmp2 = tmp1 / 1000000L; |
|
- | 722 | i += sprintf(&array[i],"%02d",tmp2); |
|
- | 723 | tmp2 = tmp1 % 1000000L; |
|
- | 724 | tmp2 /= 10; // eine Stelle zu viel |
|
- | 725 | i += sprintf(&array[i],".%05d,",tmp2); |
|
- | 726 | ||
- | 727 | ||
- | 728 | if(GPSData.Position.Longitude >= 0) i += sprintf(&array[i],"E,"); |
|
- | 729 | else i += sprintf(&array[i],"W,"); |
|
- | 730 | i += sprintf(&array[i],"%d,",GPSData.SatFix); |
|
- | 731 | i += sprintf(&array[i],"%d,",GPSData.NumOfSats); |
|
- | 732 | i += sprintf(&array[i],"%d.%d,",GPSData.Position_Accuracy/100,abs(GPSData.Position_Accuracy%100)); |
|
- | 733 | // i += sprintf(&array[i],"%d.%d,M,",GPSData.Position.Altitude/1000,abs(GPSData.Position.Altitude%1000)/100); |
|
- | 734 | tmp1 = NaviData.Altimeter / 2; // in dm |
|
- | 735 | i += sprintf(&array[i],"%d.%d,M,",tmp1 / 10,abs(tmp1 % 10)); |
|
- | 736 | i += sprintf(&array[i],",,,*"); |
|
- | 737 | } |
|
- | 738 | else |
|
- | 739 | { |
|
- | 740 | i += sprintf(&array[i], ",,,,%d,00,99.99,,,,,,*",GPSData.NumOfSats); |
|
- | 741 | } |
|
- | 742 | for(x=1; x<i-1; x++) |
|
- | 743 | { |
|
- | 744 | crc ^= array[x]; |
|
- | 745 | } |
|
- | 746 | i += sprintf(&array[i], "%02x\n\r",crc); |
|
- | 747 | AddSerialData(&UART1_tx_buffer,array,i); |
|
- | 748 | ||
- | 749 | // +++++++++++++++++++++++++++++++++++++++++++ |
|
- | 750 | /* |
|
- | 751 | |
|
- | 752 | |
|
- | 753 | ||
- | 754 | GPSData.Flags = (GPSData.Flags & 0xf0) | (UbxSol.Flags & 0x0f); // we take only the lower bits |
|
- | 755 | GPSData.NumOfSats = UbxSol.numSV; |
|
- | 756 | GPSData.SatFix = UbxSol.GPSfix; |
|
- | 757 | GPSData.Position_Accuracy = UbxSol.PAcc; |
|
- | 758 | GPSData.Speed_Accuracy = UbxSol.SAcc; |
|
- | 759 | SetGPSTime(&SystemTime); // update system time |
|
- | 760 | // NAV POSLLH |
|
- | 761 | GPSData.Position.Status = INVALID; |
|
- | 762 | GPSData.Position.Longitude = UbxPosLlh.LON; |
|
- | 763 | GPSData.Position.Latitude = UbxPosLlh.LAT; |
|
- | 764 | GPSData.Position.Altitude = UbxPosLlh.HMSL; |
|
- | 765 | GPSData.Position.Status = NEWDATA; |
|
- | 766 | // NAV VELNED |
|
- | 767 | GPSData.Speed_East = UbxVelNed.VEL_E; |
|
- | 768 | GPSData.Speed_North = UbxVelNed.VEL_N; |
|
- | 769 | GPSData.Speed_Top = -UbxVelNed.VEL_D; |
|
- | 770 | GPSData.Speed_Ground = UbxVelNed.GSpeed; |
|
- | 771 | GPSData.Heading = UbxVelNed.Heading; |
|
- | 772 | SystemTime.Year = 0; |
|
- | 773 | SystemTime.Month = 0; |
|
- | 774 | SystemTime.Day = 0; |
|
- | 775 | SystemTime.Hour = 0; |
|
- | 776 | SystemTime.Min = 0; |
|
- | 777 | SystemTime.Sec = 0; |
|
- | 778 | SystemTime.mSec = 0; |
|
- | 779 | SystemTime.Valid = 0; |
|
- | 780 | |
|
- | 781 | FromFlightCtrl.GyroHeading / 10;//NaviData.HomePositionDeviation.Bearing / 2; |
|
- | 782 | if(GPSData.Position.Latitude < 0) ToFlightCtrl.Param.Byte[5] = 1; // 1 = S |
|
- | 783 | else ToFlightCtrl.Param.Byte[5] = 0; // 1 = S |
|
- | 784 | i1 = abs(GPSData.Position.Latitude)/10000000L; |
|
- | 785 | i2 = abs(GPSData.Position.Latitude)%10000000L; |
|
- | 786 | ||
- | 787 | ||
- | 788 | ||
- | 789 | if(!(NCFlags & NC_FLAG_GPS_OK)) {i1 = 0; i2 = 0;} |
|
- | 790 | i1 *= 100; |
|
- | 791 | i1 += i2 / 100000; |
|
- | 792 | i2 = i2 % 100000; |
|
- | 793 | i2 /= 10; |
|
- | 794 | ToFlightCtrl.Param.Byte[6] = i1 % 256; |
|
- | 795 | ToFlightCtrl.Param.Byte[7] = i1 / 256; |
|
- | 796 | ToFlightCtrl.Param.Byte[8] = i2 % 256; |
|
- | 797 | ToFlightCtrl.Param.Byte[9] = i2 / 256; |
|
- | 798 | break; |
|
- | 799 | case 1: |
|
- | 800 | ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID; |
|
- | 801 | ToFlightCtrl.Param.Byte[0] = 11+3; // index +3, weil bei HoTT V4 3 Bytes eingeschoben wurden |
|
- | 802 | ToFlightCtrl.Param.Byte[1] = 8-1; // how many |
|
- | 803 | //----------------------------- |
|
- | 804 | if(GPSData.Position.Longitude < 0) ToFlightCtrl.Param.Byte[2] = 1; // 1 = E |
|
- | 805 | else ToFlightCtrl.Param.Byte[2] = 0; // 1 = S |
|
- | 806 | i1 = abs(GPSData.Position.Longitude)/10000000L; |
|
- | 807 | i2 = abs(GPSData.Position.Longitude)%10000000L; |
|
- | 808 | ||
- | 809 | */ |
|
- | 810 | } |
|
Line 676... | Line 811... | ||
676 | 811 | ||
677 | 812 | ||
678 | /**************************************************************/ |
813 | /**************************************************************/ |
679 | /* Send the answers to incomming commands at the debug uart */ |
814 | /* Send the answers to incomming commands at the debug uart */ |
Line 788... | Line 923... | ||
788 | 923 | ||
789 | MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'K', NC_ADDRESS, 2, &motorindex1, sizeof(motorindex1),(u8 *)&Motor[motorindex1], sizeof(Motor_t)); |
924 | MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'K', NC_ADDRESS, 2, &motorindex1, sizeof(motorindex1),(u8 *)&Motor[motorindex1], sizeof(Motor_t)); |
790 | UART1_MotorData_Timer = SetDelay(UART1_MotorData_Interval); |
925 | UART1_MotorData_Timer = SetDelay(UART1_MotorData_Interval); |
791 | UART1_Request_MotorData = FALSE; |
926 | UART1_Request_MotorData = FALSE; |
- | 927 | } |
|
- | 928 | else if((((NMEA_Interval > 0) && CheckDelay(NMEA_Timer))) && (UART1_tx_buffer.Locked == FALSE)) |
|
- | 929 | { |
|
- | 930 | CreateNmeaGGA(); |
|
- | 931 | NMEA_Timer = SetDelay(NMEA_Interval); |
|
- | 932 | } |
|
792 | } |
933 | |
793 | /* |
934 | /* |
794 | else if(UART1_ConfirmFrame && (UART1_tx_buffer.Locked == FALSE)) |
935 | else if(UART1_ConfirmFrame && (UART1_tx_buffer.Locked == FALSE)) |
795 | { |
936 | { |
796 | MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'B', NC_ADDRESS, 1, &UART1_ConfirmFrame, sizeof(UART1_ConfirmFrame)); |
937 | MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'B', NC_ADDRESS, 1, &UART1_ConfirmFrame, sizeof(UART1_ConfirmFrame)); |