WebSVN - Projects - Diff - Rev 2200 and 2212 - /Transportables_Koptertool/PKT/branches/branch

 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //############################################################################
 //# HISTORY  gps.c
+//#
+//#
+//#
+//#
+//#
+//#
+//#
+//#
+//# 22.09.2015 Starter
+//# - followme_add_offset(...) und followme_calculate_offset getestet mit PKT
+//# - add my_abs(...)
 //#
-//# 20.09.2015 Startet
+//# 20.09.2015 Starter
 //# - add Routine um einen Offset in Meter zu den aktuellen Koordinaten dazurechnen
 //#    followme_calculate_offset(...)
 //#
 //# 03.08.2015 cebra
     int RetVal = 1;
     distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */
     azimuth = nmea_degree2radian(azimuth);
-    end_pos->lat = asin( sin(p1.lat) * cos(distance) + cos(p1.lat) * sin(distance) * cos(azimuth));
+    end_pos->latitude = asin( sin(p1.latitude) * cos(distance) + cos(p1.latitude) * sin(distance) * cos(azimuth));
-    end_pos->lon = p1.lon + atan2( sin(azimuth) * sin(distance) * cos(p1.lat), cos(distance) - sin(p1.lat) * sin(end_pos->lat));
+    end_pos->longitude = p1.longitude + atan2( sin(azimuth) * sin(distance) * cos(p1.latitude), cos(distance) - sin(p1.latitude) * sin(end_pos->latitude));
-    if(NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon))
+    if(NMEA_POSIX(isnan)(end_pos->latitude) || NMEA_POSIX(isnan)(end_pos->longitude))
+    {
-        end_pos->lat = 0; end_pos->lon = 0;
+        end_pos->latitude = 0; end_pos->longitude = 0;
+        RetVal = 0;
+    }
-        RetVal = 0;
+    return RetVal;
-    }
+}
     return RetVal;
-}
+// Fügt den Startpostition einen Offset hinzu und gibt es als Zielposition zurück
-// Berechnet die Position der Kopters für FollowMeStep2
+//
-// Momentan wird die gleich Position ausgegeben
 // Benutzt die c_cos_8192 der FC
 // TODO: move to followme.c
 // TODO: *8192 optimieren
