7 #include <aversive/endian.h>
8 #include <aversive/wait.h>
13 #include "gps_venus.h"
15 #include "fat_config.h"
16 #include "partition.h"
18 #include "sd_raw_config.h"
21 /* note: enabling debug will make the code fail due to rx queue full */
22 #define debug_printf(fmt, a...) do { } while (0)
23 /* #define debug_printf(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__) */
27 #define GPS_PERIOD_MS 2
30 https://code.google.com/p/hardware-ntp/source/browse/trunk/pcb-1/venus634.c?r=12
33 /* some useful defines */
39 /* proccessing states */
57 uint8_t data[MAX_LEN];
60 static struct gps_frame rxframe;
62 static struct gps_pos gps_pos;
64 static struct callout gps_timer;
69 /* 0x01 - System Restart */
70 #define M_RESTART 0x01
72 uint8_t start_mode; /* 01 = Hot; 02 = Warm; 03 = Cold */
73 uint16_t utc_year; /* >= 1980 */
79 int16_t latitude; /* -9000 to 9000, 1/100th degree, positive north */
80 int16_t longitude; /* -18000 to 18000, 1/100th degree, positive east */
81 int16_t altitude; /* -1000 to 18300, meters */
85 /* 0x02 - Query Software Version */
86 #define M_QUERYVER 0x02
88 uint8_t software_type; /* 01 = System Code */
92 /* 0x05 - Serial Port Configuration */
93 #define M_SERCONF 0x05
95 uint8_t port; /* 00 = COM1 (the only port) */
96 uint8_t baud; /* 00 = 4800; 01 = 9600; 02 = 19200; 03 = 38400;
97 04 = 57600; 05 = 115200; */
98 uint8_t update; /* 00 = SRAM only; 01 = SRAM and FLASH */
103 /* 0x08 - nmea interval */
104 #define M_NMEAINTERVAL 0x08
113 uint8_t attributes; /*
121 /* 0x09 - Query Software Version */
122 #define M_OUTPUT 0x09
129 uint8_t attributes; /*
131 1 - update ram & flash
136 /* 0x0E - Update Rage */
139 uint8_t rate; /* Hz */
140 uint8_t attributes; /*
142 1 - update ram & flash
148 /* 0x37 - configure waas */
151 uint8_t enable; /* 01 = enable */
155 /* 0x3C - nav mode */
156 #define M_NAVMODE 0x3c
158 uint8_t navmode; /* 00 = car
165 void serial1_tx_cout(uint8_t c)
167 debug_printf("%.2X ", c);
168 uart_send(GPS_UART, c);
171 void venus634_send(uint8_t type, void *payload, uint16_t len)
175 debug_printf("SEND ");
177 /* now send the message */
179 serial1_tx_cout(START1);
180 serial1_tx_cout(START2);
181 serial1_tx_cout(((len+1) >> 8) & 0xff);
182 serial1_tx_cout((len+1) & 0xff);
183 /* type and payload */
184 serial1_tx_cout(type);
186 for (n = 0; n < len; n++) {
187 serial1_tx_cout(*(uint8_t *)payload);
188 crc ^= *(uint8_t *)payload;
191 /* checksum and tail */
192 serial1_tx_cout(crc);
193 serial1_tx_cout(END1);
194 serial1_tx_cout(END2);
199 void venus634_restart(void)
203 memset(&restart, 0, sizeof(m_restart_t));
204 restart.start_mode = 0x03; /* COLD */
206 venus634_send(M_RESTART, &restart, sizeof(m_restart_t));
211 void venus634_config_serial(void)
215 memset(&serconf, 0, sizeof(m_serconf_t));
220 venus634_send(M_SERCONF, &serconf, sizeof(m_serconf_t));
226 void venus634_msg_type(uint8_t mode)
230 memset(&output, 0, sizeof(m_output_t));
231 output.msg_type = mode;
232 output.attributes = 0; /* ram, init may freeze when update in flash */
234 venus634_send(M_OUTPUT, &output, sizeof(m_output_t));
240 void venus634_rate(void)
244 memset(&rate, 0, sizeof(m_rate_t));
246 rate.attributes = 0; /* ram, init may freeze when update in flash */
248 venus634_send(M_RATE, &rate, sizeof(m_rate_t));
253 /* Wide Area Augmentation System: use ground stations to increase the precision
255 void venus634_waas(void)
259 memset(&waas, 0, sizeof(m_waas_t));
261 waas.attributes = 0; /* ram, init may freeze when update in flash */
263 venus634_send(M_WAAS, &waas, sizeof(m_waas_t));
268 /* Tell we are a car instead of a pedestrian */
269 void venus634_navmode(void)
273 memset(&navmode, 0, sizeof(m_navmode_t));
274 navmode.navmode = 0; /* car */
275 navmode.attributes = 0; /* ram, init may freeze when update in flash */
277 venus634_send(M_NAVMODE, &navmode, sizeof(m_navmode_t));
282 /* frequency of NMEA messages */
283 void venus634_nmea_interval(void)
285 m_nmeainterval_t nmeainterval;
287 memset(&nmeainterval, 0, sizeof(m_nmeainterval_t));
289 /* set frequency for nmea: 1 = once every one position fix, 2 = once
290 * every two position fix, ... */
291 nmeainterval.gga = 1; /* GPGGA interval - GPS Fix Data*/
292 nmeainterval.gsa = 1; /* GNSS DOPS and Active Satellites */
293 nmeainterval.gsv = 1; /* GNSS Satellites in View */
294 nmeainterval.gll = 1; /* Geographic Position - Latitude longitude */
295 nmeainterval.rmc = 1; /* Recomended Minimum Specific GNSS Sentence */
296 nmeainterval.vtg = 1; /* Course Over Ground and Ground Speed */
297 nmeainterval.zda = 1; /* Time & Date*/
299 nmeainterval.attributes = 1; /* ram & flash */
301 venus634_send(M_NMEAINTERVAL, &nmeainterval, sizeof(m_nmeainterval_t));
306 int8_t recv_cb(uint8_t byte)
310 /* bytes 0 and 1 are start bytes */
311 if (rxframe.cur_len == 0) {
312 if (byte != START1) {
313 debug_printf("bad start1 %.2X\n", byte);
317 else if (rxframe.cur_len == 1) {
318 if (byte != START2) {
319 debug_printf("bad start2 %.2X\n", byte);
323 /* bytes 2 and 3 are the length of frame in network order */
324 else if (rxframe.cur_len == 2) {
325 rxframe.len = (uint16_t)byte << 8;
327 else if (rxframe.cur_len == 3) {
328 rxframe.len |= (uint16_t)byte;
329 if (rxframe.len > MAX_LEN) {
330 debug_printf("bad len %d\n", rxframe.len);
334 /* next bytes are data (the 4 below is the size of header) */
335 else if ((rxframe.cur_len - 4) < rxframe.len) {
336 rxframe.data[rxframe.cur_len - 4] = byte;
338 /* then it's the crc */
339 else if ((rxframe.cur_len - 4) == rxframe.len) {
342 for (i = 0; i < rxframe.len; i++)
343 crc ^= rxframe.data[i];
345 debug_printf("invalid crc\n");
350 /* and the last 2 bytes are stop bytes */
351 else if ((rxframe.cur_len - 4) == (rxframe.len + 1)) {
353 debug_printf("bad end1 %.2X\n", byte);
357 else if ((rxframe.cur_len - 4) == (rxframe.len + 2)) {
359 debug_printf("bad end2 %.2X\n", byte);
362 debug_printf("valid frame received\n");
366 else /* should not happen */
383 /* XXX use select for timeout */
384 c = uart_recv(GPS_UART);
391 int wait_ack(int msg_type)
400 /* retry if it's not the expected message */
401 if (rxframe.data[0] != 0x83 && rxframe.data[0] != 0x84)
403 if (rxframe.data[1] != msg_type)
406 if (rxframe.data[0] == 0x83)
407 printf_P(PSTR("ACK\n"));
408 else if (rxframe.data[0] == 0x84)
409 printf_P(PSTR("NACK\n"));
411 printf_P(PSTR("ZARB\n"));
418 static int decode_gps_pos(uint8_t *buf, uint16_t len)
420 struct gps_pos *pos = (struct gps_pos *)buf;
423 if (len != sizeof(*pos))
426 if (pos->msg_id != 0xA8)
429 /* fix endianness inside the frame */
430 pos->gps_week = ntohs(pos->gps_week);
431 pos->tow = ntohl(pos->tow);
433 pos->latitude = ntohl(pos->latitude);
434 pos->longitude = ntohl(pos->longitude);
435 pos->altitude = ntohl(pos->altitude);
437 pos->sea_altitude = ntohl(pos->sea_altitude);
439 pos->gdop = ntohs(pos->gdop);
440 pos->pdop = ntohs(pos->pdop);
441 pos->hdop = ntohs(pos->hdop);
442 pos->vdop = ntohs(pos->vdop);
443 pos->tdop = ntohs(pos->tdop);
445 pos->ecef_vx = ntohl(pos->ecef_vx);
446 pos->ecef_vy = ntohl(pos->ecef_vy);
447 pos->ecef_vz = ntohl(pos->ecef_vz);
449 /* update global structure */
451 memcpy(&gps_pos, pos, sizeof(gps_pos));
452 IRQ_UNLOCK(irq_flags);
457 /* display current GPS position stored in the global variable */
458 static void display_gps(void)
460 /* no need to call ntohs/ntohl because the frame is already fixed by
461 * decode_gps_pos() */
462 printf_P(PSTR("id %.2X mode %.2X svnum %.2X gpsw %.4X "
463 "tow %.10"PRIu32"\t"),
470 printf_P(PSTR("lat %.8"PRIx32" long %.8"PRIx32" alt %.8"PRIx32"\n"),
475 printf_P(PSTR("gdop %3.3f pdop %3.3f hdop %3.3f vdop %3.3f "
477 (double)gps_pos.gdop/100.,
478 (double)gps_pos.pdop/100.,
479 (double)gps_pos.hdop/100.,
480 (double)gps_pos.vdop/100.,
481 (double)gps_pos.tdop/100.);
483 printf_P(PSTR("lat %3.5f long %3.5f alt %3.5f sea_alt %3.5f\n"),
484 (double)gps_pos.latitude/10000000.,
485 (double)gps_pos.longitude/10000000.,
486 (double)gps_pos.altitude/100.,
487 (double)gps_pos.sea_altitude/100.);
489 printf_P(PSTR("vx %3.3f vy %3.3f vz %3.3f\n"),
490 (double)gps_pos.ecef_vx/100.,
491 (double)gps_pos.ecef_vy/100.,
492 (double)gps_pos.ecef_vz/100.);
495 /* called on timer event */
496 static void gps_venus_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
498 static int16_t last_valid = 0;
499 static uint8_t is_valid = 0;
511 IRQ_UNLOCK(irq_flags);
514 c = uart_recv_nowait(GPS_UART);
515 if (c < 0) /* no more char */
520 decode_gps_pos(rxframe.data, rxframe.len);
529 diff = ms - last_valid;
530 if (is_valid == 1 && diff > 100) {
533 /* update global structure */
535 memset(&gps_pos, 0, sizeof(gps_pos));
536 gps_pos.latitude = 0xffffffff;
537 IRQ_UNLOCK(irq_flags);
540 callout_schedule(cm, tim, GPS_PERIOD_MS);
543 static void venus634_configure(void)
545 /* ask the GPS to reset */
546 printf_P(PSTR("init..."));
550 printf_P(PSTR("binmode..."));
551 venus634_msg_type(2); /* binary */
554 printf_P(PSTR("waas..."));
558 printf_P(PSTR("rate..."));
562 printf_P(PSTR("GPS configuration done !\n"));
566 https://www.sparkfun.com/datasheets/GPS/Modules/AN0003_v1.4.14_FlashOnly.pdf
569 int gps_venus_init(void)
571 venus634_configure();
573 callout_init(&gps_timer, gps_venus_cb, NULL, GPS_PRIO);
574 callout_schedule(&imuboard.intr_cm, &gps_timer, GPS_PERIOD_MS); /* every 2ms */
579 void gps_get_pos(struct gps_pos *pos)
581 memcpy(pos, &gps_pos, sizeof(*pos));
596 /* get position (prevent modification of gps pos during copy) */
597 prio = callout_mgr_set_prio(&imuboard.intr_cm, GPS_PRIO);
599 callout_mgr_restore_prio(&imuboard.intr_cm, prio);
602 len = snprintf(buf, sizeof(buf),
604 "svnum %.2X lat %3.5f long %3.5f "
605 "alt %3.5f sea_alt %3.5f\n",
607 (double)gps_pos.latitude / 10000000.,
608 (double)gps_pos.longitude / 10000000.,
609 (double)gps_pos.altitude / 100.,
610 (double)gps_pos.sea_altitude / 100.);
613 if (sd_log_enabled()) {
614 if (sd_log_write(buf, len) != len) {
615 printf_P(PSTR("error writing to file\n"));
620 printf_P(PSTR("%s"), buf);