Rev 2192 |
Go to most recent revision |
Blame |
Last modification |
View Log
| RSS feed
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//############################################################################
//# 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);
}
*/