-
Notifications
You must be signed in to change notification settings - Fork 1
/
gps_utils.c
86 lines (79 loc) · 2.57 KB
/
gps_utils.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
/**
* @file gps_utils.c
* @author Marcos Canales Mayo
* @date 2 Jun 2016
* @version 1.0
*/
#include <string.h>
#include "gps_utils.h"
#include "nmea.h"
int gps_try_get_position(struct gps_position *gps_position, int num_reads){
int i;
char read_frame[100], *caux;
struct nmea_frame nmea_frame;
if(num_reads>0){
for(i=0; i<num_reads; i++){
// error checking after getting a frame (e.g. invalid checksum)
if (gps_read(read_frame) < 0 || nmea_frame_from_str(read_frame, &nmea_frame) < 0) return -1;
// check whether it is a GGA frame
if (strcmp(nmea_frame.frame_type, "GPGGA") == 0){
// datetime of the moment the position was taken
strcpy((*gps_position).datetime, nmea_frame.data[0]);
// latitude
(*gps_position).lat_deg = strtof(nmea_frame.data[1], &caux);
(*gps_position).lat_dir = nmea_frame.data[2][0];
// longitude
(*gps_position).lon_deg = strtof(nmea_frame.data[3], &caux);
(*gps_position).lon_dir = nmea_frame.data[4][0];
return 0;
}
}
}
else{
while(1){
// error checking after getting a frame (e.g. invalid checksum)
if (gps_read(read_frame) < 0 || nmea_frame_from_str(read_frame, &nmea_frame) < 0) return -1;
// check whether it is a GGA frame
if (strcmp(nmea_frame.frame_type, "GPGGA") == 0){
// datetime of the moment the position was taken
strcpy((*gps_position).datetime, nmea_frame.data[0]);
// latitude
(*gps_position).lat_deg = strtof(nmea_frame.data[1], &caux);
(*gps_position).lat_dir = nmea_frame.data[2][0];
// longitude
(*gps_position).lon_deg = strtof(nmea_frame.data[3], &caux);
(*gps_position).lon_dir = nmea_frame.data[4][0];
return 0;
}
}
}
return -1;
}
int gps_try_get_num_satellites(int *num_satellites, int num_reads){
int i;
char read_frame[100], *caux;
struct nmea_frame nmea_frame;
if(num_reads>0){
for(i=0; i<num_reads; i++){
// error checking after getting a frame (e.g. invalid checksum)
if (gps_read(read_frame) < 0 || nmea_frame_from_str(read_frame, &nmea_frame) < 0) return -1;
// check whether it is a GSV frame
if (strcmp(nmea_frame.frame_type, "GPGSV") == 0){
(*num_satellites) = (int) strtol(nmea_frame.data[2], &caux, 10);
return 0;
}
}
}
else{
while(1){
// error checking after getting a frame (e.g. invalid checksum)
if (gps_read(read_frame) < 0 || nmea_frame_from_str(read_frame, &nmea_frame) < 0) return -1;
// check whether it is a GSV frame
if (strcmp(nmea_frame.frame_type, "GPGSV") == 0){
(*num_satellites) = (int) strtol(nmea_frame.data[2], &caux, 10);
return 0;
}
}
}
return -1;
}