Blame |
Last modification |
View Log
| RSS feed
/*
* 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