191,6 → 191,8 |
u32 UART1_MotorData_Interval = 0; // in ms |
u32 UART1_Display_Timer = 0; |
u32 UART1_Display_Interval = 0; // in ms |
u32 NMEA_Timer = 0; |
u32 NMEA_Interval = 500;// in ms |
|
/********************************************************/ |
/* Initialization the UART1 */ |
262,6 → 264,7 |
// initialize the debug timer |
UART1_DebugData_Timer = SetDelay(UART1_DebugData_Interval); |
UART1_NaviData_Timer = SetDelay(UART1_NaviData_Interval)+500; |
NMEA_Timer = SetDelay(9000); |
|
// Fill Version Info Structure |
UART_VersionInfo.SWMajor = VERSION_MAJOR; |
674,7 → 677,139 |
} |
} |
|
//$GPGGA,HHMMSS.ss,BBBB.BBBB,b,LLLLL.LLLL,l,Q,NN,D.D,H.H,h,G.G,g,A.A,RRRR*PP |
//$GPGGA,090527.40,5317.15615,N,00729.08295,E,1,04,2.26,-2.6,M,45.5,M,,* |
//$GPGGA,191410,4735.5634,N,00739.3538,E,1,04,4.4,351.5,M,48.0,M,,*45 |
//$GPGGA,092120.20,,,,,0,00,99.99,,,,,,*6C |
void CreateNmeaGGA(void) |
{ |
unsigned char array[100], i = 0, crc = 0, x; |
long tmp1, tmp2; |
i += sprintf(array, "$GPGGA,"); |
// +++++++++++++++++++++++++++++++++++++++++++ |
if(SystemTime.Valid) |
{ |
i += sprintf(&array[i], "%02d%02d%02d.%02d,",SystemTime.Hour,SystemTime.Min,SystemTime.Sec,SystemTime.mSec/10); |
} |
else |
{ |
i += sprintf(&array[i], ","); |
} |
// +++++++++++++++++++++++++++++++++++++++++++ |
if(GPSData.Flags & FLAG_GPSFIXOK) |
{ |
tmp1 = abs(GPSData.Position.Latitude)/10000000L; |
i += sprintf(&array[i],"%02d",tmp1); |
|
tmp1 = abs(GPSData.Position.Latitude)%10000000L; |
tmp1 *= 6; // in Minuten |
tmp2 = tmp1 / 1000000L; |
i += sprintf(&array[i],"%02d",tmp2); |
tmp2 = tmp1 % 1000000L; |
tmp2 /= 10; // eine Stelle zu viel |
i += sprintf(&array[i],".%05d,",tmp2); |
|
if(GPSData.Position.Latitude >= 0) i += sprintf(&array[i],"N,"); |
else i += sprintf(&array[i],"S,"); |
// +++++++++++++++++++++++++++++++++++++++++++ |
|
tmp1 = abs(GPSData.Position.Longitude)/10000000L; |
i += sprintf(&array[i],"%03d",tmp1); |
|
tmp1 = abs(GPSData.Position.Longitude)%10000000L; |
tmp1 *= 6; // in Minuten |
tmp2 = tmp1 / 1000000L; |
i += sprintf(&array[i],"%02d",tmp2); |
tmp2 = tmp1 % 1000000L; |
tmp2 /= 10; // eine Stelle zu viel |
i += sprintf(&array[i],".%05d,",tmp2); |
|
|
if(GPSData.Position.Longitude >= 0) i += sprintf(&array[i],"E,"); |
else i += sprintf(&array[i],"W,"); |
i += sprintf(&array[i],"%d,",GPSData.SatFix); |
i += sprintf(&array[i],"%d,",GPSData.NumOfSats); |
i += sprintf(&array[i],"%d.%d,",GPSData.Position_Accuracy/100,abs(GPSData.Position_Accuracy%100)); |
// i += sprintf(&array[i],"%d.%d,M,",GPSData.Position.Altitude/1000,abs(GPSData.Position.Altitude%1000)/100); |
tmp1 = NaviData.Altimeter / 2; // in dm |
i += sprintf(&array[i],"%d.%d,M,",tmp1 / 10,abs(tmp1 % 10)); |
i += sprintf(&array[i],",,,*"); |
} |
else |
{ |
i += sprintf(&array[i], ",,,,%d,00,99.99,,,,,,*",GPSData.NumOfSats); |
} |
for(x=1; x<i-1; x++) |
{ |
crc ^= array[x]; |
} |
i += sprintf(&array[i], "%02x\n\r",crc); |
AddSerialData(&UART1_tx_buffer,array,i); |
|
// +++++++++++++++++++++++++++++++++++++++++++ |
/* |
|
|
|
GPSData.Flags = (GPSData.Flags & 0xf0) | (UbxSol.Flags & 0x0f); // we take only the lower bits |
GPSData.NumOfSats = UbxSol.numSV; |
GPSData.SatFix = UbxSol.GPSfix; |
GPSData.Position_Accuracy = UbxSol.PAcc; |
GPSData.Speed_Accuracy = UbxSol.SAcc; |
SetGPSTime(&SystemTime); // update system time |
// NAV POSLLH |
GPSData.Position.Status = INVALID; |
GPSData.Position.Longitude = UbxPosLlh.LON; |
GPSData.Position.Latitude = UbxPosLlh.LAT; |
GPSData.Position.Altitude = UbxPosLlh.HMSL; |
GPSData.Position.Status = NEWDATA; |
// NAV VELNED |
GPSData.Speed_East = UbxVelNed.VEL_E; |
GPSData.Speed_North = UbxVelNed.VEL_N; |
GPSData.Speed_Top = -UbxVelNed.VEL_D; |
GPSData.Speed_Ground = UbxVelNed.GSpeed; |
GPSData.Heading = UbxVelNed.Heading; |
SystemTime.Year = 0; |
SystemTime.Month = 0; |
SystemTime.Day = 0; |
SystemTime.Hour = 0; |
SystemTime.Min = 0; |
SystemTime.Sec = 0; |
SystemTime.mSec = 0; |
SystemTime.Valid = 0; |
|
FromFlightCtrl.GyroHeading / 10;//NaviData.HomePositionDeviation.Bearing / 2; |
if(GPSData.Position.Latitude < 0) ToFlightCtrl.Param.Byte[5] = 1; // 1 = S |
else ToFlightCtrl.Param.Byte[5] = 0; // 1 = S |
i1 = abs(GPSData.Position.Latitude)/10000000L; |
i2 = abs(GPSData.Position.Latitude)%10000000L; |
|
|
|
if(!(NCFlags & NC_FLAG_GPS_OK)) {i1 = 0; i2 = 0;} |
i1 *= 100; |
i1 += i2 / 100000; |
i2 = i2 % 100000; |
i2 /= 10; |
ToFlightCtrl.Param.Byte[6] = i1 % 256; |
ToFlightCtrl.Param.Byte[7] = i1 / 256; |
ToFlightCtrl.Param.Byte[8] = i2 % 256; |
ToFlightCtrl.Param.Byte[9] = i2 / 256; |
break; |
case 1: |
ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID; |
ToFlightCtrl.Param.Byte[0] = 11+3; // index +3, weil bei HoTT V4 3 Bytes eingeschoben wurden |
ToFlightCtrl.Param.Byte[1] = 8-1; // how many |
//----------------------------- |
if(GPSData.Position.Longitude < 0) ToFlightCtrl.Param.Byte[2] = 1; // 1 = E |
else ToFlightCtrl.Param.Byte[2] = 0; // 1 = S |
i1 = abs(GPSData.Position.Longitude)/10000000L; |
i2 = abs(GPSData.Position.Longitude)%10000000L; |
|
*/ |
} |
|
|
/**************************************************************/ |
/* Send the answers to incomming commands at the debug uart */ |
/**************************************************************/ |
790,6 → 925,12 |
UART1_MotorData_Timer = SetDelay(UART1_MotorData_Interval); |
UART1_Request_MotorData = FALSE; |
} |
else if((((NMEA_Interval > 0) && CheckDelay(NMEA_Timer))) && (UART1_tx_buffer.Locked == FALSE)) |
{ |
CreateNmeaGGA(); |
NMEA_Timer = SetDelay(NMEA_Interval); |
} |
|
/* |
else if(UART1_ConfirmFrame && (UART1_tx_buffer.Locked == FALSE)) |
{ |