Subversion Repositories NaviCtrl

u32 NMEA_Timer = 0;

u32 NMEA_Interval = 500;// in ms

        UART1_NaviData_Timer = SetDelay(UART1_NaviData_Interval)+500;

//$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;

*/

}

        }

        else if((((NMEA_Interval > 0) && CheckDelay(NMEA_Timer))) && (UART1_tx_buffer.Locked == FALSE))

        {

                CreateNmeaGGA();

                NMEA_Timer = SetDelay(NMEA_Interval);

        }