Section 4: Flight-Control-Functions.

............. Process GPS Data Source Code ..........

========= File-Name: 14_Process_GPS_Data.ino ===========


//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++ Start to Process GPS Data +++++++++++++++++++
//                 Last Update: 26-08-2022
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

void ProcessGPS(char* GPS_Data_Buffer, int NumChars)
  {  
   int i, j, h, k ,h_i; 
   char GPS_Char, Buff_Char;
   int index;
   int Dollar_Pos[10] = {0}; 
// This scentence key is looked for
   char GPS_Sentence[7] = "$GPRMC";  // 7 characters incl.terminating null '\0'
   int discriminant = 0;
   int sentence_char;
   char str[20];
   char GPRMC_Record[MaxChars] = {'\0'};
   int comma_pos[8] = {7,17,19,30,32,44,46,73};

//   Serial.println("Printing the GPS Data Buffer and $ positions");
 
   index=0;   
   for (h=0; h <= (NumChars-1); h++) 
      {
        GPS_Char = GPS_Data_Buffer[h];
//        Serial.print(GPS_Char);
        if (GPS_Char == '$')
          {
            Dollar_Pos[index] = h;
//             Serial.print(" ");
//             Serial.print(h);
//              Serial.print(" ");
            index++;
          }
        }
      
//  Name the last-1 GPS charachter as $ sign         
    Dollar_Pos[index] = NumChars-1;  
               
//    Serial.println();
    
//    Serial.print("Dollar Positions: "); 
//  Printing the list of $ seperators including the last one
//    for (h =0 ; h <= index; h++)  
//      {
//       Serial.print(Dollar_Pos[h]);
//       Serial.print(" , ");
//      }
//       Serial.println();

//     Serial.println("Printing the GPS Data Buffer based on $ separation.");
// Note: index is number of found $ signs
     for (h =0 ; h <= index-1; h++)  
       {
// Note: -2 because of CR LF 
       for (k = Dollar_Pos[h]; k <= Dollar_Pos[h+1]-2; k++)
         {
// print all received GPS data Sentences
//          Serial.print(GPS_Data_Buffer[k]);                 
         }
//        Serial.println();         
       }
      
// Note: last index is an artificial $ sign

// mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm
// !!!!!!!!!!!!!!!! Begin of "h-loop" !!!!!!!!!!!!!!!!!!!
// Note: index-1 is number of $ signs minus last one.
      for (h =0 ; h <= index-1; h++)   
       {                                 //
//        Serial.print(h);

//     Find the GPS Sentences-key "$GPRMC" only   
        sentence_char = 0;
        discriminant=0;
        for (k = Dollar_Pos[h]; k <= Dollar_Pos[h]+5; k++)   
          {
//             Serial.print("14: GPS_Data_Buffer[k]=>");
//             Serial.print(GPS_Data_Buffer[k]);
//             Serial.print("   14: GPS_Sentence[sentence_char]=>");
//             Serial.println(GPS_Sentence[sentence_char]);
 
             Buff_Char = GPS_Data_Buffer[k];  
             if (Buff_Char == GPS_Sentence[sentence_char])    
              { 
                discriminant++;
              } 
              sentence_char++;
           }
         
       if (discriminant==6)         // a $GPRMC key found 
         {   
//           Serial.println("Found a $GPRMC key, now processing the sentence data");
//           Serial.print("Dollar_Pos[h]= ");
//           Serial.print(Dollar_Pos[h]);
//           Serial.print("  ");
//           Serial.print(Dollar_Pos[h+1]);
//           Serial.println();

//           Serial.println("14 Found a $GPRMC key: "); 
             h_i=0;    
             for (i=Dollar_Pos[h]; i <= Dollar_Pos[h+1]-1; i++)
               { 
                 GPRMC_Record[h_i] = GPS_Data_Buffer[i];
//               Serial.print(GPRMC_Record[h_i]);
                  h_i++;
                } 
//               Serial.println();
            }


// Serial.println("--- Comma positions in Sentence are known---");      
      if ((discriminant == 6) && (h_i > 42))          
       {         // Begin if discriminant1 == 6  

//      Serial.print("comma-position= ");
//       for (i=0; i<=7; i++)
//         {
//           Serial.print(comma_pos[i]);
//           Serial.print(" ,");
//         }
//         Serial.println();
                          
//              i=0;
//              Serial.print("Time in UTC (HhMmSs): ");
//              for (j=comma_pos[i]; j<(comma_pos[i+1]-1); j++)
//                 {
//                   Serial.print(GPRMC_Record[j+1]); 
//                 }
//                  Serial.println("");
                   
//               i=1;
//               Serial.print("Status (A=OK,V=KO): ");
//               for (int j=comma_pos[i];j<(comma_pos[i+1]-1);j++)
//                 {
//                   Serial.print(GPRMC_Record[j+1]);
//                 }
//                  Serial.println("");
//                  Serial.println("");

               i=2;
               k=0;
//               Serial.print("N_Latitude: ");
               for (j=comma_pos[i]; j<=comma_pos[i+1]-2; j++)
                  {
//                   Serial.print(GPRMC_Record[j]); 
                   str[k]=GPRMC_Record[j];
                   GPS_val_Lat = atof(str);    //convert ASCII-string to float (double)
                   k++;
                 }
//                Serial.println();    
                convertLonLat(str);
                GPS_val_Lat = atof(str);     //convert ASCII-string to float (double)
//                Serial.print("GPS_val_Lat= ");
//                Serial.println(GPS_val_Lat,6); 

               i=4;
               k=0;
//               Serial.print("E-Longitude: ");
               for (j=comma_pos[i];j<(comma_pos[i+1]-1);j++)
                  {
//                   Serial.print(GPRMC_Record[j]); 
                   str[k]=GPRMC_Record[j+1];
                   GPS_val_Lon = atof(str);   //convert ASCII-string to float (double)
                   k++;
                  }
//                 Serial.println();
                 convertLonLat(str);
// convert ASCII-string to float (double)
                GPS_val_Lon = atof(str);  
                
// !!!!!!Note following println causes the piek in motorplot!!!!!!!     
//               Serial.print("GPS_val_Lon= ");
//               Serial.println(GPS_val_Lon,6);

               discriminant = 0;
        }    // end if discriminant1 == 6

      }  // hhhhhhhhhhhhhhhh End of "h-loop" hhhhhhhhhhhhhhhh 
// mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm
 }
 
//============================================================
//============================================================

// ===== Begin of function to convert GPS Latitud Reading =====
// Convert the format of a latitude from "ddmm.mmmm" to "dd.dddd".

void convertLonLat(char* s)
  {
    // Convert ddmm.mmmm to dd.mmmmm
    s[4] = s[3];  // move minutes
    s[3] = s[2];  // move tens of minutes  
    s[2] = '.';   // put decimal point
    s[8] = '\0';  // drop last digit

    // Compute fractional degrees.
    uint16_t fraction = atoi(s + 3);  // unit = 1e-3 arcmin
    fraction /= 6;                    // unit = 1e-4 deg

    // Write in ASCII, right to left.
    s[7] = '\0';
    for (int i = 6; i > 2; i--)
    {
        s[i] = fraction % 10 + '0';
        fraction /= 10;
    }
  }
// ===== End of function to convert GPS Latitud Reading ======

// ===========================================================
// ******* End of functions that process the GPS-Data ********
// ******* End of functions that process the GPS-DataE *******
// ******* End of functions that process the GPS-Data ********
// ===========================================================

Free-Drones Company 2022