73 lines
2.2 KiB
C++
73 lines
2.2 KiB
C++
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
#include <arpa/inet.h>
|
|
#include <sys/socket.h>
|
|
#include <unistd.h>
|
|
|
|
#define PORT 5005
|
|
#define BUF_SIZE 512
|
|
|
|
double nmea_to_decimal(const char *nmea, char direction) {
|
|
double val = atof(nmea);
|
|
int deg = (int)(val / 100);
|
|
double min = val - deg * 100;
|
|
double dec = deg + min / 60.0;
|
|
if (direction == 'S' || direction == 'W') dec = -dec;
|
|
return dec;
|
|
}
|
|
|
|
int main() {
|
|
int sockfd;
|
|
struct sockaddr_in server_addr, client_addr;
|
|
char buffer[BUF_SIZE];
|
|
|
|
sockfd = socket(AF_INET, SOCK_DGRAM, 0);
|
|
if (sockfd < 0) { perror("socket failed"); return 1; }
|
|
|
|
memset(&server_addr, 0, sizeof(server_addr));
|
|
server_addr.sin_family = AF_INET;
|
|
server_addr.sin_addr.s_addr = INADDR_ANY;
|
|
server_addr.sin_port = htons(PORT);
|
|
|
|
if (bind(sockfd, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) {
|
|
perror("bind failed"); return 1;
|
|
}
|
|
|
|
printf("Listening on UDP port %d...\n", PORT);
|
|
|
|
while (1) {
|
|
socklen_t addr_len = sizeof(client_addr);
|
|
int n = recvfrom(sockfd, buffer, BUF_SIZE - 1, 0, (struct sockaddr *)&client_addr, &addr_len);
|
|
if (n < 0) { perror("recvfrom failed"); break; }
|
|
buffer[n] = '\0';
|
|
|
|
char *line = strtok(buffer, "\n");
|
|
while (line != NULL) {
|
|
if (strncmp(line, "$GPGGA", 6) == 0) {
|
|
char *tokens[15];
|
|
char *ptr = strtok(line, ",");
|
|
int i = 0;
|
|
while (ptr != NULL && i < 15) {
|
|
tokens[i++] = ptr;
|
|
ptr = strtok(NULL, ",");
|
|
}
|
|
|
|
if (i >= 7 && atoi(tokens[6]) > 0) { // fix quality > 0
|
|
double lat = nmea_to_decimal(tokens[2], tokens[3][0]);
|
|
double lon = nmea_to_decimal(tokens[4], tokens[5][0]);
|
|
double alt = atof(tokens[9]);
|
|
printf("Latitude: %.6f | Longitude: %.6f | Altitude: %.2f m | Fix: %s\n",
|
|
lat, lon, alt, tokens[6]);
|
|
} else {
|
|
printf("No valid fix yet.\n");
|
|
}
|
|
}
|
|
line = strtok(NULL, "\n");
|
|
}
|
|
}
|
|
|
|
close(sockfd);
|
|
return 0;
|
|
}
|