-uint8_t followme_calculate_offset(
+uint8_t followme_add_offset(
-    const nmeaPOS *pPktPos,     /**< Start position in radians */
+    const nmeaPOS *pPktPos,                     /**< Start position in radians */
+    nmeaPOS *target_pos,                        /**< Result position in radians */
+        positionOffset *pFollowMeOffset         /**< Position Offset in Millimeters */
+    )
+{
+        nmeaPOS pktPos = *pPktPos;
+        positionOffset followMeOffset = * pFollowMeOffset;
+    target_pos->latitude = pktPos.latitude + ( ( followMeOffset.latitude * ( LAT_DIV / FOLLOWME_M2DEG ) ) ) / 1000;
+    target_pos->longitude = pktPos.longitude + ( ( followMeOffset.longitude * ( LONG_DIV / FOLLOWME_M2DEG ) * (8192/1000) ) / my_abs( c_cos_8192( (pktPos.latitude / LONG_DIV ) ) ) );
+        return 1;
+}
+// schlanke abs-Methode
+// TODO: move to mymath.h
+int16_t my_abs(int16_t input)
+{
+        if(input < 0)
+                return -input;
+        else
+                return input;
+}
+// Rechnet einen Offset aus Radius und Winkel nach Lat/Long
+// Benutzt die c_cos_8192 und c_sin_8192 der FC
+// TODO: move to followme.c
+uint8_t followme_calculate_offset(
-    nmeaPOS *target_pos,        /**< Result position in radians */
+                int32_t radius,                         // in mm
-    int d_lat,                  /**< Distance lat(m) */
+                int16_t angle,                          // in Grad °
     return PosDev;
-}
-///**
-// * \brief Calculate distance between two points
-// * \return Distance in meters
-// */
-//int32_t nmea_distance(
-//        const nmeaPOS *from_pos,    /**< From position in radians */
-//        const nmeaPOS *to_pos       /**< To position in radians */
-//        )
-//{
-//  int32_t dist = ((int32_t)NMEA_EARTHRADIUS_M) * acos(
-//        sin(to_pos->lat) * sin(from_pos->lat) +
-//        cos(to_pos->lat) * cos(from_pos->lat) * cos(to_pos->lon - from_pos->lon)
-//        );
-//    return dist;
-//}
-//// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet)
-//// zur aktuellen Position(nach Motorstart)
-//geo_t calc_geo(HomePos_t *home, GPS_Pos_t *pos)
-//{ double lat1, lon1, lat2, lon2, d1, dlat;
-//        geo_t geo;
-//
-//        lon1 = MK_pos.Home_Lon;
-//        lat1 = MK_pos.Home_Lat;
-//        lon2 = (double)pos->Longitude   / 10000000.0;
-//        lat2 = (double)pos->Latitude    / 10000000.0;
-//
-//        // Formel verwendet von http://www.kompf.de/gps/distcalc.html
-//        // 111.3 km = Abstand zweier Breitenkreise und/oder zweier Längenkreise am Äquator
-//        // es wird jedoch in Meter weiter gerechnet
-//        d1       = 111300 * (double)cos((double)(lat1 + lat2) / 2 * DEG_TO_RAD) * (lon1 - lon2);
-//        dlat = 111300 * (double)(lat1 - lat2);
-//        // returns a value in metres http://www.kompf.de/gps/distcalc.html
-//        geo.bearing = fmod((RAD_TO_DEG * (double)atan2(d1, dlat)) + 180, 360); // +180 besserer Vergleich mit MkCockpit
-//        if (geo.bearing > 360) geo.bearing -= 360; // bekam schon Werte über 400
-//        geo.distance = sqrt(d1 * d1 + dlat * dlat);
-//        return(geo);
-//}
-// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet)
-// zur aktuellen Position(nach Motorstart)
-//--------------------------------------------------------------
-//--------------------------------------------------------------
-/*
-geo_t calc_geo( HomePos_t *home, GPS_Pos_t *pos )
-{
-    int32_t lat1, lon1, lat2, lon2;
-        int32_t d1, dlat;
-        geo_t geo;
-        lon1 = home->Home_Lon;
-        lat1 = home->Home_Lat;
-        lon2 = pos->Longitude;
-        lat2 = pos->Latitude;
-        if( !CheckGPS )
-        {
-            writex_gpspos(  0, 3, home->Home_Lat , MNORMAL,  0,0);    // Anzeige: Breitengrad (Latitude)
-            writex_gpspos( 11, 3, home->Home_Lon , MNORMAL,  0,0);    // Anzeige: Laengengrad (Longitude)
-            writex_gpspos(  0, 4, pos->Latitude  , MNORMAL,  0,0);    // Anzeige: Breitengrad (Latitude)
-            writex_gpspos( 11, 4, pos->Longitude , MNORMAL,  0,0);    // Anzeige: Laengengrad (Longitude)
-            //lcd_puts_at (0, 3, my_itoa(home->Home_Lat, 10, 7, 7), 0);     // 30.05.2014 OG: my_itoa() gibt es nicht mehr
-            //lcd_puts_at (11, 3, my_itoa(home->Home_Lon, 10, 7, 7), 0);    // 30.05.2014 OG: my_itoa() gibt es nicht mehr
-            //lcd_puts_at (0, 4, my_itoa(pos->Latitude, 10, 7, 7), 0);      // 30.05.2014 OG: my_itoa() gibt es nicht mehr
-            //lcd_puts_at (11, 4, my_itoa(pos->Longitude, 10, 7, 7), 0);    // 30.05.2014 OG: my_itoa() gibt es nicht mehr
-        }
-        // Formel verwendet von http://www.kompf.de/gps/distcalc.html
-        // 111.3 km = Abstand zweier Breitenkreise und/oder zweier Langenkreise am Äquator
-        // es wird jedoch in dm Meter weiter gerechnet
-        // (tlon1 - tlon2)/10) sonst uint32_t-Überlauf bei cos(0) gleich 1
-        d1       = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000;
-        dlat = 1113 * (lat1 - lat2) / 10000;
-        geo.bearing = (my_atan2(d1, dlat) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit
-        geo.distance = sqrt32(d1 * d1 + dlat * dlat);
-        if( !CheckGPS )
-        {
-            lcd_printp_at (0, 5, PSTR("Bear:"), 0);
-            lcdx_printf_at_P( 5, 5, MNORMAL, 0,0, PSTR("%3d"), geo.bearing );
-            //lcd_puts_at (5, 5, my_itoa((uint32_t)geo.bearing, 3, 0, 0), 0);       // 30.05.2014 OG: my_itoa() gibt es nicht mehr
-            lcd_printp_at (8, 5, PSTR("\x1e"), 0);
-            lcd_printp_at (9, 5, PSTR("Dist:"), 0);
-            lcdx_printf_at_P( 15, 5, MNORMAL, 0,0, PSTR("%3d"), geo.distance );
-            //lcd_puts_at (15, 5, my_itoa((uint32_t)geo.distance, 3, 1, 1), 0);     // 30.05.2014 OG: my_itoa() gibt es nicht mehr
-            lcd_printp_at (20, 5, PSTR("m"), 0);
-        }

Subversion Repositories Projects

(root)/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2Merge/gps/gps.c - Rev 2200 → 2212