Distanz und Peilung zwischen 2 Positionen berechnen

Die Entfernung und die Peilung sollen von Position 1 (Länge / Breite) zu Position 2 (Länge / Breite) berechnet werden. Dabei werden beide Positionen in Grad mit Kommastellen eingegeben. Die Software soll auf dem Arduino laufen.

Die Distanz wird in m und sm (Seemeilen) ausgegeben, die Peilung in Grad 0….359 Grad.

Zum Testen wird die Position 2 um 3 Grad nach Norden verlegt. Das sind 3 x 60 sm also 180 sm und die Peilung ist Nord also 0 Grad.

Das Programm

// Entfernung und Peilung zwischen zwei Positionen berechnen
// beide Längen und Breiten werden in Grad eingegeben
// Vorlage von http://forum.arduino.cc/index.php?topic=45760.0
//
// Matthias Busse Version 1.0 vom 25.11.2014

double breite1=54.0; // Position 1
double laenge1=10.0;
double breite2=57.0; // Position 2
double laenge2=10.0; // 3 Grad nördlich sind Distanz 180 sm, Peilung 0 Grad
double distanz, peilung;

void setup() {
  Serial.begin(4800);
}

void loop() {
  Serial.print("Distanz (M): ");
  distanz = CalcDistanz(breite1,laenge1,breite2,laenge2)/1852.216; // 1852 m = 1 Seemeile
  Serial.println(distanz,2);
  Serial.print("Peilung (Grad): ");
  peilung = CalcPeilung(breite1,laenge1,breite2,laenge2);
  Serial.println(peilung,2);
  delay(5000);
}

double CalcDistanz(double breite1, double laenge1, double breite2, double laenge2) {
// Berechnet die Distance vom breite1/laenge1 zu breite2/laenge2 in Grad
// Rückgabe Distance in m
// Matthias Busse 25.11.2014 Version 1.0
double dlaenge, dbreite, a, c;
double dist = 0.0;

  dlaenge = gtor(laenge2 - laenge1);
  dbreite = gtor(breite2 - breite1);
  a = pow(sin(dbreite/2),2) + cos(gtor(breite1)) * cos(gtor(breite2)) * pow(sin(dlaenge/2),2);
  c = 2.0 * atan2(sqrt(a), sqrt(1-a));
  dist = 6371001 * c;  //Radius der Erde (6378140 m am Äquator / 6371001 m im Mittel)
  return dist;
}

double CalcPeilung(double breite1, double laenge1, double breite2, double laenge2) {
// Berechnet die Peilung von breite1/laenge1 zu breite2/laenge2 in Grad
// Ausgabe Peilung in Grad
// Matthias Busse 25.11.2014 Version 1.0
breite1 = gtor(breite1);
laenge1 = gtor(laenge1);
breite2 = gtor(breite2);
laenge2 = gtor(laenge2);  // Winkel berechnen
  double peil = atan2(sin(laenge2-laenge1)*cos(breite2), (cos(breite1)*sin(breite2))-(sin(breite1)*cos(breite2)*cos(laenge2-laenge1)));
  peil = rtog(peil); // in Grad umrechnen
  peil = fmod((peil + 360.0), 360); // mod macht -90 zu 270
  return peil;
}

double gtor(double fgrad){ // Grad in Rad umwandeln
  return(fgrad * PI / 180);
}

double rtog(double frad){ // Rad in Grad umwandeln
  return(frad * 180.0 / PI);
}

von Matthias Busse

Ein Gedanke zu „Distanz und Peilung zwischen 2 Positionen berechnen

  1. Pingback: NMEA2000 Ankeralarm mit dem Arduino aufgebaut | Shelvin – Elektronik ausprobiert und erläutert

Schreibe einen Kommentar

Deine E-Mail-Adresse wird nicht veröffentlicht. Erforderliche Felder sind mit * markiert.