Mudelismifoorum

Code: Select all

void GPS_Arvutus()    // hakkame arvutama
{
  if (gps_OK[0] == 'A'){
      // konverdime GPSi andmed Ddmm.mmmm -> dd.dddd
			// latitude  gps_LAT
      gps_LAT[9]=0x30;  // taastame
  		afterDecimal[0] = gps_LAT[2];
			afterDecimal[1] = gps_LAT[3];
			afterDecimal[2] = gps_LAT[4];
			afterDecimal[3] = gps_LAT[5];
			afterDecimal[4] = gps_LAT[6];
			afterDecimal[5] = gps_LAT[7];
			afterDecimal[6] = gps_LAT[8];

   currentLat = (float)(atol(gps_LAT)/100) + atof(afterDecimal)/60;

       // konverdime GPSi andmed 0Ddmm.mmmm -> dd.dddd
			// longtidude gps_LONG
      gps_LONG[10]=0x30;  // taastame
			afterDecimal[0] = gps_LONG[2];
			afterDecimal[1] = gps_LONG[3];
			afterDecimal[2] = gps_LONG[4];
			afterDecimal[3] = gps_LONG[5];
			afterDecimal[4] = gps_LONG[6];
			afterDecimal[5] = gps_LONG[7];
			afterDecimal[6] = gps_LONG[8];

   currentLong = (float)(atol(gps_LONG)/100) + atof(afterDecimal)/60;

// hakkame arvutama suunda "bearing"
	LatTo_m	= 111377.60;
  LongTo_m = 59132.84;

   Lat = 0.0;
   Long = 0.0;
   angle = 0.0;
   Lat_m = 0.0;
   Long_m = 0.0;
	// current waypoint
//	currentWayptLat = 58.34663;
//  currentWayptLong = 25.61145;

    Lat = currentWayptLat - currentLat;
    Long = currentWayptLong - currentLong;

// Siin kontrollime kas on vaja vahetada sihtpunkti 1852*0,0003=0.54m
// 1 kaareminut ~1 meremiil (1852m)
// 0.0000040  +-0.0003   0.5m
// 0.000015   +-0.0006   1.1m
// Lat  0.000030   +-0.0017   3.1m
// Long 0.000060   +-0.0035   6.4m
// --------------------------------
// Lat  0.000070   +-0.0042   7.7m
// Long 0.000140   +-0.0080   14m
// --------------------------------
// Lat  0.000090   +-0.0053  9.8m
// Long 0.000180   +-0.0100 18.5m
   if((Lat<0.000070 && Lat>-0.000070) && (Long<0.000140 && Long>-0.000140)) {
         SDrida++;
         EEprom_Write(0, SDrida);   // salvestame mällu uue sihtkoha punkti aadressi
      if (SDrida == 99) SDflag = 1; // edaspidi enam kaardilt ei loe
       SihtpunktFailist();          // loeme uue sihtpunkti failist
    Lat = currentWayptLat - currentLat;
    Long = currentWayptLong - currentLong;
    Lcd_Out(1, 1, "Sp");          // Print text at LCD
    }
        Lat_m = Lat * LatTo_m;
    Long_m = Long * LongTo_m;

  rad = atan2(long_m, lat_m);
  angle = 180 * rad / 3.14;

  if ((Lat > 0) && (Long < 0))  // kas on neljandas veerandis?
   angle = 360 + angle;
   else
  if ((Lat < 0) && (Long < 0)) // kas on kolmandas veerandis?
   angle = 180 + angle + 180;
   else;
      FloatToStr(angle, bearing);

  bearing[5]=0;                            // märgime teksti lõpu muidu LCD kirjutab jura
  Lcd_Out(2, 16, bearing);         // Print text at LCD
  }
  else
  Lcd_Out(2, 16, "---.-");         // ei lubatud arvutada...
}