| 0,0 → 1,242 |
|---|
| //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| |
| //############################################################################ |
| //# HISTORY gps.c |
| //# |
| //# 03.08.2015 cebra |
| //# - add: Routine um aus gegebenen Koordinaten mit Abstand und Winkel eine ZielKoordinate zu berechnen |
| //# int nmea_move_horz( |
| //# const nmeaPOS *start_pos, /**< Start position in radians */ |
| //# nmeaPOS *end_pos, /**< Result position in radians */ |
| //# double azimuth, /**< Azimuth (degree) [0, 359] */ |
| //# double distance) /**< Distance (km) */ |
| //# |
| //# 27.06.2014 OG - NEU |
| //# - chg: auf #include "../gps/mymath.h" angepasst |
| //# |
| //# 20.06.2014 OG - NEU |
| //############################################################################ |
| |
| |
| #include "../cpu.h" |
| #include <string.h> |
| #include <util/delay.h> |
| #include <avr/interrupt.h> |
| #include <stdlib.h> |
| #include <math.h> |
| #include "../main.h" |
| |
| #include "../mk-data-structs.h" |
| #include "../gps/mymath.h" |
| #include "gps.h" |
| |
| |
| /* |
| // definiert in: mk_data-stucts.h |
| typedef struct |
| { |
| u16 Distance; // distance to target in cm |
| s16 Bearing; // course to target in deg |
| } __attribute__((packed)) GPS_PosDev_t; |
| */ |
| |
| /* |
| // definiert in: mk_data-stucts.h |
| typedef struct |
| { |
| s32 Longitude; // in 1E-7 deg |
| s32 Latitude; // in 1E-7 deg |
| s32 Altitude; // in mm |
| u8 Status; // validity of data |
| } __attribute__((packed)) GPS_Pos_t; |
| */ |
| |
| |
| //-------------------------------------------------------------- |
| |
| #define NMEA_PI (3.141592653589793) /**< PI value */ |
| #define NMEA_PI180 (NMEA_PI / 180) /**< PI division by 180 */ |
| #define NMEA_EARTHRADIUS_KM (6378) /**< Earth's mean radius in km */ |
| #define R (6371) |
| #define NMEA_EARTHRADIUS_M (NMEA_EARTHRADIUS_KM * 1000) /**< Earth's mean radius in m */ |
| #define NMEA_EARTH_SEMIMAJORAXIS_M (6378137.0) /**< Earth's semi-major axis in m according WGS84 */ |
| #define NMEA_EARTH_SEMIMAJORAXIS_KM (NMEA_EARTHMAJORAXIS_KM / 1000) /**< Earth's semi-major axis in km according WGS 84 */ |
| #define NMEA_EARTH_FLATTENING (1 / 298.257223563) /**< Earth's flattening according WGS 84 */ |
| #define NMEA_DOP_FACTOR (5) /**< Factor for translating DOP to meters */ |
| |
| # define NMEA_POSIX(x) x |
| |
| |
| |
| /** |
| * \fn nmea_degree2radian |
| * \brief Convert degree to radian |
| */ |
| double nmea_degree2radian(double val) |
| { return (val * NMEA_PI180); } |
| |
| |
| //------------------------------------------------------------------------------------------ |
| nmeaPOS NMEApos; |
| nmeaPOS NMEATarget; |
| |
| /** |
| * \brief Horizontal move of point position |
| */ |
| int nmea_move_horz( |
| const nmeaPOS *start_pos, /**< Start position in radians */ |
| nmeaPOS *end_pos, /**< Result position in radians */ |
| double azimuth, /**< Azimuth (degree) [0, 359] */ |
| double distance /**< Distance (km) */ |
| ) |
| { |
| nmeaPOS p1 = *start_pos; |
| 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->lon = p1.lon + atan2( sin(azimuth) * sin(distance) * cos(p1.lat), cos(distance) - sin(p1.lat) * sin(end_pos->lat)); |
| |
| if(NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)) |
| { |
| end_pos->lat = 0; end_pos->lon = 0; |
| RetVal = 0; |
| } |
| |
| return RetVal; |
| } |
| |
| |
| |
| //############################################################################################### |
| |
| |
| |
| //-------------------------------------------------------------- |
| GPS_PosDev_t gps_Deviation( GPS_Pos_t pos1, GPS_Pos_t pos2 ) |
| { |
| int32_t lat1, lon1, lat2, lon2; |
| int32_t d1, dlat; |
| GPS_PosDev_t PosDev; |
| |
| lon1 = pos1.Longitude; |
| lat1 = pos1.Latitude; |
| |
| lon2 = pos2.Longitude; |
| lat2 = pos2.Latitude; |
| |
| d1 = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000; |
| dlat = (1113 * (lat1 - lat2) / 10000); |
| |
| PosDev.Bearing = (my_atan2(d1, dlat) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit |
| PosDev.Distance = sqrt32( d1 * d1 + dlat * dlat ); // |
| //PosDev.Distance = sqrt32( d1 * d1 + dlat * dlat ) * 10; // *10 um von dm auf cm zu kommen |
| |
| 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); |
| } |
| |
| |
| return(geo); |
| } |
| */ |
| |
| Property changes: |
|---|
| Added: svn:mime-type |
|---|
| +text/plain |
|---|
| \ No newline at end of property |
|---|