4,18 → 4,7 |
//############################################################################ |
//# HISTORY gps.c |
//# |
//# |
//# |
//# |
//# |
//# |
//# |
//# |
//# 22.09.2015 Starter |
//# - followme_add_offset(...) und followme_calculate_offset getestet mit PKT |
//# - add my_abs(...) |
//# |
//# 20.09.2015 Starter |
//# 20.09.2015 Startet |
//# - add Routine um einen Offset in Meter zu den aktuellen Koordinaten dazurechnen |
//# followme_calculate_offset(...) |
//# |
47,6 → 36,29 |
#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 */ |
97,13 → 109,13 |
distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */ |
azimuth = nmea_degree2radian(azimuth); |
|
end_pos->latitude = asin( sin(p1.latitude) * cos(distance) + cos(p1.latitude) * sin(distance) * cos(azimuth)); |
end_pos->lat = asin( sin(p1.lat) * cos(distance) + cos(p1.lat) * sin(distance) * cos(azimuth)); |
|
end_pos->longitude = p1.longitude + atan2( sin(azimuth) * sin(distance) * cos(p1.latitude), cos(distance) - sin(p1.latitude) * sin(end_pos->latitude)); |
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->latitude) || NMEA_POSIX(isnan)(end_pos->longitude)) |
if(NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)) |
{ |
end_pos->latitude = 0; end_pos->longitude = 0; |
end_pos->lat = 0; end_pos->lon = 0; |
RetVal = 0; |
} |
|
129,14 → 141,11 |
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 ) ) ) ); |
target_pos->lat = pktPos.lat + ( ( followMeOffset.offset_lat * ( LAT_DIV / FOLLOWME_M2DEG ) ) ) / 1000; |
target_pos->lon = pktPos.lon + ( ( followMeOffset.offset_long * ( LONG_DIV / FOLLOWME_M2DEG ) * (8192/1000) ) / my_abs( c_cos_8192( (pktPos.lat / LONG_DIV ) ) ) ); |
return 1; |
} |
|
|
// schlanke abs-Methode |
// TODO: move to mymath.h |
int16_t my_abs(int16_t input) |
{ |
if(input < 0) |
157,8 → 166,8 |
{ |
angle %= 360; // map angle to 0° - 360° |
|
followMeOffset->latitude = ( radius * c_cos_8192( angle ) ) / 8192; |
followMeOffset->longitude = ( radius * c_sin_8192( angle ) ) / 8192; |
followMeOffset->offset_lat = ( radius * c_cos_8192( angle ) ) / 8192; |
followMeOffset->offset_long = ( radius * c_sin_8192( angle ) ) / 8192; |
|
return 1; |
} |
192,4 → 201,103 |
} |
|
|
///** |
// * \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); |
} |
*/ |
|