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"
20 #define debug_printf(args...) do { } while (0)
21 /* #define debug_printf(args...) printf(args) */
26 https://code.google.com/p/hardware-ntp/source/browse/trunk/pcb-1/venus634.c?r=12
29 /* some useful defines */
35 /* proccessing states */
53 uint8_t data[MAX_LEN];
56 static struct gps_frame rxframe;
58 static struct gps_pos gps_pos;
60 static struct callout gps_timer;
65 /* 0x01 - System Restart */
66 #define M_RESTART 0x01
68 uint8_t start_mode; /* 01 = Hot; 02 = Warm; 03 = Cold */
69 uint16_t utc_year; /* >= 1980 */
75 int16_t latitude; /* -9000 to 9000, 1/100th degree, positive north */
76 int16_t longitude; /* -18000 to 18000, 1/100th degree, positive east */
77 int16_t altitude; /* -1000 to 18300, meters */
81 /* 0x02 - Query Software Version */
82 #define M_QUERYVER 0x02
84 uint8_t software_type; /* 01 = System Code */
88 /* 0x05 - Serial Port Configuration */
89 #define M_SERCONF 0x05
91 uint8_t port; /* 00 = COM1 (the only port) */
92 uint8_t baud; /* 00 = 4800; 01 = 9600; 02 = 19200; 03 = 38400;
93 04 = 57600; 05 = 115200; */
94 uint8_t update; /* 00 = SRAM only; 01 = SRAM and FLASH */
99 /* 0x08 - nema interval */
100 #define M_NEMAINTERVAL 0x08
109 uint8_t attributes; /*
117 /* 0x09 - Query Software Version */
118 #define M_OUTPUT 0x09
128 /* 0x0E - Update Rage */
131 uint8_t rate; /* Hz */
132 uint8_t attributes; /*
134 1 - update ram & flash
140 /* 0x37 - configure waas */
143 uint8_t enable; /* 01 = enable */
147 /* 0x3C - nav mode */
148 #define M_NAVMODE 0x3c
150 uint8_t navmode; /* 00 = car
160 void serial1_tx_cout(uint8_t c)
162 debug_printf("%.2X ", c);
163 uart_send(GPS_UART, c);
166 void venus634_send(uint8_t type, void *payload, uint16_t len)
170 debug_printf("SEND ");
172 /* now send the message */
174 serial1_tx_cout(START1);
175 serial1_tx_cout(START2);
176 serial1_tx_cout(((len+1) >> 8) & 0xff);
177 serial1_tx_cout((len+1) & 0xff);
178 /* type and payload */
179 serial1_tx_cout(type);
181 for (n = 0; n < len; n++) {
182 serial1_tx_cout(*(uint8_t *)payload);
183 crc ^= *(uint8_t *)payload;
186 /* checksum and tail */
187 serial1_tx_cout(crc);
188 serial1_tx_cout(END1);
189 serial1_tx_cout(END2);
194 void venus634_restart(void)
198 memset(&restart,0,sizeof(m_restart_t));
199 restart.start_mode = 0x03; /* COLD */
201 venus634_send(M_RESTART,&restart,sizeof(m_restart_t));
206 void venus634_config_serial(void)
210 memset(&serconf,0,sizeof(m_serconf_t));
215 venus634_send(M_SERCONF,&serconf,sizeof(m_serconf_t));
221 void venus634_msg_type(void)
225 memset(&output,0,sizeof(m_output_t));
226 output.msg_type = 0x02; /* binary msg */
228 venus634_send(M_OUTPUT,&output,sizeof(m_output_t));
234 void venus634_rate(void)
238 memset(&rate,0,sizeof(m_rate_t));
240 //rate.rate = 0; /* ram */
242 venus634_send(M_RATE,&rate,sizeof(m_rate_t));
247 void venus634_waas(void)
251 memset(&waas,0,sizeof(m_waas_t));
253 waas.attributes = 0; /* ram */
255 venus634_send(M_WAAS,&waas,sizeof(m_waas_t));
261 void venus634_navmode(void)
265 memset(&navmode,0,sizeof(m_navmode_t));
266 navmode.navmode = 0; /* car */
267 navmode.attributes = 0; /* ram */
269 venus634_send(M_NAVMODE,&navmode,sizeof(m_navmode_t));
275 void venus634_nema_interval(void)
277 m_nemainterval_t nemainterval;
279 memset(&nemainterval,0,sizeof(m_nemainterval_t));
280 nemainterval.gga = 1;
281 nemainterval.gsa = 1;
282 nemainterval.gsv = 1;
283 nemainterval.gll = 1;
284 nemainterval.rmc = 1;
285 nemainterval.vtg = 1;
286 nemainterval.zda = 1;
288 nemainterval.attributes = 1; /* ram flash */
290 venus634_send(M_NEMAINTERVAL,&nemainterval,sizeof(m_nemainterval_t));
295 int8_t recv_cb(uint8_t byte)
299 /* bytes 0 and 1 are start bytes */
300 if (rxframe.cur_len == 0) {
301 if (byte != START1) {
302 debug_printf("bad start1 %.2X\n", byte);
306 else if (rxframe.cur_len == 1) {
307 if (byte != START2) {
308 debug_printf("bad start2 %.2X\n", byte);
312 /* bytes 2 and 3 are the length of frame in network order */
313 else if (rxframe.cur_len == 2) {
314 rxframe.len = (uint16_t)byte << 8;
316 else if (rxframe.cur_len == 3) {
317 rxframe.len |= (uint16_t)byte;
318 if (rxframe.len > MAX_LEN) {
319 debug_printf("bad len %d\n", rxframe.len);
323 /* next bytes are data (the 4 below is the size of header) */
324 else if ((rxframe.cur_len - 4) < rxframe.len) {
325 rxframe.data[rxframe.cur_len - 4] = byte;
327 /* then it's the crc */
328 else if ((rxframe.cur_len - 4) == rxframe.len) {
331 for (i = 0; i < rxframe.len; i++)
332 crc ^= rxframe.data[i];
334 debug_printf("invalid crc\n");
339 /* and the last 2 bytes are stop bytes */
340 else if ((rxframe.cur_len - 4) == (rxframe.len + 1)) {
342 debug_printf("bad end1 %.2X\n", byte);
346 else if ((rxframe.cur_len - 4) == (rxframe.len + 2)) {
348 debug_printf("bad end2 %.2X\n", byte);
351 debug_printf("valid frame received\n");
355 else /* should not happen */
372 /* XXX use select for timeout */
373 c = uart_recv(GPS_UART);
380 int wait_ack(int msg_type)
389 /* retry if it's not the expected message */
390 if (rxframe.data[0] != 0x83 && rxframe.data[0] != 0x84)
392 if (rxframe.data[1] != msg_type)
395 if (rxframe.data[0] == 0x83)
397 else if (rxframe.data[0] == 0x84)
407 static int decode_gps_pos(uint8_t *buf, uint16_t len)
409 struct gps_pos *pos = (struct gps_pos *)buf;
411 if (len != sizeof(*pos))
414 gps_pos.gps_week = ntohs(pos->gps_week);
415 gps_pos.tow = ntohl(pos->tow);
417 gps_pos.latitude = ntohl(pos->latitude);
418 gps_pos.longitude = ntohl(pos->longitude);
419 gps_pos.altitude = ntohl(pos->altitude);
421 gps_pos.sea_altitude = ntohl(pos->sea_altitude);
423 gps_pos.gdop = ntohs(pos->gdop);
424 gps_pos.pdop = ntohs(pos->pdop);
425 gps_pos.hdop = ntohs(pos->hdop);
426 gps_pos.vdop = ntohs(pos->vdop);
427 gps_pos.tdop = ntohs(pos->tdop);
429 gps_pos.ecef_vx = ntohl(pos->ecef_vx);
430 gps_pos.ecef_vy = ntohl(pos->ecef_vy);
431 gps_pos.ecef_vz = ntohl(pos->ecef_vz);
436 /* display current GPS position stored in the global variable */
437 static void display_gps(void)
439 printf("id %.2X mode %.2X svnum %.2X gpsw %.4X tow %.10"PRIu32"\t",
446 printf("lat %.8"PRIx32" long %.8"PRIx32" alt %.8"PRIx32"\n",
451 printf("gdop %3.3f pdop %3.3f hdop %3.3f vdop %3.3f tdop %3.3f\n",
452 (double)gps_pos.gdop/100.,
453 (double)gps_pos.pdop/100.,
454 (double)gps_pos.hdop/100.,
455 (double)gps_pos.vdop/100.,
456 (double)gps_pos.tdop/100.);
458 printf("lat %3.5f long %3.5f alt %3.5f sea_alt %3.5f\n",
459 (double)gps_pos.latitude/10000000.,
460 (double)gps_pos.longitude/10000000.,
461 (double)gps_pos.altitude/100.,
462 (double)gps_pos.sea_altitude/100.);
464 printf("vx %3.3f vy %3.3f vz %3.3f\n",
465 (double)gps_pos.ecef_vx/100.,
466 (double)gps_pos.ecef_vy/100.,
467 (double)gps_pos.ecef_vz/100.);
470 static void gps_venus_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
480 c = uart_recv_nowait(GPS_UART);
481 if (c < 0) /* no more char */
486 decode_gps_pos(rxframe.data, rxframe.len);
493 callout_schedule(cm, tim, 2);
496 static void venus634_configure(void)
498 /* ask the GPS to reset */
503 /* it seems we must wait that the GPS is restarted, else it doesn't work
507 printf("binmode...");
519 printf("GPS configuration done !\n");
523 https://www.sparkfun.com/datasheets/GPS/Modules/AN0003_v1.4.14_FlashOnly.pdf
526 int gps_venus_init(void)
528 venus634_configure();
530 callout_init(&gps_timer, gps_venus_cb, NULL, GPS_PRIO);
531 callout_schedule(&imuboard.intr_cm, &gps_timer, 2); /* every 2ms */
536 static uint8_t find_file_in_dir(struct fat_fs_struct* fs, struct fat_dir_struct* dd, const char* name, struct fat_dir_entry_struct* dir_entry)
540 while(fat_read_dir(dd, dir_entry))
542 if(strcmp(dir_entry->long_name, name) == 0)
552 static struct fat_file_struct* open_file_in_dir(struct fat_fs_struct* fs, struct fat_dir_struct* dd, const char* name)
554 struct fat_dir_entry_struct file_entry;
555 if(!find_file_in_dir(fs, dd, name, &file_entry))
558 return fat_open_file(fs, &file_entry);
561 static struct fat_file_struct *open_log_file(void)
563 struct fat_file_struct *fd;
564 struct fat_fs_struct *fs;
565 struct partition_struct *partition ;
566 struct fat_dir_struct *dd;
567 struct fat_dir_entry_struct directory;
568 struct fat_dir_entry_struct file_entry;
572 /* setup sd card slot */
573 if (!sd_raw_init()) {
575 printf_P(PSTR("MMC/SD initialization failed\n"));
580 /* open first partition */
581 partition = partition_open(sd_raw_read,
582 sd_raw_read_interval,
583 #if SD_RAW_WRITE_SUPPORT
584 sd_raw_write, sd_raw_write_interval,
591 /* If the partition did not open, assume the storage device
592 * is a "superfloppy", i.e. has no MBR.
594 partition = partition_open(sd_raw_read,
595 sd_raw_read_interval,
596 #if SD_RAW_WRITE_SUPPORT
598 sd_raw_write_interval,
606 printf_P(PSTR("opening partition failed\n"));
612 /* open file system */
613 fs = fat_open(partition);
616 printf_P(PSTR("opening filesystem failed\n"));
621 /* open root directory */
622 fat_get_dir_entry_of_path(fs, "/", &directory);
623 dd = fat_open_dir(fs, &directory);
626 printf_P(PSTR("opening root directory failed\n"));
631 /* print some card information as a boot message */
632 //print_disk_info(fs);
634 printf("choose log file name\n");
636 snprintf(name, sizeof(name), "log%.4d", i++);
637 if (!find_file_in_dir(fs, dd, name, &file_entry))
641 printf("create log file %s\n", name);
642 if (!fat_create_file(dd, name, &file_entry)) {
643 printf_P(PSTR("error creating file: "));
646 fd = open_file_in_dir(fs, dd, name);
648 printf_P(PSTR("error opening "));
655 void gps_get_pos(struct gps_pos *pos)
657 memcpy(pos, &gps_pos, sizeof(*pos));
662 struct fat_file_struct *fd = NULL;
670 fd = open_log_file();
672 printf("open log failed\r\n");
681 /* get position (prevent modification of gps pos during copy) */
682 prio = callout_mgr_set_prio(&imuboard.intr_cm, GPS_PRIO);
684 callout_mgr_restore_prio(&imuboard.intr_cm, prio);
687 len = snprintf(buf, sizeof(buf),
689 "svnum %.2X lat %3.5f long %3.5f "
690 "alt %3.5f sea_alt %3.5f\n",
692 (double)gps_pos.latitude/10000000.,
693 (double)gps_pos.longitude/10000000.,
694 (double)gps_pos.altitude/100.,
695 (double)gps_pos.sea_altitude/100.);
700 if (fat_write_file(fd, (unsigned char *)buf, len) != len) {
701 printf_P(PSTR("error writing to file\n"));