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 "main.h"
#include "tracking.h"
#include <avr/pgmspace.h>
#include "fifo.h"
#include "bluetooth.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};
HomePos_t MK_pos
; // Home position of station
//GPS_Pos_t currentPos; // Current position of flying object
int8_t NMEAsatsInUse
; // Number of satelites currently in use
int32_t NMEAlatitude
, NMEAlongitude
;
char NMEATime
[9] = "GP:Ti:me";
//char NMEADate [6];
// 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,,
// Skip 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)
if (*strtok(0, ",") != '0')// Qualität
{
// Satellites in use
NMEAsatsInUse
= atoi(strtok(0, ",")); //Anzahl Sats
// // Skip dilution
// strtok(0, ","); //Dilution
// // Altitude
// currentPos.Altitude = floatStrToInt(strtok(0, ","), 3);
// currentPos.Latitude = latitude;
// currentPos.Longitude = longitude;
//
// if ((coldstart) && (satsInUse > 5)) {
// // First position after reboot (or change of mode) will be the home position (facing north)
// MK_pos.Home_Lon = (double)currentPos.Longitude / 10000000.0;
// MK_pos.Home_Lat = (double)currentPos.Latitude / 10000000.0;
// MK_pos.Home_Lon7 = currentPos.Longitude;
// MK_pos.Home_Lat7 = currentPos.Latitude;
// MK_pos.Home_Alt = currentPos.Altitude;
// 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 (rx_ready
== 1 && rx_len
> 0) {
// Calculate checksum
for (i
= 1; i
< rx_len
&& rx_buffer
[i
] != '*'; i
++) {
tmpCRC
^= rx_buffer
[i
];
}
if (rx_len
>= i
+ 3) {
crc
= hexDigitToInt
(rx_buffer
[i
+ 1]) << 4 | hexDigitToInt
(rx_buffer
[i
+ 2]);
if (crc
== tmpCRC
) {
rx_buffer
[i
] = 0;
strcpy(data_decode
, &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;
rx_ready
= 0; // Unlock buffer, next NMEA string can be received
return ret
;
}
#endif