| /Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/gps/gps.c |
|---|
| 0,0 → 1,274 |
| //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //############################################################################ |
| //# HISTORY gps.c |
| //# |
| //# 20.09.2015 Startet |
| //# - add Routine um einen Offset in Meter zu den aktuellen Koordinaten dazurechnen |
| //# followme_calculate_offset(...) |
| //# |
| //# 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 */ |
| // Definitonen für FollowMeStep2 |
| #define LONG_DIV 10000000 |
| #define LAT_DIV LONG_DIV |
| #define FOLLOWME_M2DEG 111111 |
| #define FOLLOWME_ROUND_100 100 |
| # 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; |
| } |
| // 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 |
| uint8_t followme_calculate_offset( |
| const nmeaPOS *pPktPos, /**< Start position in radians */ |
| nmeaPOS *target_pos, /**< Result position in radians */ |
| int d_lat, /**< Distance lat(m) */ |
| int d_long /**< Distance long(m) */ |
| ) |
| { |
| nmeaPOS pktPos = *pPktPos; |
| target_pos->lat = pktPos.lat + ( d_lat * ( LAT_DIV / FOLLOWME_M2DEG ) ); |
| target_pos->lon = pktPos.lon + ( d_long * ( LONG_DIV / FOLLOWME_M2DEG ) * 8192 ) / abs ( c_cos_8192( (int16_t)(pktPos.lat / LONG_DIV ) ) ); |
| return 1; |
| } |
| //############################################################################################### |
| //-------------------------------------------------------------- |
| 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 |
| /Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/gps/gps.h |
|---|
| 0,0 → 1,57 |
| //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //############################################################################ |
| //# HISTORY gps.h |
| //# |
| //# 20.06.2014 OG - NEU |
| //############################################################################ |
| #ifndef GPS_H_ |
| #define GPS_H_ |
| GPS_PosDev_t gps_Deviation( GPS_Pos_t pos1, GPS_Pos_t pos2 ); |
| /** |
| * Position data in fractional degrees or radians |
| */ |
| typedef struct _nmeaPOS |
| { |
| int32_t lat; /**< Latitude */ |
| int32_t lon; /**< Longitude */ |
| } nmeaPOS; |
| extern nmeaPOS NMEApos; |
| extern nmeaPOS NMEATarget; |
| 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) */ |
| ); |
| int nmea_move_horz1( |
| 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) */ |
| ); |
| uint8_t followme_calculate_offset( |
| const nmeaPOS *pkt_pos, /**< Start position in radians */ |
| nmeaPOS *target_pos, /**< Result position in radians */ |
| int d_lat, /**< Distance lat(m) */ |
| int d_lon /**< Distance long(m) */ |
| ); |
| #endif // #define GPS_H_ |
| Property changes: |
| Added: svn:mime-type |
| +text/plain |
| \ No newline at end of property |
| /Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/gps/gpsmouse.c |
|---|
| 0,0 → 1,532 |
| //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //############################################################################ |
| //# HISTORY gpsmouse.c |
| //# |
| //# 30.07.2015 CB |
| //# - add: Anzeige von CRC Fehlern im NMEA Datensatz in GPSMouse_ShowData |
| //# |
| //# 28.07.2015 CB |
| //# - fix: Überprüfung von führenden 00 in der MAC-Adresse der BT Maus abgeschaltet |
| //# |
| //# 28.06.2014 OG |
| //# - fix: GPSMouse_ShowData() - wenn BT-Datenverlust dann immer GPSMouse_Disconnect() |
| //# auch wenn waitsatfix Modus aktiv ist |
| //# - fix: GPSMouse_ShowData() - Rueckgabe von 0 wenn Verbindung bereits vorhanden war |
| //# und dabei aber ein Verbindungsverlust zur BT GPS-Maus auftrat |
| //# |
| //# 27.06.2014 OG |
| //# - chg: GPSMouse_Disconnect() - Anzeige von BT-Datenverlust optimiert |
| //# - chg: GPSMouse_ShowData() - nach GPSMouse_Connect ein clear_key_all() eingefuegt |
| //# |
| //# 24.06.2014 OG |
| //# - add: GPSMouse_Connect(), GPSMouse_Disconnect() |
| //# |
| //# 23.06.2014 OG |
| //# - chg: GPSMouse_ShowData() - neue Parameter und Return-Codes fuer |
| //# hoehere Flexibilitaet |
| //# |
| //# 22.06.2014 OG - NEU |
| //# ehemals BT_ShowGPSData() in setup.c |
| //############################################################################ |
| #include "../cpu.h" |
| #include <avr/io.h> |
| #include <avr/interrupt.h> |
| #include <avr/pgmspace.h> |
| #include <string.h> |
| #include <util/delay.h> |
| #include "../main.h" |
| #include "../lcd/lcd.h" |
| #include "../setup/setup.h" |
| #include "../eeprom/eeprom.h" |
| #include "../lipo/lipo.h" |
| #include "../timer/timer.h" |
| #include "../bluetooth/fifo.h" |
| #include "../bluetooth/bluetooth.h" |
| #include "../uart/uart1.h" |
| #include "../utils/menuctrl.h" |
| #include "../utils/xutils.h" |
| #include "../pkt/pkt.h" |
| #include "../messages.h" |
| //#include "../gps/gps_nmea.h" |
| #include "../avr-nmea-gps-library/nmea.h" |
| #include "../gps/gpsmouse.h" |
| static const char STR_FIX0[] PROGMEM = " no"; |
| static const char STR_FIX1[] PROGMEM = " GPS"; |
| static const char STR_FIX2[] PROGMEM = "DGPS"; |
| static const char STR_FIX6[] PROGMEM = "esti"; |
| static const char STR_FIX[] PROGMEM = " ---"; |
| //-------------------------------------------------------------- |
| // retcode = GPSMouse_Connect() |
| // |
| // RUECKGABE: |
| // -2: kein BTM222 eingebaut |
| // -1: keine GPS Maus konfiguriert |
| // 0: Fehler bei connect, kein connect durchgeführt (evtl. GPS Maus nicht eingeschaltet) |
| // 1: verbunden |
| //-------------------------------------------------------------- |
| int8_t GPSMouse_Connect( void ) |
| { |
| uint8_t status; |
| if( !BT_CheckBTM222() ) |
| { |
| // PKT_Message_P( text, error, timeout, beep, clearscreen) |
| PKT_Message_P( strGet(STR_NOBTM222), true, 1000, true, true); // 1000 = max. 10 Sekunden anzeigen; "kein BTM222 vorh." |
| return -2; // -2: kein BTM222 eingebaut |
| } |
| // deaktiviert, MAC Adressen von Handys haben keine 00 in der MAC-Adresse |
| // if( (Config.gps_UsedMac[5] == '0') || (Config.gps_UsedMac[6] == '0') ) |
| // { |
| // PKT_Message_P( text, error, timeout, beep, clearscreen) |
| // PKT_Message_P( strGet(STR_NOGPSMOUSE), true, 1000, true, true); // 1000 = max. 10 Sekunden anzeigen; "keine GPS-Maus!" |
| // return -1; // -1: keine GPS Maus konfiguriert |
| // } |
| //-------------------- |
| // verbinde BT GPS-Maus |
| //-------------------- |
| PKT_Title( Config.gps_UsedDevName, true, true); // PKT_Title( text, lShowLipo, clearscreen ) |
| lcdx_printp_center( 2, strGet(STR_GPSMOUSECONNECT), MNORMAL, 0,1); // "suche GPS-Maus" |
| PKT_Gauge_Begin( 0 ); // Gauge: 0 = Default fuer y verwenden |
| set_BTOn(); |
| if( Config.BTIsSlave ) |
| { |
| bt_downlink_init(); |
| } |
| status = bt_connect( Config.gps_UsedMac ); |
| uart1_flush(); |
| fifo_clear(&in_fifo); |
| UART1_receiveNMEA = true; // Uartflag setzen zur Erkennung der NMEA Datensätze |
| PKT_Gauge_End(); // Gauge: Ende |
| if( !status ) // keine Verbindung zum BT-Device |
| { |
| set_BTOff(); |
| // PKT_Message_P( text, error, timeout, beep, clearscreen) |
| PKT_Message_P( strGet(STR_GPSMOUSECONNECTION), true, 1000, true, true); // 1000 = max. 10 Sekunden anzeigen; "GPS-Maus Verbindung" |
| return 0; // 0: Fehler bei connect, kein connect durchgeführt (evtl. GPS Maus nicht eingeschaltet) |
| } |
| return 1; // 1 = ok, Verbindung steht |
| } |
| //-------------------------------------------------------------- |
| // GPSMouse_Disconnect() |
| //-------------------------------------------------------------- |
| void GPSMouse_Disconnect( void ) |
| { |
| PKT_Title( Config.gps_UsedDevName, true, true); //PKT_Title( text, lShowLipo, clearscreen ) |
| //--------------------- |
| // GPS beenden |
| //--------------------- |
| UART1_receiveNMEA = false; |
| if( !receiveNMEA ) // ggf. anders implementieren, z.B. via Parameter; aber erstmal geht's auch so |
| { |
| //lcdx_cls_row( y, mode, yoffs ) |
| lcdx_cls_row( 7, MINVERSX, 0 ); |
| lcdx_printp_center( 7, strGet(STR_BT_LOSTDATA), MINVERSX, 0,0); // "BT Datenverlust" |
| set_beep( 600, 0xffff, BeepNormal); |
| } |
| lcdx_printp_center( 2, strGet(STR_GPSMOUSEDISCONNECT), MNORMAL, 0,1); // "trenne GPS-Maus" |
| PKT_Gauge_Begin( 0 ); // Gauge: 0 = Default fuer y verwenden |
| receiveNMEA = false; // Uartflag zurücksetzen, es kommen keine NMEA Datensätze mehr |
| if (!bt_disconnect()); |
| { |
| // lcdx_printp_center( 2, PSTR("Error on disconnect"), MNORMAL, 0,1); // Fehler beim Disconncet aufgetreten |
| // _delay_ms(2000); // 09.08.2015 cebra, muss noch untersucht werden, ist aber jetzt kein Problem |
| } |
| set_BTOff(); |
| PKT_Gauge_End(); // Gauge: Ende |
| } |
| //-------------------------------------------------------------- |
| // retcode = GPSMouse_ShowData( mode, waitsatfix ) |
| // |
| // zeigt GPS-Daten der aktuellen BT-Maus an |
| // |
| // PARAMETER: |
| // show: |
| // was soll in der 2. Zeile angezeigt werden |
| // GPSMOUSE_SHOW_TIME || GPSMOUSE_SHOW_WAITSATFIX |
| // |
| // waitsatifix: |
| // 0 = Verbindungsaufbau; Daten anzeigen und beenden der |
| // BT-Verbindung (fuer setup.c) |
| // n = Zeit (via timer) die nach einem erfolgreichen Satfix gewartet wird bis |
| // GPSMouse_ShowData() beendet wird - die Verbindung zur GPS-Maus bleibt erhalten |
| // und kann weiterverwendet werden! (z.B. in followme.c) |
| // |
| // RUECKGABE: |
| // -2: kein BTM222 eingebaut |
| // -1: keine GPS Maus konfiguriert |
| // 0: Fehler bei connect, kein connect durchgeführt (evtl. GPS Maus nicht eingeschaltet) |
| // 1: connect zur GPS Maus ok, Benutzer hat Anzeige beenden |
| // 2: connect zur GPS Maus ok, Satix hat Anzeige beendet |
| // |
| // zusammengefasst: |
| // >0 : Ok |
| // <=0: Fehler |
| //-------------------------------------------------------------- |
| int8_t GPSMouse_ShowData( uint8_t show, uint16_t waitsatfix ) |
| { |
| uint8_t redraw = true; |
| uint8_t state_posfix = 0; |
| const char *pStr; |
| int8_t yoffs; |
| int8_t retcode; |
| //-------------------- |
| // BT GPS-Maus verbinden |
| //-------------------- |
| retcode = GPSMouse_Connect(); |
| if( retcode <= 0 ) |
| { |
| return retcode; |
| } |
| timer_nmea_timeout = NMEA_TIMEOUT; // Timeout Timer setzen |
| clear_key_all(); |
| //--------------------- |
| // zeige GPS-Daten |
| //--------------------- |
| receiveNMEA = true; |
| do |
| { |
| //------------------------- |
| // Screen redraw |
| //------------------------- |
| if( redraw ) |
| { |
| PKT_Title( Config.gps_UsedDevName, true, true); //PKT_Title( text, lShowLipo, clearscreen ) |
| if( show == GPSMOUSE_SHOW_TIME ) // Zeile 2: GPS-Zeit |
| { |
| lcdx_printp_at( 6, 1, PSTR("["), MNORMAL, -1,2); // Mitte: GPS Time |
| lcdx_printp_at(15, 1, PSTR("]"), MNORMAL, -2,2); // .. |
| lcd_line( 0, 13, 34, 13, 1); // .. |
| lcd_line(92, 13, 127, 13, 1); // .. |
| } |
| if( show == GPSMOUSE_SHOW_WAITSATFIX ) // Zeile 2: "warte auf Satfix..." |
| { |
| yoffs = 1; |
| lcdx_cls_row( 1, MINVERS, yoffs); |
| lcdx_printp_center( 1, strGet(STR_WAITSATFIX), MINVERS, 0,yoffs); // "warte auf Satfix..." |
| } |
| // yoffs = 1; |
| // lcdx_cls_row( 1, MINVERS, yoffs); |
| // lcdx_printp_center( 1,strGet(STR_WAITNMEA), MINVERS, 0,yoffs); // "warte auf NMEA..." |
| lcdx_printp_at( 0, 2, PSTR("Fix:") , MNORMAL, 0,3); // Links; Fix |
| lcdx_printp_at( 0, 3, PSTR("Sat:") , MNORMAL, 0,3); // Links; Sat |
| lcdx_printp_at( 0, 4, PSTR("Alt:") , MNORMAL, 0,3); // Links; Alt |
| lcdx_printp_at(11, 2, PSTR("RCnt:"), MNORMAL, 0,3); // Rechts: RCnt - Zaehler empfangener GPGGA Datenpakete |
| lcdx_printp_at(11, 3, PSTR("CErr:"), MNORMAL, 0,3); // Rechts: RErr - Zaehler fehlerhafte NMEA Pakete |
| lcdx_printp_at(11, 4, PSTR("HDOP:"), MNORMAL, 0,3); // Rechts: HDOP |
| lcdx_cls_row( 5, MINVERS, 4); // Hintergrund invers |
| lcdx_printp_at( 1, 5, strGet(START_LASTPOS1), MINVERS, 0,4); // "Breitengr. Längengr." |
| lcd_rect( 0, (4*8)+11, 127, (2*8)+4, 1); // Rahmen |
| redraw = false; |
| } |
| //------------------------- |
| // PKT-LiPo anzeigen |
| //------------------------- |
| show_Lipo(); |
| //------------------------- |
| // PKT-Update geht hier nicht... |
| // wegen der Hardware-Verbindung |
| //------------------------- |
| //if( PKT_CtrlHook() ) |
| //{ |
| // redraw = true; |
| //} |
| //------------------------- |
| //------------------------- |
| if(NMEA_isdataready()) // NMEA Daten vorhanden? |
| { |
| if( show == GPSMOUSE_SHOW_TIME ) |
| { |
| lcdx_print_at(7, 1, (uint8_t *)NMEA.Time, MNORMAL, -2,2); // Mitte: GPS Time |
| } |
| //-------------- |
| // Sat-Fix |
| //-------------- |
| //writex_ndigit_number_u( 6, 2, NMEA.SatFix , 2,0, MNORMAL, 0,3); // Links: Fix (als Zahl) |
| switch( NMEA.SatFix ) |
| { |
| case 0: pStr = STR_FIX0; break; // kein Sat-Fix! |
| case 1: pStr = STR_FIX1; break; // GPS - Fix: OK |
| case 2: pStr = STR_FIX2; break; // DGPS - Fix: OK |
| case 6: pStr = STR_FIX6; break; // Estimated - wird hier als nicht Ok angesehen weil nicht klar was es bedeutet |
| default: pStr = 0; // Sat-Fix Code unbekannt! |
| lcdx_printf_at_P( 4, 2, MNORMAL, 1,3, PSTR(" ? %d"), NMEA.SatFix); // -> Zahl wird angezeigt - ggf. recherchieren was da bedeutet (Fix ok oder nicht) |
| break; |
| } |
| if( pStr ) |
| { |
| lcdx_printf_at_P( 4, 2, MNORMAL, 1,3, PSTR("%4S"), pStr); // GPS Fix als String ausgeben |
| } |
| if( NMEA.SatFix==1 || NMEA.SatFix==2 ) // Sat-Fix OK => einen Haken anzeigen |
| lcdx_putc( 8, 2, SYMBOL_CHECK, MNORMAL, 2,3 ); |
| else |
| lcdx_putc( 8, 2, ' ', MNORMAL, 2,3 ); |
| //-------------- |
| // Sat-Anzahl |
| //-------------- |
| writex_ndigit_number_u( 6, 3, NMEA.SatsInUse, 2,0, MNORMAL, 1,3); // Links: Sat-Anzahl |
| if( NMEA.SatsInUse >= GPSMOUSE_MINSAT ) // Haken angezeigt wenn MINSAT ok |
| lcdx_putc( 8, 3, SYMBOL_CHECK, MNORMAL, 2,3 ); |
| else |
| lcdx_putc( 8, 3, ' ', MNORMAL, 2,3 ); |
| //-------------- |
| // Altitude |
| //-------------- |
| lcdx_printf_at_P( 4, 4, MNORMAL, 1,3, PSTR("%4.1d"), NMEA.Altitude); // Links: Altitude |
| //-------------- |
| // empfangene GPGGA Datenpakete |
| //-------------- |
| lcdx_printf_at_P(16, 2, MNORMAL, 0,3, PSTR("%5ld"), NMEA.Counter ); // Rechts: Anzahl empfangener NMEA GPGGA-Pakete |
| //-------------- |
| // NMEA CRC Fehler |
| //-------------- |
| lcdx_printf_at_P(16, 3, MNORMAL, 0,3, PSTR("%5ld"), NMEA_getCRCerror() ); // Rechts: Anzahl CRC Fehler im NMEA GPGGA-Pakete |
| // lcdx_printf_at( 15, 3, mode, 0, 3, "%3.5ld", (uint32_t)NMEA_getBearing()); |
| // writex_ndigit_number_u_10th(15, 3, NMEA_getBearing(), 4,0, MNORMAL, 0,3); |
| //-------------- |
| // HDOP |
| //-------------- |
| if( NMEA.HDOP == 0 ) // Rechts:: HDOP |
| lcdx_printp_at(16, 4, PSTR(" ---"), MNORMAL, 0,3); |
| else |
| writex_ndigit_number_u_10th(16, 4, NMEA.HDOP, 4,0, MNORMAL, 0,3); |
| //-------------- |
| // Lat / Lon |
| //-------------- |
| writex_gpspos( 1, 6, NMEA.Latitude , MNORMAL, 0,6 ); // unten links: Latitude |
| writex_gpspos(12, 6, NMEA.Longitude, MNORMAL, 0,6 ); // unten rechts: Longitude |
| //-------------- |
| //-------------- |
| if( waitsatfix > 0 ) |
| { |
| if( (NMEA.SatFix==1 || NMEA.SatFix==2) && (state_posfix==0) ) |
| { |
| state_posfix = 1; |
| timer = waitsatfix; // Verzoegerung bzgl. Exit (z.B. 600 = 6 Sekunden) |
| } |
| if( (NMEA.SatFix!=1 && NMEA.SatFix!=2) && (state_posfix==1) && (timer!=0) ) |
| { |
| state_posfix = 0; |
| } |
| if( (NMEA.SatFix==1 || NMEA.SatFix==2) && (state_posfix==1) && (timer==0) ) |
| { |
| clear_key_all(); |
| return 2; // 2: connect zur GPS Maus ok, Satix hat Anzeige beendet |
| } |
| } |
| } |
| if (timer_nmea_timeout == 0) receiveNMEA = false; |
| } while( !get_key_press(1 << KEY_ESC) && receiveNMEA ); |
| //--------------------- |
| // GPS beenden |
| //--------------------- |
| if( (waitsatfix == 0) || !receiveNMEA ) |
| { |
| GPSMouse_Disconnect(); |
| } |
| clear_key_all(); |
| return (receiveNMEA ? 1 : 0); // 1: connect zur GPS Maus ok, Benutzer hat Anzeige beenden |
| } |
| // save, Anzeige der Daten im Format der neuen NMEA Routinen, nach Umbau auf NMEA Structur nicht mehr notwendig |
| // { |
| // |
| // |
| // if( show == GPSMOUSE_SHOW_TIME ) |
| // { |
| //// lcdx_print_at(7, 1, (uint8_t *)NMEA.Time, MNORMAL, -2,2); // Mitte: GPS Time |
| // |
| // writex_ndigit_number_u( 7, 1, NMEA_getHour(), 2,0, MNORMAL, -2,2); |
| // lcdx_printp_at( 9, 1, PSTR(":") , MNORMAL, -2,2); |
| // writex_ndigit_number_u( 10, 1, NMEA_getMinute(), 2,0, MNORMAL, -2,2); |
| // lcdx_printp_at( 12, 1, PSTR(":") , MNORMAL, -2,2); |
| // writex_ndigit_number_u( 13, 1, NMEA_getSecond(), 2,0, MNORMAL, -2,2); |
| // |
| // } |
| // |
| // //-------------- |
| // // Sat-Fix |
| // //-------------- |
| // //writex_ndigit_number_u( 6, 2, NMEA.SatFix , 2,0, MNORMAL, 0,3); // Links: Fix (als Zahl) |
| // switch( NMEA_getGPSfix() ) |
| // { |
| // case 0: pStr = STR_FIX0; break; // kein Sat-Fix! |
| // case 1: pStr = STR_FIX1; break; // GPS - Fix: OK |
| // case 2: pStr = STR_FIX2; break; // DGPS - Fix: OK |
| // case 6: pStr = STR_FIX6; break; // Estimated - wird hier als nicht Ok angesehen weil nicht klar was es bedeutet |
| // default: pStr = 0; // Sat-Fix Code unbekannt! |
| // lcdx_printf_at_P( 4, 2, MNORMAL, 1,3, PSTR(" ? %d"), NMEA_getGPSfix()); // -> Zahl wird angezeigt - ggf. recherchieren was da bedeutet (Fix ok oder nicht) |
| // break; |
| // } |
| // |
| // if( pStr ) |
| // { |
| // lcdx_printf_at_P( 4, 2, MNORMAL, 1,3, PSTR("%4S"), pStr); // GPS Fix als String ausgeben |
| // } |
| // |
| // if( NMEA_getGPSfix()==1 || NMEA_getGPSfix()==2 ) // Sat-Fix OK => einen Haken anzeigen |
| // lcdx_putc( 8, 2, SYMBOL_CHECK, MNORMAL, 2,3 ); |
| // else |
| // lcdx_putc( 8, 2, ' ', MNORMAL, 2,3 ); |
| // |
| // |
| // //-------------- |
| // // Sat-Anzahl |
| // //-------------- |
| // writex_ndigit_number_u( 6, 3, NMEA_getSatellites(), 2,0, MNORMAL, 1,3); // Links: Sat-Anzahl |
| // if( NMEA_getSatellites() >= GPSMOUSE_MINSAT ) // Haken angezeigt wenn MINSAT ok |
| // lcdx_putc( 8, 3, SYMBOL_CHECK, MNORMAL, 2,3 ); |
| // else |
| // lcdx_putc( 8, 3, ' ', MNORMAL, 2,3 ); |
| // |
| // |
| // //-------------- |
| // // Altitude |
| // //-------------- |
| // lcdx_printf_at_P( 4, 4, MNORMAL, 1,3, PSTR("%4.1d"), NMEA_getAltitude()); // Links: Altitude |
| // |
| // |
| // //-------------- |
| // // empfangene GPGGA Datenpakete |
| // //-------------- |
| // lcdx_printf_at_P(16, 2, MNORMAL, 0,3, PSTR("%5ld"), NMEA_getNMEAcounter() ); // Rechts: Anzahl empfangener NMEA GPGGA-Pakete |
| // |
| // |
| // //-------------- |
| // // NMEA CRC Fehler |
| // //-------------- |
| // lcdx_printf_at_P(16, 3, MNORMAL, 0,3, PSTR("%5ld"), NMEA_getCRCerror() ); // Rechts: Anzahl empfangener NMEA GPGGA-Pakete |
| // |
| //// writex_ndigit_number_u(1, 5, UART1_GPGGA , 4,0, MNORMAL, 0,3); |
| //// writex_ndigit_number_u(5, 5, bt_overrun , 4,0, MNORMAL, 0,3); |
| //// writex_ndigit_number_u(10, 5, bt_bufferoverflow , 4,0, MNORMAL, 0,3); |
| // |
| // |
| // //-------------- |
| // // HDOP |
| // //-------------- |
| // if(NMEA_getHDOP() == 0 ) // Rechts:: HDOP |
| // lcdx_printp_at(16, 4, PSTR(" ---"), MNORMAL, 0,3); |
| // else |
| // writex_ndigit_number_u_10th(16, 4, NMEA_getHDOP(), 4,0, MNORMAL, 0,3); |
| // |
| // |
| // //-------------- |
| // // Lat / Lon |
| // //-------------- |
| // writex_gpspos( 1, 6, NMEA_getLatitude() , MNORMAL, 0,6 ); // unten links: Latitude |
| // writex_gpspos(12, 6, NMEA_getLongitude(), MNORMAL, 0,6 ); // unten rechts: Longitude |
| // |
| // |
| // //-------------- |
| // //-------------- |
| // if( waitsatfix > 0 ) |
| // { |
| // if( (NMEA_getGPSfix()==1 || NMEA_getGPSfix()==2) && (state_posfix==0) ) |
| // { |
| // state_posfix = 1; |
| // timer = waitsatfix; // Verzoegerung bzgl. Exit (z.B. 600 = 6 Sekunden) |
| // } |
| // |
| // if( (NMEA_getGPSfix()!=1 && NMEA_getGPSfix()!=2) && (state_posfix==1) && (timer!=0) ) |
| // { |
| // state_posfix = 0; |
| // } |
| // |
| // if( (NMEA_getGPSfix()==1 || NMEA_getGPSfix()==2) && (state_posfix==1) && (timer==0) ) |
| // { |
| // clear_key_all(); |
| // return 2; // 2: connect zur GPS Maus ok, Satix hat Anzeige beendet |
| // } |
| // } |
| // |
| // } |
| Property changes: |
| Added: svn:mime-type |
| +text/plain |
| \ No newline at end of property |
| /Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/gps/gpsmouse.h |
|---|
| 0,0 → 1,46 |
| //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //############################################################################ |
| //# HISTORY gpsmouse.h |
| //# |
| //# 24.06.2014 OG |
| //# - chg: Parameteraenderung bei GPSMouse_ShowData() |
| //# - add: GPSMouse_Connect(), GPSMouse_Disconnect() |
| //# |
| //# 22.06.2014 OG |
| //# - add: GPSMouse_ShowData() |
| //# - add: #define GPSMOUSE_SHOWDATA, GPSMOUSE_WAITSATFIX |
| //# |
| //# 22.06.2014 OG - NEU |
| //############################################################################ |
| #ifndef GPSMOUSE_H_ |
| #define GPSMOUSE_H_ |
| //---------------------------------- |
| // GPSMouse_ShowData() - show |
| //---------------------------------- |
| #define GPSMOUSE_SHOW_TIME 1 |
| #define GPSMOUSE_SHOW_WAITSATFIX 2 |
| //---------------------------------- |
| // minimal akzeptierte Sat-Zahl fuer |
| // Steuerungsfunktionen des MK |
| //---------------------------------- |
| #define GPSMOUSE_MINSAT 6 |
| //---------------------------------- |
| // Export |
| //---------------------------------- |
| int8_t GPSMouse_Connect( void ); |
| void GPSMouse_Disconnect( void ); |
| int8_t GPSMouse_ShowData( uint8_t show, uint16_t waitsatfix ); |
| #endif // #define GPSMOUSE_H_ |
| Property changes: |
| Added: svn:mime-type |
| +text/plain |
| \ No newline at end of property |
| /Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/gps/mymath.c |
|---|
| 0,0 → 1,214 |
| /****************************************************************/ |
| /* */ |
| /* NG-Video 5,8GHz */ |
| /* */ |
| /* Copyright (C) 2011 - gebad */ |
| /* */ |
| /* This code is distributed under the GNU Public License */ |
| /* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
| /* */ |
| /****************************************************************/ |
| //############################################################################ |
| //# HISTORY mymath.c |
| //# |
| //# 24.04.2013 OG |
| //# - chg: 'uint16_t atantab[T] PROGMEM' nach 'const uint16_t atantab[T] PROGMEM' |
| //# wegen Compile-Error in AtmelStudio 6 |
| //############################################################################ |
| #include <stdio.h> |
| #include <stdlib.h> |
| #include <math.h> |
| #include <avr/pgmspace.h> |
| #include "mymath.h" |
| // http://www.mikrocontroller.net/articles/AVR_Arithmetik#avr-gcc_Implementierung_.2832_Bit.29 |
| unsigned int sqrt32(uint32_t q) |
| { uint16_t r, mask; |
| uint16_t i = 8*sizeof (r) -1; |
| r = mask = 1 << i; |
| for (; i > 0; i--) { |
| mask >>= 1; |
| if (q < (uint32_t) r*r) |
| r -= mask; |
| else |
| r += mask; |
| } |
| if (q < (uint32_t) r*r) |
| r -= 1; |
| return r; |
| } |
| // aus Orginal MK source code |
| // sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
| const uint16_t sinlookup[91] = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, \ |
| 1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, \ |
| 3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, \ |
| 4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, \ |
| 6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, \ |
| 7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, \ |
| 7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, \ |
| 8161, 8172, 8181, 8187, 8191, 8192}; |
| //double c_cos_8192(int16_t angle) |
| int16_t c_cos_8192(int16_t angle) |
| { |
| int8_t m,n; |
| int16_t sinus; |
| angle = 90 - angle; |
| // avoid negative angles |
| if (angle < 0) |
| { |
| m = -1; |
| angle = -angle; |
| } |
| else m = +1; |
| // fold angle to interval 0 to 359 |
| angle %= 360; |
| // check quadrant |
| if (angle <= 90) n = 1; // first quadrant |
| else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant |
| else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant |
| else {angle = 360 - angle; n = -1;} //fourth quadrant |
| // get lookup value |
| sinus = sinlookup[angle]; |
| // calculate sinus value |
| return (sinus * m * n); |
| } |
| // ---------------------------------------------------------------------------------------------- |
| const int16_t arccos64[65] = {90,89,88,87,86, 85, 84, 83, 83, 82, 81, 80, 79, 78, 77, 76, |
| 75, 74, 73, 72, 71, 71, 70, 69, 68, 67, 66, 65, 64, 63, 62, |
| 61, 60, 59, 58, 57, 56, 55, 54, 53, 51, 50, 49, 48, 47, 45, |
| 44, 43, 41, 40, 39, 37, 36, 34, 32, 31, 29, 27, 25, 23, 20, |
| 18, 14, 10, 0}; |
| int16_t c_arccos2(int32_t a, int32_t b) |
| { |
| if(a>b) return(0); |
| return(arccos64[64 * a / b]); |
| } |
| /* ---------------------------------------------------------------------------------------------- |
| atan2.S |
| Author: Reiner Patommel |
| atan2.S uses CORDIC, an algorithm which was developed in 1956 by Jack Volder. |
| CORDIC can be used to calculate trigonometric, hyperbolic and linear |
| functions and is until today implemented in most pocket calculators. |
| The algorithm makes only use of simple integer arithmetic. |
| The CORDIC equations in vectoring mode for trigonometric functions are: |
| Xi+1 = Xi - Si * (Yi * 1 / 2^i) |
| Yi+1 = Yi + Si * (Xi * 1 / 2^i) |
| Zi+1 = Zi - Si * atan(1 / 2^i) |
| where Si = +1 if Yi < 0 else Si = -1 |
| The rotation angles are limited to -PI/2 and PI/2 i.e. |
| -90 degrees ... +90 degrees |
| For the calculation of atan2(x,y) we need a small pre-calculated table of |
| angles or radians with the values Tn = atan(1/2^i). |
| We rotate the vector(Xo,Yo) in steps to the x-axis i.e. we drive Y to 0 and |
| accumulate the rotated angles or radians in Z. The direction of the rotation |
| will be positive or negative depending on the sign of Y after the previous |
| rotation and the rotation angle will decrease from step to step. (The first |
| step is 45 degrees, the last step is 0.002036 degrees for n = 15). |
| After n rotations the variables will have the following values: |
| Yn = ideally 0 |
| Xn = c*sqrt(Xo^2+Yo^2) (magnitude of the vector) |
| Zn = Zo+atan(Yo/Xo) (accumulated rotation angles or radians) |
| c, the cordic gain, is the product Pn of sqrt(1+2^(-2i)) and amounts to |
| approx. 1.64676 for n = 15. |
| In order to represent X, Y and Z as integers we introduce a scaling factor Q |
| which is chosen as follows: |
| 1. We normalize Xo and Yo (starting values) to be less than or equal to 1*Q and |
| set Zo = 0 i.e. Xomax = 1*Q, Yomax = 1*Q, Zo = 0. |
| 2. With Xo = 1*Q and Yo = 1*Q, Xn will be Xnmax = Q*c*sqrt(2) = 2.329*Q |
| 3. In order to boost accuracy we only cover the rotation angles between 0 and PI/2 |
| i.e. X and Z are always > 0 and therefore can be unsigned. |
| (We do the quadrant correction afterwards based on the initial signs of x and y) |
| 4. If X is an unsigned int, Xnmax = 65536, and Q = 65536/2.329 = 28140. |
| i.e. |
| Xnmax = 65536 --> unsigned int |
| Ynmax = +/- 28140 --> signed int |
| Znmax = PI/2 * 28140 = 44202 --> unsigned int |
| The systematic error is 90/44202 = 0.002036 degrees or 0.0000355 rad. |
| Below is atan2 and atan in C: */ |
| #define getAtanVal(x) (uint16_t)pgm_read_word(&atantab[x]) |
| const uint16_t atantab[T] PROGMEM = {22101,13047,6894,3499,1756,879,440,220,110,55,27,14,7,3,2,1}; |
| int16_t my_atan2(int32_t y, int32_t x) |
| { unsigned char i; |
| uint32_t xQ; |
| int32_t yQ; |
| uint32_t angle = 0; |
| uint32_t tmp; |
| double x1, y1; |
| x1 = abs(x); |
| y1 = abs(y); |
| if (y1 == 0) { |
| if (x < 0) |
| return (180); |
| else |
| return 0; |
| } |
| if (x1 < y1) { |
| x1 /= y1; |
| y1 = 1; |
| } |
| else { |
| y1 /= x1; |
| x1 = 1; |
| } |
| xQ = SCALED(x1); |
| yQ = SCALED(y1); |
| for (i = 0; i < T-1; i++) { |
| tmp = xQ >> i; |
| if (yQ < 0) { |
| xQ -= yQ >> i; |
| yQ += tmp; |
| angle -= getAtanVal(i); |
| } |
| else { |
| xQ += yQ >> i; |
| yQ -= tmp; |
| angle += getAtanVal(i); |
| } |
| } |
| angle = RAD_TO_DEG * angle/Q; |
| if (x <= 0) |
| angle = 180 - angle; |
| if (y <= 0) |
| return(-angle); |
| return(angle); |
| } |
| Property changes: |
| Added: svn:mime-type |
| +text/plain |
| \ No newline at end of property |
| /Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/gps/mymath.h |
|---|
| 0,0 → 1,15 |
| #ifndef _MYMATH_H_ |
| #define _MYMATH_H_ |
| #include <avr/io.h> |
| #define T 16 |
| #define Q 28140 |
| #define SCALED(X) ((int32_t)((X) * Q)) |
| #define RAD_TO_DEG 57.2957795 // radians to degrees = 180 / PI |
| uint16_t sqrt32(uint32_t qzahl); |
| int16_t c_cos_8192(int16_t angle); |
| int16_t my_atan2(int32_t y, int32_t x); |
| #endif |
| Property changes: |
| Added: svn:mime-type |
| +text/plain |
| \ No newline at end of property |
| /Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/gps/. |
|---|
| Property changes: |
| Added: svn:ignore |
| +_doc |
| + |
| +Copy of gpsmouse.h |
| + |
| +_old_source |
| + |
| +Copy of gpsmouse.c |