-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathgps.c
327 lines (304 loc) · 9.91 KB
/
gps.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
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
#include <msp430.h>
#include <inttypes.h>
#include "fix.h"
#include "hw.h"
#include "uart.h"
void gps_startup_delay(void);
/*
* gps_receive_ack
*
* waits for transmission of an ACK/NAK message from the GPS.
*
* returns 1 if ACK was received, 0 if NAK was received
*
*/
uint8_t gps_receive_ack(uint8_t class_id, uint8_t msg_id) {
int match_count = 0;
int msg_ack = 0;
char rx_byte;
char ack[] = {0xB5, 0x62, 0x05, 0x01, 0x02, 0x00, 0x00, 0x00};
char nak[] = {0xB5, 0x62, 0x05, 0x00, 0x02, 0x00, 0x00, 0x00};
ack[6] = class_id;
nak[6] = class_id;
ack[7] = msg_id;
nak[7] = msg_id;
uart_flush_rx();
/* runs until ACK/NAK packet is received, possibly add a timeout.
* can crash if a message ACK is missed (watchdog resets */
while(1) {
rx_byte = uart_getc();
if (rx_byte == ack[match_count] || rx_byte == nak[match_count]) {
if (match_count == 3) { /* test ACK/NAK byte */
if (rx_byte == ack[match_count]) {
msg_ack = 1;
} else {
msg_ack = 0;
}
}
if (match_count == 7) {
return msg_ack;
}
match_count++;
} else {
match_count = 0;
}
}
}
/*
* gps_receive_payload
*
* retrieves the payload of a packet with a given class and message-id with the retrieved length.
* the caller has to ensure suitable buffer length!
*
* returns the length of the payload
*
*/
uint16_t gps_receive_payload(uint8_t class_id, uint8_t msg_id, unsigned char *payload) {
uint8_t rx_byte;
enum {UBX_A, UBX_B, CLASSID, MSGID, LEN_A, LEN_B, PAYLOAD} state = UBX_A;
uint16_t payload_cnt = 0;
uint16_t payload_len = 0;
uart_flush_rx();
while(1) {
rx_byte = uart_getc();
switch (state) {
case UBX_A:
if (rx_byte == 0xB5) state = UBX_B;
else state = UBX_A;
break;
case UBX_B:
if (rx_byte == 0x62) state = CLASSID;
else if (rx_byte == 0xB5) state = UBX_B;
else state = UBX_A;
break;
case CLASSID:
if (rx_byte == class_id)state = MSGID;
else state = UBX_A;
break;
case MSGID:
if (rx_byte == msg_id) state = LEN_A;
else state = UBX_A;
break;
case LEN_A:
payload_len = rx_byte;
state = LEN_B;
break;
case LEN_B:
payload_len |= ((uint16_t)rx_byte << 8);
state = PAYLOAD;
break;
case PAYLOAD:
payload[payload_cnt] = rx_byte;
payload_cnt++;
if (payload_cnt == payload_len)
return payload_len;
break;
default:
state = UBX_A;
}
}
}
/*
* gps_get_fix
*
* retrieves a GPS fix from the module. if validity flag is not set, date/time and position/altitude are
* assumed not to be reliable!
*
* argument is call by reference to avoid large stack allocations
*
*/
void gps_get_fix(struct gps_fix *fix) {
static uint8_t response[92]; /* PVT response length is 92 bytes */
char pvt[] = {0xB5, 0x62, 0x01, 0x07, 0x00, 0x00, 0x08, 0x19};
int32_t alt_tmp;
/* request position */
uart_puts(pvt, sizeof(pvt));
gps_receive_payload(0x01, 0x07, response);
fix->num_svs = response[23];
fix->type = response[20];
fix->year = response[4] + (response[5] << 8);
fix->month = response[6];
fix->day = response[7];
fix->hour = response[8];
fix->min = response[9];
fix->sec = response[10];
fix->lat = (int32_t) (
(uint32_t)(response[28]) + ((uint32_t)(response[29]) << 8) + ((uint32_t)(response[30]) << 16) + ((uint32_t)(response[31]) << 24)
);
fix->lon = (int32_t) (
(uint32_t)(response[24]) + ((uint32_t)(response[25]) << 8) + ((uint32_t)(response[26]) << 16) + ((uint32_t)(response[27]) << 24)
);
alt_tmp = (((int32_t)
((uint32_t)(response[36]) + ((uint32_t)(response[37]) << 8) + ((uint32_t)(response[38]) << 16) + ((uint32_t)(response[39]) << 24))
) / 1000);
if (alt_tmp <= 0) {
fix->alt = 1;
} else if (alt_tmp > 50000) {
fix->alt = 50000;
} else {
fix->alt = (uint16_t) alt_tmp;
}
}
/*
* gps_disable_nmea_output
*
* disables all NMEA messages to be output from the GPS.
* even though the parser can cope with NMEA messages and ignores them, it
* may save power to disable them completely.
*
* returns if ACKed by GPS
*
*/
uint8_t gps_disable_nmea_output(void) {
char nonmea[] = {
0xB5, 0x62, 0x06, 0x00, 20, 0x00, /* UBX-CFG-PRT */
0x01, 0x00, 0x00, 0x00, /* UART1, reserved, no TX ready */
0xe0, 0x08, 0x00, 0x00, /* UART mode (8N1) */
0x80, 0x25, 0x00, 0x00, /* UART baud rate (9600) */
0x01, 0x00, /* input protocols (uBx only) */
0x01, 0x00, /* output protocols (uBx only) */
0x00, 0x00, /* flags */
0x00, 0x00, /* reserved */
0xaa, 0x79 /* checksum */
};
uart_puts(nonmea, sizeof(nonmea));
//FIXME: sometimes, the ublox ACK is scrambled directly after power up...
// however, it accepts the message and no harm is done.
//return gps_receive_ack(0x06, 0x00);
return 0;
}
/*
* gps_set_gps_only
*
* tells the uBlox to only use the GPS satellites
*
* returns if ACKed by GPS
*
*/
uint8_t gps_set_gps_only(void) {
char gpsonly[] = {
0xB5, 0x62, 0x06, 0x3E, 36, 0x00, /* UBX-CFG-GNSS */
0x00, 32, 32, 4, /* use 32 channels, 4 configs following */
0x00, 16, 32, 0, 0x01, 0x00, 0x00, 0x00, /* GPS enable, all channels */
0x03, 0, 0, 0, 0x00, 0x00, 0x00, 0x00, /* BeiDou disable, 0 channels */
0x05, 0, 0, 0, 0x00, 0x00, 0x00, 0x00, /* QZSS disable, 0 channels */
0x06, 0, 0, 0, 0x00, 0x00, 0x00, 0x00, /* GLONASS disable, 0 channels */
0xeb, 0x72 /* checksum */
};
uart_puts(gpsonly, sizeof(gpsonly));
return gps_receive_ack(0x06, 0x3E);
}
/*
* gps_set_airborne_model
*
* tells the GPS to use the airborne positioning model. Should be used to
* get stable lock up to 50km altitude
*
* working uBlox MAX-M8Q
*
* returns if ACKed by GPS
*
*/
uint8_t gps_set_airborne_model(void) {
char model6[] = {
0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, /* UBX-CFG-NAV5 */
0xFF, 0xFF, /* parameter bitmask */
0x06, /* dynamic model */
0x03, /* fix mode */
0x00, 0x00, 0x00, 0x00, /* 2D fix altitude */
0x10, 0x27, 0x00, 0x00, /* 2D fix altitude variance */
0x05, /* minimum elevation */
0x00, /* reserved */
0xFA, 0x00, /* position DOP */
0xFA, 0x00, /* time DOP */
0x64, 0x00, /* position accuracy */
0x2C, 0x01, /* time accuracy */
0x00, /* static hold threshold */
0x3C, /* DGPS timeout */
0x00, /* min. SVs above C/No thresh */
0x00, /* C/No threshold */
0x00, 0x00, /* reserved */
0xc8, 0x00, /* static hold max. distance */
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* reserved */
0x1a, 0x28 /* checksum */
};
uart_puts(model6, sizeof(model6));
//FIXME: sometimes, the ublox ACK is scrambled directly after power up...
// however, it accepts the message and no harm is done.
//return gps_receive_ack(0x06, 0x24);
return 0;
}
/*
* gps_set_power_save
*
* enables cyclic tracking on the uBlox M8Q
*
* returns if ACKed by GPS
*
*/
uint8_t gps_set_power_save(void) {
char powersave[] = {
0xB5, 0x62, 0x06, 0x3B, 44, 0, /* UBX-CFG-PM2 */
0x01, 0x00, 0x00, 0x00, /* v1, reserved 1..3 */
0x00, 0b00010000, 0b00000000, 0x00, /* on/off-mode, update ephemeris */
0xC0, 0xD4, 0x01, 0x00, /* update period, ms, 120s */
0xC0, 0xD4, 0x01, 0x00, /* search period, ms, 120s */
0x00, 0x00, 0x00, 0x00, /* grid offset */
0x00, 0x00, /* on-time after first fix */
0x01, 0x00, /* minimum acquisition time */
0x00, 0x00, 0x00, 0x00, /* reserved 4,5 */
0x00, 0x00, 0x00, 0x00, /* reserved 6 */
0x00, 0x00, 0x00, 0x00, /* reserved 7 */
0x00, 0x00, 0x00, 0x00, /* reserved 8,9,10 */
0x00, 0x00, 0x00, 0x00, /* reserved 11 */
0xa9, 0x77
};
uart_puts(powersave, sizeof(powersave));
return gps_receive_ack(0x06, 0x3B);
}
/*
* gps_power_save
*
* enables or disables the power save mode (which was configured before)
*/
uint8_t gps_power_save(int on) {
char recvmgmt[] = {
0xB5, 0x62, 0x06, 0x11, 2, 0, /* UBX-CFG-RXM */
0x08, 0x01, /* reserved, enable power save mode */
0x22, 0x92
};
if (!on) {
recvmgmt[7] = 0x00; /* continuous mode */
recvmgmt[8] = 0x21; /* new checksum */
recvmgmt[9] = 0x91;
}
uart_puts(recvmgmt, sizeof(recvmgmt));
return gps_receive_ack(0x06, 0x11);
}
/*
* gps_save_settings
*
* saves the GPS settings to flash. should be done when power save is disabled and all
* settings are configured.
*/
uint8_t gps_save_settings(void) {
char cfg[] = {
0xB5, 0x62, 0x06, 0x09, 12, 0, /* UBX-CFG-CFG */
0x00, 0x00, 0x00, 0x00, /* clear no sections */
0x1f, 0x1e, 0x00, 0x00, /* save all sections */
0x00, 0x00, 0x00, 0x00, /* load no sections */
0x58, 0x59
};
uart_puts(cfg, sizeof(cfg));
return gps_receive_ack(0x06, 0x09);
}
/*
* gps_startup_delay
*
* waits for the GPS to start up. this value is empirical.
* we could also wait for the startup string
*/
void gps_startup_delay(void) {
/* wait for the GPS to startup */
hw_delay_ms(1000);
}