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 #define debug_printf(args...) do { } while (0)
22 /* #define debug_printf(args...) printf(args) */
27 https://code.google.com/p/hardware-ntp/source/browse/trunk/pcb-1/venus634.c?r=12
30 /* some useful defines */
36 /* proccessing states */
54 uint8_t data[MAX_LEN];
57 static struct gps_frame rxframe;
59 static struct gps_pos gps_pos;
61 static struct callout gps_timer;
66 /* 0x01 - System Restart */
67 #define M_RESTART 0x01
69 uint8_t start_mode; /* 01 = Hot; 02 = Warm; 03 = Cold */
70 uint16_t utc_year; /* >= 1980 */
76 int16_t latitude; /* -9000 to 9000, 1/100th degree, positive north */
77 int16_t longitude; /* -18000 to 18000, 1/100th degree, positive east */
78 int16_t altitude; /* -1000 to 18300, meters */
82 /* 0x02 - Query Software Version */
83 #define M_QUERYVER 0x02
85 uint8_t software_type; /* 01 = System Code */
89 /* 0x05 - Serial Port Configuration */
90 #define M_SERCONF 0x05
92 uint8_t port; /* 00 = COM1 (the only port) */
93 uint8_t baud; /* 00 = 4800; 01 = 9600; 02 = 19200; 03 = 38400;
94 04 = 57600; 05 = 115200; */
95 uint8_t update; /* 00 = SRAM only; 01 = SRAM and FLASH */
100 /* 0x08 - nmea interval */
101 #define M_NMEAINTERVAL 0x08
110 uint8_t attributes; /*
118 /* 0x09 - Query Software Version */
119 #define M_OUTPUT 0x09
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 /* 0x3C - nav mode */
149 #define M_NAVMODE 0x3c
151 uint8_t navmode; /* 00 = car
161 void serial1_tx_cout(uint8_t c)
163 debug_printf("%.2X ", c);
164 uart_send(GPS_UART, c);
167 void venus634_send(uint8_t type, void *payload, uint16_t len)
171 debug_printf("SEND ");
173 /* now send the message */
175 serial1_tx_cout(START1);
176 serial1_tx_cout(START2);
177 serial1_tx_cout(((len+1) >> 8) & 0xff);
178 serial1_tx_cout((len+1) & 0xff);
179 /* type and payload */
180 serial1_tx_cout(type);
182 for (n = 0; n < len; n++) {
183 serial1_tx_cout(*(uint8_t *)payload);
184 crc ^= *(uint8_t *)payload;
187 /* checksum and tail */
188 serial1_tx_cout(crc);
189 serial1_tx_cout(END1);
190 serial1_tx_cout(END2);
195 void venus634_restart(void)
199 memset(&restart,0,sizeof(m_restart_t));
200 restart.start_mode = 0x03; /* COLD */
202 venus634_send(M_RESTART,&restart,sizeof(m_restart_t));
207 void venus634_config_serial(void)
211 memset(&serconf,0,sizeof(m_serconf_t));
216 venus634_send(M_SERCONF,&serconf,sizeof(m_serconf_t));
222 void venus634_msg_type(void)
226 memset(&output,0,sizeof(m_output_t));
227 output.msg_type = 0x02; /* binary msg */
229 venus634_send(M_OUTPUT,&output,sizeof(m_output_t));
235 void venus634_rate(void)
239 memset(&rate,0,sizeof(m_rate_t));
241 rate.attributes = 0; /* ram */
243 venus634_send(M_RATE,&rate,sizeof(m_rate_t));
248 /* Wide Area Augmentation System: use ground stations to increase the precision
250 void venus634_waas(void)
254 memset(&waas,0,sizeof(m_waas_t));
256 waas.attributes = 0; /* ram */
258 venus634_send(M_WAAS,&waas,sizeof(m_waas_t));
263 /* Tell we are a car instead of a pedestrian */
264 void venus634_navmode(void)
268 memset(&navmode,0,sizeof(m_navmode_t));
269 navmode.navmode = 0; /* car */
270 navmode.attributes = 0; /* ram */
272 venus634_send(M_NAVMODE,&navmode,sizeof(m_navmode_t));
277 /* frequency of NMEA messages */
278 void venus634_nmea_interval(void)
280 m_nmeainterval_t nmeainterval;
282 memset(&nmeainterval,0,sizeof(m_nmeainterval_t));
284 /* set frequency for nmea: 1 = once every one position fix, 2 = once
285 * every two position fix, ... */
286 nmeainterval.gga = 1; /* GPGGA interval - GPS Fix Data*/
287 nmeainterval.gsa = 1; /* GNSS DOPS and Active Satellites */
288 nmeainterval.gsv = 1; /* GNSS Satellites in View */
289 nmeainterval.gll = 1; /* Geographic Position - Latitude longitude */
290 nmeainterval.rmc = 1; /* Recomended Minimum Specific GNSS Sentence */
291 nmeainterval.vtg = 1; /* Course Over Ground and Ground Speed */
292 nmeainterval.zda = 1; /* Time & Date*/
294 nmeainterval.attributes = 1; /* ram flash */
296 venus634_send(M_NMEAINTERVAL,&nmeainterval,sizeof(m_nmeainterval_t));
301 int8_t recv_cb(uint8_t byte)
305 /* bytes 0 and 1 are start bytes */
306 if (rxframe.cur_len == 0) {
307 if (byte != START1) {
308 debug_printf("bad start1 %.2X\n", byte);
312 else if (rxframe.cur_len == 1) {
313 if (byte != START2) {
314 debug_printf("bad start2 %.2X\n", byte);
318 /* bytes 2 and 3 are the length of frame in network order */
319 else if (rxframe.cur_len == 2) {
320 rxframe.len = (uint16_t)byte << 8;
322 else if (rxframe.cur_len == 3) {
323 rxframe.len |= (uint16_t)byte;
324 if (rxframe.len > MAX_LEN) {
325 debug_printf("bad len %d\n", rxframe.len);
329 /* next bytes are data (the 4 below is the size of header) */
330 else if ((rxframe.cur_len - 4) < rxframe.len) {
331 rxframe.data[rxframe.cur_len - 4] = byte;
333 /* then it's the crc */
334 else if ((rxframe.cur_len - 4) == rxframe.len) {
337 for (i = 0; i < rxframe.len; i++)
338 crc ^= rxframe.data[i];
340 debug_printf("invalid crc\n");
345 /* and the last 2 bytes are stop bytes */
346 else if ((rxframe.cur_len - 4) == (rxframe.len + 1)) {
348 debug_printf("bad end1 %.2X\n", byte);
352 else if ((rxframe.cur_len - 4) == (rxframe.len + 2)) {
354 debug_printf("bad end2 %.2X\n", byte);
357 debug_printf("valid frame received\n");
361 else /* should not happen */
378 /* XXX use select for timeout */
379 c = uart_recv(GPS_UART);
386 int wait_ack(int msg_type)
395 /* retry if it's not the expected message */
396 if (rxframe.data[0] != 0x83 && rxframe.data[0] != 0x84)
398 if (rxframe.data[1] != msg_type)
401 if (rxframe.data[0] == 0x83)
403 else if (rxframe.data[0] == 0x84)
413 static int decode_gps_pos(uint8_t *buf, uint16_t len)
415 struct gps_pos *pos = (struct gps_pos *)buf;
418 if (len != sizeof(*pos))
421 if (pos->msg_id != 0xA8) /* XXX not tested */
424 gps_pos.gps_week = ntohs(pos->gps_week);
425 gps_pos.tow = ntohl(pos->tow);
427 gps_pos.latitude = ntohl(pos->latitude);
428 gps_pos.longitude = ntohl(pos->longitude);
429 gps_pos.altitude = ntohl(pos->altitude);
431 gps_pos.sea_altitude = ntohl(pos->sea_altitude);
433 gps_pos.gdop = ntohs(pos->gdop);
434 gps_pos.pdop = ntohs(pos->pdop);
435 gps_pos.hdop = ntohs(pos->hdop);
436 gps_pos.vdop = ntohs(pos->vdop);
437 gps_pos.tdop = ntohs(pos->tdop);
439 gps_pos.ecef_vx = ntohl(pos->ecef_vx);
440 gps_pos.ecef_vy = ntohl(pos->ecef_vy);
441 gps_pos.ecef_vz = ntohl(pos->ecef_vz);
443 /* update global structure */
445 memcpy(&gps_pos, pos, sizeof(gps_pos));
446 IRQ_UNLOCK(irq_flags);
451 /* display current GPS position stored in the global variable */
452 static void display_gps(void)
454 printf("id %.2X mode %.2X svnum %.2X gpsw %.4X tow %.10"PRIu32"\t",
461 printf("lat %.8"PRIx32" long %.8"PRIx32" alt %.8"PRIx32"\n",
466 printf("gdop %3.3f pdop %3.3f hdop %3.3f vdop %3.3f tdop %3.3f\n",
467 (double)gps_pos.gdop/100.,
468 (double)gps_pos.pdop/100.,
469 (double)gps_pos.hdop/100.,
470 (double)gps_pos.vdop/100.,
471 (double)gps_pos.tdop/100.);
473 printf("lat %3.5f long %3.5f alt %3.5f sea_alt %3.5f\n",
474 (double)gps_pos.latitude/10000000.,
475 (double)gps_pos.longitude/10000000.,
476 (double)gps_pos.altitude/100.,
477 (double)gps_pos.sea_altitude/100.);
479 printf("vx %3.3f vy %3.3f vz %3.3f\n",
480 (double)gps_pos.ecef_vx/100.,
481 (double)gps_pos.ecef_vy/100.,
482 (double)gps_pos.ecef_vz/100.);
485 static void gps_venus_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
495 c = uart_recv_nowait(GPS_UART);
496 if (c < 0) /* no more char */
501 decode_gps_pos(rxframe.data, rxframe.len);
508 callout_schedule(cm, tim, 2);
511 static void venus634_configure(void)
513 /* ask the GPS to reset */
518 /* it seems we must wait that the GPS is restarted, else it doesn't work
522 printf("binmode...");
534 printf("GPS configuration done !\n");
538 https://www.sparkfun.com/datasheets/GPS/Modules/AN0003_v1.4.14_FlashOnly.pdf
541 int gps_venus_init(void)
543 venus634_configure();
545 callout_init(&gps_timer, gps_venus_cb, NULL, GPS_PRIO);
546 callout_schedule(&imuboard.intr_cm, &gps_timer, 2); /* every 2ms */
551 void gps_get_pos(struct gps_pos *pos)
553 memcpy(pos, &gps_pos, sizeof(*pos));
570 /* get position (prevent modification of gps pos during copy) */
571 prio = callout_mgr_set_prio(&imuboard.intr_cm, GPS_PRIO);
573 callout_mgr_restore_prio(&imuboard.intr_cm, prio);
576 len = snprintf(buf, sizeof(buf),
578 "svnum %.2X lat %3.5f long %3.5f "
579 "alt %3.5f sea_alt %3.5f\n",
581 (double)gps_pos.latitude/10000000.,
582 (double)gps_pos.longitude/10000000.,
583 (double)gps_pos.altitude/100.,
584 (double)gps_pos.sea_altitude/100.);
587 if (sd_log_enabled()) {
589 if (sd_log_write(buf, len) != len) {
590 printf_P(PSTR("error writing to file\n"));