| 0,0 → 1,694 |
|---|
| /* |
| * tracking.c |
| * |
| * Created on: 13.02.2012 |
| * Author: cebra |
| */ |
| |
| #include "../cpu.h" |
| #include <string.h> |
| #include <util/delay.h> |
| #include <avr/interrupt.h> |
| #include <stdlib.h> |
| #include <math.h> |
| #include "../main.h" |
| #include "../tracking/tracking.h" |
| #include "../tracking/ng_servo.h" |
| #include <avr/pgmspace.h> |
| #include "../bluetooth/fifo.h" |
| #include "../bluetooth/bluetooth.h" |
| #include "../lcd/lcd.h" |
| |
| #include "../mk-data-structs.h" |
| #include "tools.h" |
| #include "../messages.h" |
| #include "../lcd/lcd.h" |
| #include "../eeprom/eeprom.h" |
| #include "../timer/timer.h" |
| #include "../uart/uart1.h" |
| #include "../uart/usart.h" |
| #include "../osd/osd.h" |
| #include "../tracking/mymath.h" |
| #include "../setup/setup.h" |
| |
| |
| |
| |
| #ifdef HWVERSION3_9 |
| |
| //#define MAX_POWER 10 |
| //#define getPower(x) (int32_t)pgm_read_dword(&powers[x]) |
| //const int32_t PROGMEM powers[MAX_POWER] = {1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000}; |
| #define DLEFT 0 |
| #define DRIGHT 1 |
| #define DEG_TO_RAD 0.0174533 // degrees to radians = PI / 180 |
| #define RAD_TO_DEG 57.2957795 // radians to degrees = 180 / PI |
| #define AltFaktor 22.5 |
| #define TIMEOUT 200 // 2 sec |
| |
| NaviData_t *naviData; |
| |
| mk_param_struct_t *mk_param_struct; |
| |
| HomePos_t MK_pos; // Home position of station |
| GPS_Pos_t currentPos; // Current position of flying object |
| uint8_t NMEAsatsInUse; // Number of satelites currently in use BT-Mouse |
| //uint8_t MKsatsInUse; // Number of satelites currently in use Mikrokopter |
| int32_t NMEAlatitude, NMEAlongitude; // Longitude, Latitude |
| int16_t NMEAaltitude; // Höhe in Meter |
| uint8_t posfix; // GPS Fix, 0 = Fix not available or invalid,1 = GPS SPS Mode, fix valid, |
| // 2 = Differential GPS, SPS Mode, fix valid, 6 = Dead Reckoning Mode, fix valid |
| int16_t HDOP; // Horizontal Dilution of Precision, 1.1 -xx.x, niederiger = besser |
| |
| |
| uint8_t tracking = TRACKING_MIN; |
| uint8_t track_hyst = TRACKING_HYSTERESE; |
| uint8_t track_tx =0; |
| uint8_t coldstart; // Flag erstmaliger MK-Start(Motore) nur nach GPS-Fix |
| geo_t geo; |
| int16_t anglePan, angleTilt; |
| |
| |
| char NMEAtime[9] = "GP:Ti:me"; |
| //char NMEADate [6]; |
| |
| //// 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) |
| { |
| lcd_puts_at (0, 3, my_itoa(home->Home_Lat, 10, 7, 7), 0); |
| lcd_puts_at (11, 3, my_itoa(home->Home_Lon, 10, 7, 7), 0); |
| lcd_puts_at (0, 4, my_itoa(pos->Latitude, 10, 7, 7), 0); |
| lcd_puts_at (11, 4, my_itoa(pos->Longitude, 10, 7, 7), 0); |
| } |
| // lcd_printp_at (0, 3, PSTR("H"), 0); |
| // lcd_printp_at (0, 4, PSTR("M"), 0); |
| |
| // 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); |
| lcd_puts_at (5, 5, my_itoa((uint32_t)geo.bearing, 3, 0, 0), 0); |
| lcd_printp_at (8, 5, PSTR("\x1e"), 0); |
| lcd_printp_at (9, 5, PSTR("Dist:"), 0); |
| lcd_puts_at (15, 5, my_itoa((uint32_t)geo.distance, 3, 1, 1), 0); |
| lcd_printp_at (20, 5, PSTR("m"), 0); |
| } |
| |
| |
| return(geo); |
| } |
| |
| |
| void do_tracking(void) |
| { static uint8_t hysteresis = 0; |
| |
| geo = calc_geo(&MK_pos, ¤tPos); |
| angleTilt = my_atan2((currentPos.Altitude - MK_pos.Home_Alt) / 100, geo.distance); |
| if (geo.distance < 40 || (geo.distance < 60 && hysteresis)) { // < 4m ==> Pan-Servo in Mittelstellung. Hysterese bis 6m, damit Servo im Grenzbereich nicht wild rumschl�gt |
| geo.bearing = MK_pos.direction; |
| if (currentPos.Altitude - MK_pos.Home_Alt < 4000) angleTilt = 0; // man fliegt nicht direkt �ber Kopf |
| hysteresis = 1; |
| } |
| else { |
| hysteresis = 0; |
| } |
| |
| // egal wo der Übergangspunkt 359, 360, 1grd ist => Winkelübergangspunkt auf 0 bzw. 180grd des Servos bringen |
| // 360 grd negative Winkelwerte als positive |
| anglePan = (geo.bearing + 450 - MK_pos.direction) % 360; // 450 = 360 + 90 |
| |
| if (angleTilt < 0) angleTilt = 0; |
| if (angleTilt > 180) angleTilt = 180; |
| |
| if (anglePan >= 180) { // zwecks 360grd-Abdeckung flipt Pan-/Tilt-Servo |
| anglePan = anglePan - 180; |
| angleTilt = 180 - angleTilt; |
| } |
| |
| servoSetAngle(0, anglePan); |
| servoSetAngle(1, angleTilt); |
| if (!CheckGPS) |
| { |
| lcd_printp_at (0, 6, PSTR("Pan :"), 0); |
| write_ndigit_number_u (6, 6, anglePan, 3, 1,0); |
| lcd_printp_at (11, 6, PSTR("Tilt:"), 0); |
| write_ndigit_number_u (17, 6, angleTilt, 3, 1,0); |
| } |
| |
| // write_ndigit_number_u (0, 5, (uint16_t)(currentPos.Altitude/10000000), 2, 0,0); |
| //// lcd_printp_at (4, 4, PSTR("."), 0); |
| // write_ndigit_number_u (2, 5, (uint16_t)((currentPos.Altitude/1000) % 10000), 4, 1,0); |
| // write_ndigit_number_u (6, 5, (uint16_t)((currentPos.Altitude/10) % 100), 2, 1,0); |
| // |
| // write_ndigit_number_u (10, 5, (uint16_t)(MK_pos.Home_Alt/10000000), 2, 0,0); |
| //// lcd_printp_at (4, 4, PSTR("."), 0); |
| // write_ndigit_number_u (12, 5, (uint16_t)((MK_pos.Home_Alt/1000) % 10000), 4, 1,0); |
| // write_ndigit_number_u (16, 5, (uint16_t)((MK_pos.Home_Alt/10) % 100), 2, 1,0); |
| |
| |
| } |
| //******************************************************************************************************* |
| uint8_t PKT_trackingBT(void) // Tracking mit NMEA-Daten von BT-Maus |
| |
| { |
| |
| uint8_t BT_WhasOn = 0; |
| uint8_t BT_status; |
| uint8_t flag; |
| uint8_t tmp_dat; |
| coldstart =1; |
| |
| { |
| // lcd_printp_at(0,1, PSTR("try NMEA data from:"), 0); |
| lcd_puts_at (0, 1,Config.gps_UsedDevName, 0); |
| set_BTOn(); |
| BT_WhasOn = true; |
| if (Config.BTIsSlave==true) |
| { |
| bt_downlink_init(); |
| } |
| lcd_printp_at (18, 1, PSTR(" ?? "), 0); |
| BT_status = bt_connect(Config.gps_UsedMac); |
| if (BT_status==true) |
| { |
| lcd_printp_at (18, 1, PSTR(" OK "), 0); |
| receiveNMEA = true; |
| } |
| else lcd_printp_at (17, 1, PSTR("FAIL"), 2); |
| |
| if (receiveNMEA==true) |
| { |
| lcd_printp_at (0, 2, PSTR("S Latitude Longitude"), 2); |
| lcd_cls_line (0,1,20); |
| lcd_printp_at (0, 3, PSTR("H"), 0); |
| lcd_printp_at (0, 4, PSTR("M"), 0); |
| bt_rx_ready = 0; |
| |
| SwitchToNC(); |
| mode = 'O'; |
| // disable debug... |
| // RS232_request_mk_data (0, 'd', 0); |
| tmp_dat = 0; |
| SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
| |
| // request OSD Data from NC every 100ms |
| // RS232_request_mk_data (1, 'o', 100); |
| tmp_dat = 10; |
| // OSD_active = true; // benötigt für Navidata Ausgabe an SV2 |
| SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
| |
| flag = 0; |
| timer = TIMEOUT; |
| abo_timer = ABO_TIMEOUT; |
| |
| |
| do |
| { |
| // bt_rx_ready = 0; |
| if (!bt_receiveNMEA()) |
| break; |
| if (rxd_buffer_locked) |
| { |
| timer = TIMEOUT; |
| Decode64 (); |
| naviData = (NaviData_t *) pRxData; |
| |
| //#ifdef DEBUG |
| // debug_pgm(PSTR("setup Tracking_NMEA")); |
| //#endif |
| currentPos = naviData->CurrentPosition; |
| // currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5; |
| |
| // uint32_t lat = currentPos.Latitude; |
| // uint32_t lon = currentPos.Longitude; |
| |
| // write_ndigit_number_u (2, 4, (uint16_t)(lat/10000000), 2, 0,0); |
| // lcd_printp_at (4, 4, PSTR("."), 0); |
| // write_ndigit_number_u (5, 4, (uint16_t)((lat/1000) % 10000), 4, 1,0); |
| // write_ndigit_number_u (9, 4, (uint16_t)((lat/10) % 100), 2, 1,0); |
| // |
| |
| // write_ndigit_number_u (12, 4, (uint16_t)(lon/10000000), 2, 0,0); |
| // lcd_printp_at (14, 4, PSTR("."), 0); |
| // write_ndigit_number_u (15, 4, (uint16_t)((lon/1000) % 10000), 4, 1,0); |
| // write_ndigit_number_u (19, 4, (uint16_t)((lon/10) % 100),2, 1,0); |
| |
| Tracking_NMEA(); |
| |
| |
| // write_ndigit_number_u (2, 3, (uint16_t)(NMEAlatitude/10000000), 2, 0,0); |
| // lcd_printp_at (4, 3, PSTR("."), 0); |
| // write_ndigit_number_u (5, 3, (uint16_t)((NMEAlatitude/1000) % 10000), 4, 1,0); |
| // write_ndigit_number_u (9, 3, (uint16_t)((NMEAlatitude/10) % 100), 2, 1,0); |
| // |
| // |
| // write_ndigit_number_u (12, 3, (uint16_t)(NMEAlongitude/10000000), 2, 0,0); |
| // lcd_printp_at (14, 3, PSTR("."), 0); |
| // write_ndigit_number_u (15, 3, (uint16_t)((NMEAlongitude/1000) % 10000), 4, 1,0); |
| // write_ndigit_number_u (19, 3, (uint16_t)((NMEAlongitude/10) % 100), 2, 1,0); |
| |
| // lcd_printp_at (0, 2, PSTR("GPS Time: "), 0); |
| if (!CheckGPS) |
| { |
| lcd_puts_at (13, 0, NMEAtime, 2); |
| lcd_printp_at (16, 1, PSTR("Sat:"), 0); |
| write_ndigit_number_u (19, 1, NMEAsatsInUse, 2, 1,0); |
| lcd_printp_at (0, 1, PSTR("Fix:"), 0); |
| write_ndigit_number_u (4, 1, posfix, 1, 1,0); |
| lcd_printp_at (6, 1, PSTR("HDOP:"), 0); |
| write_ndigit_number_u_10th (11, 1, HDOP, 3, 0,0); |
| } |
| |
| |
| rxd_buffer_locked = FALSE; |
| |
| if (!abo_timer) |
| { // renew abo every 3 sec |
| // request OSD Data from NC every 100ms |
| // RS232_request_mk_data (1, 'o', 100); |
| tmp_dat = 10; |
| SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
| |
| abo_timer = ABO_TIMEOUT; |
| } |
| }//if (rxd_buffer_locked) |
| |
| if (!timer) |
| { |
| OSD_Timeout(flag); |
| flag = 0; |
| error = 1; |
| |
| } |
| |
| |
| } //end do |
| |
| |
| while (!get_key_press (1 << KEY_ENTER) || !receiveNMEA==true || error ==1); |
| // while (!get_key_press (1 << KEY_ENTER)); |
| |
| lcd_cls_line(0,1,21); |
| lcd_cls_line(0,2,21); |
| lcd_cls_line(0,3,21); |
| lcd_cls_line(0,4,21); |
| lcd_cls_line(0,5,21); |
| lcd_cls_line(0,6,21); |
| if (!receiveNMEA) lcd_printp_at (0, 2, PSTR("lost BT data"), 0); |
| // if (error ==1) lcd_printp_at (0, 2, PSTR("lost Wi.232 data"), 0); |
| lcd_printp_at (0, 3, PSTR("GPS trennen"), 0); |
| } |
| else |
| { |
| lcd_printp_at (0, 4, PSTR("Error at connecting"), 0); |
| lcd_printp_at (0, 5, PSTR("switch on BT Mouse!!"), 0); |
| while (!get_key_press (1 << KEY_ENTER)); |
| } |
| receiveNMEA = false; |
| if (!bt_disconnect()) lcd_printp_at (0, 3, PSTR("Fehler beim Trennen"), 0); |
| |
| set_BTOff(); |
| return true; |
| } |
| } |
| |
| //******************************************************************************************************* |
| |
| uint8_t PKT_trackingMK(void) // Tracking mit GPS-Daten vom Mikrokopter |
| |
| { |
| |
| // uint8_t BT_WhasOn = 0; |
| // uint8_t BT_status; |
| uint8_t GPSfix=0; |
| uint8_t tmp_dat; |
| uint8_t toggletimer=0; |
| coldstart = true; |
| |
| |
| lcd_printp_at (0, 2, PSTR("S Latitude Longitude"), 2); |
| |
| lcd_cls_line (0,1,20); |
| // lcd_printp_at (0, 3, PSTR("H"), 0); |
| // lcd_printp_at (0, 4, PSTR("M"), 0); |
| |
| SwitchToNC(); |
| mode = 'O'; |
| // disable debug... |
| // RS232_request_mk_data (0, 'd', 0); |
| tmp_dat = 0; |
| SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
| |
| // request OSD Data from NC every 100ms |
| // RS232_request_mk_data (1, 'o', 100); |
| tmp_dat = 10; |
| SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
| timer = TIMEOUT; |
| abo_timer = ABO_TIMEOUT; |
| error = 0; |
| |
| do |
| { |
| |
| if (rxd_buffer_locked) |
| { |
| timer = TIMEOUT; |
| Decode64 (); |
| naviData = (NaviData_t *) pRxData; |
| //OSD_Screen_Element (18, 1, OSD_SATS_IN_USE,1); |
| //if (GPSfix == true) OSD_Screen_Element (0, 1, OSD_STATUS_FLAGS,1); |
| OSD_Element_SatsInUse( 18, 1, 1); |
| if (GPSfix == true) OSD_Element_StatusFlags( 0, 1); |
| |
| if (!(naviData->NCFlags & NC_FLAG_GPS_OK)) |
| { |
| toggletimer++; |
| if (toggletimer == 50) toggletimer = 0; |
| if (toggletimer == 25) lcd_printp_at(0,1, PSTR("Whait for GPS Fix "), 2); |
| if (toggletimer == 1) lcd_printp_at(0,1, PSTR("Whait for GPS Fix "), 0); |
| |
| rxd_buffer_locked = false; |
| GPSfix = false; |
| |
| } |
| else GPSfix = true; |
| |
| if (GPSfix) |
| { |
| if (coldstart) |
| { |
| |
| // erst nach Neustart NGVideo und beim Motorstart werden Daten vom MK übernommen |
| if (naviData->FCStatusFlags & FC_FLAG_MOTOR_START) { |
| MK_pos.Home_Lon = naviData->HomePosition.Longitude; |
| MK_pos.Home_Lat = naviData->HomePosition.Latitude; |
| MK_pos.Home_Alt = naviData->HomePosition.Altitude; |
| MK_pos.direction = naviData->CompassHeading; |
| coldstart = false; |
| rxd_buffer_locked = false; |
| lcd_printp_at(0,1, PSTR(" "), 0); |
| |
| } |
| else |
| { |
| lcd_printp_at(0,1, PSTR("GPS ok, start ok "), 0); |
| rxd_buffer_locked = false; |
| } |
| |
| } |
| else |
| { //run |
| |
| |
| currentPos = naviData->CurrentPosition; |
| currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5; |
| do_tracking(); |
| // lcd_puts_at (13, 0, NMEAtime, 2); |
| // lcd_printp_at (16, 1, PSTR("Sat:"), 0); |
| // write_ndigit_number_u (19, 1, NMEAsatsInUse, 2, 1,0); |
| // lcd_printp_at (0, 1, PSTR("Fix:"), 0); |
| // write_ndigit_number_u (4, 1, posfix, 1, 1,0); |
| // lcd_printp_at (6, 1, PSTR("HDOP:"), 0); |
| // write_ndigit_number_u_10th (11, 1, HDOP, 3, 0,0); |
| rxd_buffer_locked = FALSE; |
| |
| |
| } // run |
| } |
| if (!abo_timer) |
| { // renew abo every 3 sec |
| // request OSD Data from NC every 100ms |
| // RS232_request_mk_data (1, 'o', 100); |
| tmp_dat = 10; |
| SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
| |
| abo_timer = ABO_TIMEOUT; |
| } |
| |
| } //rx_buffer_locked |
| |
| if (!timer) |
| { |
| OSD_Timeout(1); |
| error = 1; |
| } |
| |
| |
| |
| } //end do |
| |
| |
| while ((!get_key_press (1 << KEY_ENTER)) && (error ==0)); |
| |
| |
| lcd_cls_line(0,1,21); |
| lcd_cls_line(0,2,21); |
| lcd_cls_line(0,3,21); |
| lcd_cls_line(0,4,21); |
| lcd_cls_line(0,5,21); |
| lcd_cls_line(0,6,21); |
| |
| if (error ==1) |
| { |
| lcd_printp_at (0, 2, PSTR("lost Wi.232 data"), 0); |
| _delay_ms(2000); |
| } |
| return true; |
| } |
| |
| //******************************************************************************************************* |
| |
| void PKT_tracking(void) |
| { |
| |
| |
| |
| get_key_press(KEY_ALL); |
| lcd_cls (); |
| if (Config.gps_UsedGPSMouse==GPS_Bluetoothmouse1) lcd_printp_at(0,0, PSTR("Tracking Bluetooth "), 2); |
| if (Config.gps_UsedGPSMouse==GPS_Mikrokopter) lcd_printp_at(0,0, PSTR(" Tracking Mikrokopter"), 2); |
| lcd_printp_at (16, 7, PSTR("Ende"), 0); |
| |
| |
| if (Config.gps_UsedGPSMouse==GPS_Bluetoothmouse1) PKT_trackingBT(); |
| if (Config.gps_UsedGPSMouse==GPS_Mikrokopter) PKT_trackingMK(); |
| get_key_press(KEY_ALL); |
| |
| } |
| |
| |
| |
| |
| |
| //// Trying to avoid floating point maths here. Converts a floating point string to an integer with a smaller unit |
| //// i.e. floatStrToInt("4.5", 2) = 4.5 * 1E2 = 450 |
| //int32_t floatStrToInt(const char *s, int32_t power1) |
| //{ char *endPtr; |
| // int32_t v = strtol(s, &endPtr, 10); |
| // |
| // if (*endPtr == '.') { |
| // for (s = endPtr + 1; *s && power1; s++) { |
| // v = v * 10 + (*s - '0'); |
| // --power1; |
| // } |
| // } |
| // if (power1) { |
| // // Table to avoid multiple multiplications |
| // v = v * getPower(power1); |
| // } |
| // return v; |
| //} |
| |
| // NMEA latitudes are in the form ddmm.mmmmm, we want an integer in 1E-7 degree steps |
| int32_t getLatitude(const char *s, const char *NS) |
| { int32_t deg = (s[0] - '0') * 10 + s[1] - '0'; // First 2 chars are full degrees |
| int32_t min = floatStrToInt(&s[2], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps |
| |
| deg = deg * 10000000 + min; |
| if (*NS == 'S') deg = -deg; |
| return deg; |
| } |
| |
| |
| // NMEA longitudes are in the form dddmm.mmmmm, we want an integer in 1E-7 degree steps |
| int32_t getLongitude(const char *s, const char *WE) |
| { int32_t deg = ((s[0] - '0') * 10 + s[1] - '0') * 10 + s[2] - '0'; // First 3 chars are full degrees |
| int32_t min = floatStrToInt(&s[3], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps |
| |
| deg = deg * 10000000 + min; |
| if (*WE == 'W') deg = -deg; |
| return deg; |
| } |
| |
| void getNMEATime( const char *s) |
| { |
| uint8_t sem = 0; |
| uint8_t i; |
| |
| for ( i=0;i < 6; i++ ) |
| { |
| NMEAtime[sem++] = s[i]; |
| if (i==1 || i==3) NMEAtime[sem++] = ':'; |
| |
| } |
| NMEAtime[sem] = '\0'; |
| } |
| |
| |
| |
| |
| //$GPGGA,191410.000,4735.5634,N,00739.3538,E,1,04,4.4,351.5,M,48.0,M,,*45 |
| // ^ ^ ^ ^ ^ ^ ^ ^ |
| // | | | | | | | | |
| // | | | | | | | Höhe Geoid minus |
| // | | | | | | | Höhe Ellipsoid (WGS84) |
| // | | | | | | | in Metern (48.0,M) |
| // | | | | | | | |
| // | | | | | | Höhe über Meer (über Geoid)in Metern (351.5,M) |
| // | | | | | | |
| // | | | | | HDOP (horizontal dilution |
| // | | | | | of precision) Genauigkeit |
| // | | | | | |
| // | | | | Anzahl der erfassten Satelliten |
| // | | | | |
| // | | | Qualität der Messung |
| // | | | (0 = ungültig) |
| // | | | (1 = GPS) |
| // | | | (2 = DGPS) |
| // | | | (6 = geschätzt nur NMEA-0183 2.3) |
| // | | | |
| // | | Längengrad |
| // | | |
| // | Breitengrad |
| // | |
| // Uhrzeit |
| |
| void Tracking_NMEA(void) |
| { |
| char *token; |
| |
| |
| |
| if (decodeNMEA()) { |
| token = strtok((char*)data_decode, ","); |
| if (!strcmp(token, "GPGGA")) |
| { |
| // $GPGGA,220613.400,4843.5080,N,00922.9583,E,1,7,2.23,287.1,M,48.0,M,, |
| // Time |
| getNMEATime(strtok(0, ".")); //Zeit |
| |
| strtok(0, ","); // Skip Rest von der Zeit |
| // Latitude |
| NMEAlatitude = getLatitude(strtok(0, ","), strtok(0, ",")); //N |
| // Longitude |
| NMEAlongitude = getLongitude(strtok(0, ","), strtok(0, ","));//E |
| // Signal valid? (Position Fix Indicator) |
| posfix = atoi(strtok(0, ",")); // Qualität |
| // Satellites in use |
| NMEAsatsInUse = atoi(strtok(0, ",")); //Anzahl Sats |
| // Dilition, best = 0.0 |
| HDOP = floatStrToInt(strtok(0, ","),1); //Dilution |
| |
| // // Altitude |
| NMEAaltitude = floatStrToInt(strtok(0, ","), 1); |
| // currentPos.Altitude = altitude; |
| // currentPos.Latitude = latitude; |
| // currentPos.Longitude = longitude; |
| |
| //TODO: erstmal test if ((coldstart) && (satsInUse > 5)) { |
| // First position after reboot (or change of mode) will be the home position (facing north) |
| MK_pos.Home_Lon = NMEAlongitude; |
| MK_pos.Home_Lat = NMEAlatitude; |
| |
| MK_pos.Home_Alt = NMEAaltitude; |
| MK_pos.direction = 0; |
| // coldstart = 0; |
| // Double_Beep(DBEEPNMEAFIX, DBEEPMEAFIXP); |
| |
| do_tracking(); |
| // } |
| } |
| } |
| // Displ_GPS(); // letzte empfangene Daten auch bei ausgeschalteter NMEA sichtbar |
| } |
| |
| |
| uint8_t hexDigitToInt(uint8_t digit) |
| { |
| if (digit >= '0' && digit <= '9') return digit - '0'; |
| if (digit >= 'a' && digit <= 'f') return digit - 'a' + 10; |
| if (digit >= 'A' && digit <= 'F') return digit - 'A' + 10; |
| return 0; |
| } |
| |
| |
| uint8_t decodeNMEA(void) |
| { |
| uint8_t ret = 0; |
| uint8_t crc; |
| uint8_t tmpCRC = 0; |
| uint8_t i; |
| |
| if (bt_rx_ready == 1 && bt_rx_len > 0) { |
| // Calculate checksum |
| for (i = 1; i < bt_rx_len && bt_rx_buffer[i] != '*'; i++) { |
| tmpCRC ^= bt_rx_buffer[i]; |
| } |
| if (bt_rx_len >= i + 3) { |
| crc = hexDigitToInt(bt_rx_buffer[i + 1]) << 4 | hexDigitToInt(bt_rx_buffer[i + 2]); |
| if (crc == tmpCRC) { |
| bt_rx_buffer[i] = 0; |
| strcpy(data_decode, &bt_rx_buffer[1]); // Data without $, crc |
| ret = 1; |
| // wi232RX = 1; // So antenna-symbol will blink |
| // cli(); |
| // rx_timeout = 0; // Got valid data, reset counter |
| // sei(); |
| } |
| } |
| } |
| // if (rx_timeout < RX_TIME_OLD) wi232RX = 1; |
| bt_rx_ready = 0; // Unlock buffer, next NMEA string can be received |
| return ret; |
| } |
| #endif |
| Property changes: |
|---|
| Added: svn:mime-type |
|---|
| +text/plain |
|---|
| \ No newline at end of property |
|---|