7 #include <aversive/endian.h>
8 #include <aversive/wait.h>
14 #include "gps_venus.h"
16 /* note: enabling debug will make the code fail due to rx queue full */
17 #define debug_printf(fmt, a...) do { } while (0)
18 /* #define debug_printf(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__) */
22 #define GPS_PERIOD_MS 2
25 https://code.google.com/p/hardware-ntp/source/browse/trunk/pcb-1/venus634.c?r=12
28 /* some useful defines */
34 /* proccessing states */
52 uint8_t data[MAX_LEN];
55 static struct gps_frame rxframe;
57 static struct callout gps_timer;
62 /* 0x01 - System Restart */
63 #define M_RESTART 0x01
65 uint8_t start_mode; /* 01 = Hot; 02 = Warm; 03 = Cold */
66 uint16_t utc_year; /* >= 1980 */
72 int16_t latitude; /* -9000 to 9000, 1/100th degree, positive north */
73 int16_t longitude; /* -18000 to 18000, 1/100th degree, positive east */
74 int16_t altitude; /* -1000 to 18300, meters */
78 /* 0x02 - Query Software Version */
79 #define M_QUERYVER 0x02
81 uint8_t software_type; /* 01 = System Code */
85 /* 0x05 - Serial Port Configuration */
86 #define M_SERCONF 0x05
88 uint8_t port; /* 00 = COM1 (the only port) */
89 uint8_t baud; /* 00 = 4800; 01 = 9600; 02 = 19200; 03 = 38400;
90 04 = 57600; 05 = 115200; */
91 uint8_t update; /* 00 = SRAM only; 01 = SRAM and FLASH */
96 /* 0x08 - nmea interval */
97 #define M_NMEAINTERVAL 0x08
106 uint8_t attributes; /*
114 /* 0x09 - Query Software Version */
115 #define M_OUTPUT 0x09
122 uint8_t attributes; /*
124 1 - update ram & flash
129 /* 0x0E - Update Rage */
132 uint8_t rate; /* Hz */
133 uint8_t attributes; /*
135 1 - update ram & flash
141 /* 0x37 - configure waas */
144 uint8_t enable; /* 01 = enable */
148 /* 0x39 - configure position pinning */
149 #define M_PINNING 0x39
151 uint8_t enable; /* 0 = default, 1 = enable, 2 = disable */
152 uint8_t attributes; /* 0 = ram, 1 = ram+flash */
155 /* 0x3B - configure position pinning */
156 #define M_PINNING_PARAMS 0x3B
160 uint16_t unpin_speed;
163 uint8_t attributes; /* 0 = ram, 1 = ram+flash */
164 } m_pinning_params_t;
166 /* 0x3C - nav mode */
167 #define M_NAVMODE 0x3c
169 uint8_t navmode; /* 00 = car
176 static void serial1_tx_cout(uint8_t c)
178 debug_printf("%.2X ", c);
179 uart_send(GPS_UART, c);
182 static void venus634_send(uint8_t type, void *payload, uint16_t len)
186 debug_printf("SEND ");
188 /* now send the message */
190 serial1_tx_cout(START1);
191 serial1_tx_cout(START2);
192 serial1_tx_cout(((len+1) >> 8) & 0xff);
193 serial1_tx_cout((len+1) & 0xff);
194 /* type and payload */
195 serial1_tx_cout(type);
197 for (n = 0; n < len; n++) {
198 serial1_tx_cout(*(uint8_t *)payload);
199 crc ^= *(uint8_t *)payload;
202 /* checksum and tail */
203 serial1_tx_cout(crc);
204 serial1_tx_cout(END1);
205 serial1_tx_cout(END2);
210 static void venus634_restart(void)
214 memset(&restart, 0, sizeof(m_restart_t));
215 restart.start_mode = 0x03; /* COLD */
217 venus634_send(M_RESTART, &restart, sizeof(m_restart_t));
222 __attribute__((unused))
223 static void venus634_config_serial(void)
227 memset(&serconf, 0, sizeof(m_serconf_t));
232 venus634_send(M_SERCONF, &serconf, sizeof(m_serconf_t));
237 static void venus634_msg_type(uint8_t mode)
241 memset(&output, 0, sizeof(m_output_t));
242 output.msg_type = mode;
243 output.attributes = 0; /* ram, init may freeze when update in flash */
245 venus634_send(M_OUTPUT, &output, sizeof(m_output_t));
251 static void venus634_rate(void)
255 memset(&rate, 0, sizeof(m_rate_t));
257 rate.attributes = 0; /* ram, init may freeze when update in flash */
259 venus634_send(M_RATE, &rate, sizeof(m_rate_t));
264 /* Wide Area Augmentation System: use ground stations to increase the precision
266 static void venus634_waas(void)
270 memset(&waas, 0, sizeof(m_waas_t));
272 waas.attributes = 0; /* ram, init may freeze when update in flash */
274 venus634_send(M_WAAS, &waas, sizeof(m_waas_t));
279 /* Configure position pinning – Enable or disable position pinning of GNSS
281 static void venus634_pinning(void)
285 memset(&pinning, 0, sizeof(m_pinning_t));
287 pinning.attributes = 0; /* ram */
289 venus634_send(M_PINNING, &pinning, sizeof(m_pinning_t));
292 /* Configure position pinning parameters */
293 static void venus634_pinning_params(void)
295 m_pinning_params_t pinning_params;
297 /* just set everything to 0 */
298 memset(&pinning_params, 0, sizeof(m_pinning_params_t));
299 pinning_params.attributes = 0; /* ram */
301 venus634_send(M_PINNING_PARAMS, &pinning_params,
302 sizeof(m_pinning_params_t));
306 /* Tell we are a car instead of a pedestrian */
307 __attribute__((unused))
308 static void venus634_navmode(void)
312 memset(&navmode, 0, sizeof(m_navmode_t));
313 navmode.navmode = 0; /* car */
314 navmode.attributes = 0; /* ram, init may freeze when update in flash */
316 venus634_send(M_NAVMODE, &navmode, sizeof(m_navmode_t));
321 /* frequency of NMEA messages */
322 __attribute__((unused))
323 static void venus634_nmea_interval(void)
325 m_nmeainterval_t nmeainterval;
327 memset(&nmeainterval, 0, sizeof(m_nmeainterval_t));
329 /* set frequency for nmea: 1 = once every one position fix, 2 = once
330 * every two position fix, ... */
331 nmeainterval.gga = 1; /* GPGGA interval - GPS Fix Data*/
332 nmeainterval.gsa = 1; /* GNSS DOPS and Active Satellites */
333 nmeainterval.gsv = 1; /* GNSS Satellites in View */
334 nmeainterval.gll = 1; /* Geographic Position - Latitude longitude */
335 nmeainterval.rmc = 1; /* Recomended Minimum Specific GNSS Sentence */
336 nmeainterval.vtg = 1; /* Course Over Ground and Ground Speed */
337 nmeainterval.zda = 1; /* Time & Date*/
339 nmeainterval.attributes = 1; /* ram & flash */
341 venus634_send(M_NMEAINTERVAL, &nmeainterval, sizeof(m_nmeainterval_t));
346 static int8_t recv_cb(uint8_t byte)
350 /* bytes 0 and 1 are start bytes */
351 if (rxframe.cur_len == 0) {
352 if (byte != START1) {
353 debug_printf("bad start1 %.2X\n", byte);
357 else if (rxframe.cur_len == 1) {
358 if (byte != START2) {
359 debug_printf("bad start2 %.2X\n", byte);
363 /* bytes 2 and 3 are the length of frame in network order */
364 else if (rxframe.cur_len == 2) {
365 rxframe.len = (uint16_t)byte << 8;
367 else if (rxframe.cur_len == 3) {
368 rxframe.len |= (uint16_t)byte;
369 if (rxframe.len > MAX_LEN) {
370 debug_printf("bad len %d\n", rxframe.len);
374 /* next bytes are data (the 4 below is the size of header) */
375 else if ((rxframe.cur_len - 4) < rxframe.len) {
376 rxframe.data[rxframe.cur_len - 4] = byte;
378 /* then it's the crc */
379 else if ((rxframe.cur_len - 4) == rxframe.len) {
382 for (i = 0; i < rxframe.len; i++)
383 crc ^= rxframe.data[i];
385 debug_printf("invalid crc\n");
390 /* and the last 2 bytes are stop bytes */
391 else if ((rxframe.cur_len - 4) == (rxframe.len + 1)) {
393 debug_printf("bad end1 %.2X\n", byte);
397 else if ((rxframe.cur_len - 4) == (rxframe.len + 2)) {
399 debug_printf("bad end2 %.2X\n", byte);
402 debug_printf("valid frame received\n");
406 else /* should not happen */
417 static int recv_msg(void)
422 uint16_t t1, t2, diff;
426 IRQ_UNLOCK(irq_flags);
428 printf_P(PSTR("recv_msg()\n"));
432 IRQ_UNLOCK(irq_flags);
435 printf_P(PSTR("recv_msg timeout\n"));
439 c = uart_recv_nowait(GPS_UART);
443 debug_printf("%2.2x\n", c);
451 static int wait_ack(int msg_type)
455 uint16_t t1, t2, diff;
459 IRQ_UNLOCK(irq_flags);
469 IRQ_UNLOCK(irq_flags);
472 printf_P(PSTR("wait_ack timeout\n"));
476 /* retry if it's not the expected message */
477 if (rxframe.data[0] != 0x83 && rxframe.data[0] != 0x84)
479 if (rxframe.data[1] != msg_type)
482 if (rxframe.data[0] == 0x83)
483 printf_P(PSTR("ACK\n"));
484 else if (rxframe.data[0] == 0x84)
485 printf_P(PSTR("NACK\n"));
487 printf_P(PSTR("ZARB\n"));
494 static int decode_gps_pos(uint8_t *buf, uint16_t len)
496 struct gps_pos *pos = (struct gps_pos *)buf;
499 if (len != sizeof(*pos))
502 if (pos->msg_id != 0xA8)
505 /* fix endianness inside the frame */
506 pos->gps_week = ntohs(pos->gps_week);
507 pos->tow = ntohl(pos->tow);
509 pos->latitude = ntohl(pos->latitude);
510 pos->longitude = ntohl(pos->longitude);
511 pos->altitude = ntohl(pos->altitude);
513 pos->sea_altitude = ntohl(pos->sea_altitude);
515 pos->gdop = ntohs(pos->gdop);
516 pos->pdop = ntohs(pos->pdop);
517 pos->hdop = ntohs(pos->hdop);
518 pos->vdop = ntohs(pos->vdop);
519 pos->tdop = ntohs(pos->tdop);
521 pos->ecef_vx = ntohl(pos->ecef_vx);
522 pos->ecef_vy = ntohl(pos->ecef_vy);
523 pos->ecef_vz = ntohl(pos->ecef_vz);
525 /* update global structure */
527 memcpy(&gps_pos, pos, sizeof(gps_pos));
528 IRQ_UNLOCK(irq_flags);
533 /* called on timer event */
534 static void gps_venus_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
536 static int16_t last_valid = 0;
537 static uint8_t is_valid = 0;
549 IRQ_UNLOCK(irq_flags);
552 c = uart_recv_nowait(GPS_UART);
553 if (c < 0) /* no more char */
558 decode_gps_pos(rxframe.data, rxframe.len);
567 diff = ms - last_valid;
568 if (is_valid == 1 && diff > 500) {
571 /* update global structure */
573 memset(&gps_pos, 0, sizeof(gps_pos));
574 gps_pos.latitude = 0xffffffff;
575 IRQ_UNLOCK(irq_flags);
578 callout_schedule(cm, tim, GPS_PERIOD_MS);
581 static void venus634_configure(void)
583 /* ask the GPS to reset */
584 printf_P(PSTR("init..."));
588 printf_P(PSTR("binmode..."));
589 venus634_msg_type(2); /* binary */
592 printf_P(PSTR("waas..."));
596 printf_P(PSTR("pinning..."));
600 printf_P(PSTR("pinning parameters..."));
601 venus634_pinning_params();
602 wait_ack(M_PINNING_PARAMS);
604 printf_P(PSTR("rate..."));
608 printf_P(PSTR("GPS configuration done !\n"));
612 https://www.sparkfun.com/datasheets/GPS/Modules/AN0003_v1.4.14_FlashOnly.pdf
615 int gps_venus_init(void)
617 venus634_configure();
619 callout_init(&gps_timer, gps_venus_cb, NULL, GPS_PRIO);
620 callout_schedule(&imuboard.intr_cm, &gps_timer, GPS_PERIOD_MS); /* every 2ms */