gps_venus.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <stdint.h>
   4 #include <string.h>
   5
   6 #include <aversive.h>
   7 #include <aversive/endian.h>
   8 #include <aversive/wait.h>
   9 #include <callout.h>
  10 #include <uart.h>
  11
  12 #include "main.h"
  13 #include "gps_venus.h"
  14 #include "fat.h"
  15 #include "fat_config.h"
  16 #include "partition.h"
  17 #include "sd_raw.h"
  18 #include "sd_raw_config.h"
  19
  20 #define debug_printf(args...) do { } while (0)
  21 /* #define debug_printf(args...) printf(args) */
  22
  23 #define GPS_UART 0
  24
  25 /*
  26   https://code.google.com/p/hardware-ntp/source/browse/trunk/pcb-1/venus634.c?r=12
  27  */
  28
  29 /* some useful defines */
  30 #define START1 0xA0
  31 #define START2 0xA1
  32 #define END1 0x0D
  33 #define END2 0x0A
  34
  35 /* proccessing states */
  36
  37 #define S_RESET 0
  38 #define S_POSTRESET 1
  39 #define S_START1 2
  40 #define S_START2 3
  41 #define S_LENGTH1 4
  42 #define S_LENGTH2 5
  43 #define S_DISCARD 6
  44 #define S_COPY 7
  45 #define S_ENDCOPY 8
  46 #define S_END 9
  47 #define S_END1 10
  48
  49 #define MAX_LEN 0x200
  50 struct gps_frame {
  51         uint16_t len;
  52         uint16_t cur_len;
  53         uint8_t data[MAX_LEN];
  54 };
  55
  56 static struct gps_frame rxframe;
  57
  58 static struct gps_pos gps_pos;
  59
  60 static struct callout gps_timer;
  61
  62 /* INPUT FORMATS */
  63
  64 /* packet types */
  65 /* 0x01 - System Restart */
  66 #define M_RESTART 0x01
  67 typedef struct {
  68         uint8_t start_mode; /* 01 = Hot; 02 = Warm; 03 = Cold */
  69         uint16_t utc_year; /* >= 1980 */
  70         uint8_t utc_month;
  71         uint8_t utc_day;
  72         uint8_t utc_hour;
  73         uint8_t utc_minute;
  74         uint8_t utc_second;
  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 */
  78 } m_restart_t;
  79
  80
  81 /* 0x02 - Query Software Version */
  82 #define M_QUERYVER 0x02
  83 typedef struct {
  84         uint8_t software_type; /* 01 = System Code */
  85 } m_queryver_t;
  86
  87
  88 /* 0x05 - Serial Port Configuration */
  89 #define M_SERCONF 0x05
  90 typedef struct {
  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 */
  95 } m_serconf_t;
  96
  97
  98
  99 /* 0x08 - nema interval */
 100 #define M_NEMAINTERVAL 0x08
 101 typedef struct {
 102         uint8_t gga;
 103         uint8_t gsa;
 104         uint8_t gsv;
 105         uint8_t gll;
 106         uint8_t rmc;
 107         uint8_t vtg;
 108         uint8_t zda;
 109         uint8_t attributes; /*
 110                               0 - sram
 111                               1 - flash
 112                              */
 113
 114 } m_nemainterval_t;
 115
 116
 117 /* 0x09 - Query Software Version */
 118 #define M_OUTPUT 0x09
 119 typedef struct {
 120         uint8_t msg_type; /*
 121                             00 = no output
 122                             01 = nema output
 123                             02 = binary output
 124                            */
 125 } m_output_t;
 126
 127
 128 /* 0x0E - Update Rage */
 129 #define M_RATE 0x0E
 130 typedef struct {
 131         uint8_t rate; /* Hz */
 132         uint8_t attributes; /*
 133                               0 - update ram
 134                               1 - update ram & flash
 135                             */
 136 } m_rate_t;
 137
 138
 139
 140 /* 0x37 - configure waas */
 141 #define M_WAAS 0x37
 142 typedef struct {
 143         uint8_t enable; /* 01 = enable */
 144         uint8_t attributes;
 145 } m_waas_t;
 146
 147 /* 0x3C - nav mode */
 148 #define M_NAVMODE 0x3c
 149 typedef struct {
 150         uint8_t navmode; /* 00 = car
 151                              01 = pedestrian
 152                            */
 153         uint8_t attributes;
 154 } m_navmode_t;
 155
 156
 157 #define DBG_SEND
 158 #define DBG_RECV
 159
 160 void serial1_tx_cout(uint8_t c)
 161 {
 162         debug_printf("%.2X ", c);
 163         uart_send(GPS_UART, c);
 164 }
 165
 166 void venus634_send(uint8_t type, void *payload, uint16_t len)
 167 {
 168         uint8_t crc = 0, n;
 169
 170         debug_printf("SEND ");
 171
 172         /* now send the message */
 173         /* header */
 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);
 180         crc ^= type;
 181         for (n = 0; n < len; n++) {
 182                 serial1_tx_cout(*(uint8_t *)payload);
 183                 crc ^= *(uint8_t *)payload;
 184                 payload++;
 185         }
 186         /* checksum and tail */
 187         serial1_tx_cout(crc);
 188         serial1_tx_cout(END1);
 189         serial1_tx_cout(END2);
 190
 191         debug_printf("\n");
 192 }
 193
 194 void venus634_restart(void)
 195 {
 196         m_restart_t restart;
 197
 198         memset(&restart,0,sizeof(m_restart_t));
 199         restart.start_mode = 0x03; /* COLD */
 200
 201         venus634_send(M_RESTART,&restart,sizeof(m_restart_t));
 202
 203         return;
 204 }
 205
 206 void venus634_config_serial(void)
 207 {
 208         m_serconf_t serconf;
 209
 210         memset(&serconf,0,sizeof(m_serconf_t));
 211         serconf.port = 0;
 212         serconf.baud = 4;
 213         serconf.update = 1;
 214
 215         venus634_send(M_SERCONF,&serconf,sizeof(m_serconf_t));
 216
 217         return;
 218 }
 219
 220
 221 void venus634_msg_type(void)
 222 {
 223         m_output_t output;
 224
 225         memset(&output,0,sizeof(m_output_t));
 226         output.msg_type = 0x02; /* binary msg */
 227
 228         venus634_send(M_OUTPUT,&output,sizeof(m_output_t));
 229
 230         return;
 231 }
 232
 233
 234 void venus634_rate(void)
 235 {
 236         m_rate_t rate;
 237
 238         memset(&rate,0,sizeof(m_rate_t));
 239         rate.rate = 20;
 240         //rate.rate = 0; /* ram */
 241
 242         venus634_send(M_RATE,&rate,sizeof(m_rate_t));
 243
 244         return;
 245 }
 246
 247 void venus634_waas(void)
 248 {
 249         m_waas_t waas;
 250
 251         memset(&waas,0,sizeof(m_waas_t));
 252         waas.enable = 1;
 253         waas.attributes = 0; /* ram */
 254
 255         venus634_send(M_WAAS,&waas,sizeof(m_waas_t));
 256
 257         return;
 258 }
 259
 260
 261 void venus634_navmode(void)
 262 {
 263         m_navmode_t navmode;
 264
 265         memset(&navmode,0,sizeof(m_navmode_t));
 266         navmode.navmode = 0; /* car */
 267         navmode.attributes = 0; /* ram */
 268
 269         venus634_send(M_NAVMODE,&navmode,sizeof(m_navmode_t));
 270
 271         return;
 272 }
 273
 274
 275 void venus634_nema_interval(void)
 276 {
 277         m_nemainterval_t nemainterval;
 278
 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;
 287
 288         nemainterval.attributes = 1; /* ram flash */
 289
 290         venus634_send(M_NEMAINTERVAL,&nemainterval,sizeof(m_nemainterval_t));
 291
 292         return;
 293 }
 294
 295 int8_t recv_cb(uint8_t byte)
 296 {
 297         uint16_t i;
 298
 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);
 303                         goto reset_buf;
 304                 }
 305         }
 306         else if (rxframe.cur_len == 1) {
 307                 if (byte != START2) {
 308                         debug_printf("bad start2 %.2X\n", byte);
 309                         goto reset_buf;
 310                 }
 311         }
 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;
 315         }
 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);
 320                         goto reset_buf;
 321                 }
 322         }
 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;
 326         }
 327         /* then it's the crc */
 328         else if ((rxframe.cur_len - 4) == rxframe.len) {
 329                 uint8_t crc = 0;
 330
 331                 for (i = 0; i < rxframe.len; i++)
 332                         crc ^= rxframe.data[i];
 333                 if (byte != crc) {
 334                         debug_printf("invalid crc\n");
 335                         goto reset_buf;
 336                 }
 337
 338         }
 339         /* and the last 2 bytes are stop bytes */
 340         else if ((rxframe.cur_len - 4) == (rxframe.len + 1)) {
 341                 if (byte != END1) {
 342                         debug_printf("bad end1 %.2X\n", byte);
 343                         goto reset_buf;
 344                 }
 345         }
 346         else if ((rxframe.cur_len - 4) == (rxframe.len + 2)) {
 347                 if (byte != END2) {
 348                         debug_printf("bad end2 %.2X\n", byte);
 349                         goto reset_buf;
 350                 }
 351                 debug_printf("valid frame received\n");
 352                 rxframe.cur_len = 0;
 353                 return 0;
 354         }
 355         else /* should not happen */
 356                 goto reset_buf;
 357
 358         rxframe.cur_len ++;
 359         return 1;
 360
 361  reset_buf:
 362         rxframe.cur_len = 0;
 363         return 1;
 364 }
 365
 366 int recv_msg(void)
 367 {
 368         int ret;
 369         int16_t c;
 370
 371         while (1) {
 372                 /* XXX use select for timeout */
 373                 c = uart_recv(GPS_UART);
 374                 ret = recv_cb(c);
 375                 if (ret == 0)
 376                         return 0;
 377         }
 378 }
 379
 380 int wait_ack(int msg_type)
 381 {
 382         int ret;
 383
 384         while (1) {
 385                 ret = recv_msg();
 386                 if (ret < 0)
 387                         return -1;
 388
 389                 /* retry if it's not the expected message */
 390                 if (rxframe.data[0] != 0x83 && rxframe.data[0] != 0x84)
 391                         continue;
 392                 if (rxframe.data[1] != msg_type)
 393                         continue;
 394
 395                 if (rxframe.data[0] == 0x83)
 396                         printf("ACK\n");
 397                 else if (rxframe.data[0] == 0x84)
 398                         printf("NACK\n");
 399                 else
 400                         printf("ZARB\n");
 401                 break;
 402         }
 403
 404         return 0;
 405 }
 406
 407 static int decode_gps_pos(uint8_t *buf, uint16_t len)
 408 {
 409         struct gps_pos *pos = (struct gps_pos *)buf;
 410
 411         if (len != sizeof(*pos))
 412                 return -1;
 413
 414         gps_pos.gps_week = ntohs(pos->gps_week);
 415         gps_pos.tow = ntohl(pos->tow);
 416
 417         gps_pos.latitude = ntohl(pos->latitude);
 418         gps_pos.longitude = ntohl(pos->longitude);
 419         gps_pos.altitude = ntohl(pos->altitude);
 420
 421         gps_pos.sea_altitude = ntohl(pos->sea_altitude);
 422
 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);
 428
 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);
 432
 433         return 0;
 434 }
 435
 436 /* display current GPS position stored in the global variable */
 437 static void display_gps(void)
 438 {
 439         printf("id %.2X mode %.2X svnum %.2X gpsw %.4X tow %.10"PRIu32"\t",
 440                 gps_pos.msg_id,
 441                 gps_pos.mode,
 442                 gps_pos.sv_num,
 443                 gps_pos.gps_week,
 444                 gps_pos.tow);
 445
 446         printf("lat %.8"PRIx32" long %.8"PRIx32" alt %.8"PRIx32"\n",
 447                 gps_pos.latitude,
 448                 gps_pos.longitude,
 449                 gps_pos.altitude);
 450
 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.);
 457
 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.);
 463
 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.);
 468 }
 469
 470 static void gps_venus_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
 471 {
 472         int16_t c;
 473         int ret;
 474
 475         (void)cm;
 476         (void)tim;
 477         (void)arg;
 478
 479         while (1) {
 480                 c = uart_recv_nowait(GPS_UART);
 481                 if (c < 0) /* no more char */
 482                         goto resched;
 483
 484                 ret = recv_cb(c);
 485                 if (ret == 0) {
 486                         decode_gps_pos(rxframe.data, rxframe.len);
 487                         if (0)
 488                                 display_gps();
 489                 }
 490         }
 491
 492  resched:
 493         callout_schedule(cm, tim, 2);
 494 }
 495
 496 static void venus634_configure(void)
 497 {
 498         /* ask the GPS to reset */
 499         printf("init...");
 500         venus634_restart();
 501         wait_ack(M_RESTART);
 502
 503         /* it seems we must wait that the GPS is restarted, else it doesn't work
 504          * properly */
 505         wait_ms(500);
 506
 507         printf("binmode...");
 508         venus634_msg_type();
 509         wait_ack(M_OUTPUT);
 510
 511         printf("waas...");
 512         venus634_waas();
 513         wait_ack(M_WAAS);
 514
 515         printf("rate...");
 516         venus634_rate();
 517         wait_ack(M_RATE);
 518
 519         printf("GPS configuration done !\n");
 520 }
 521
 522 /*
 523   https://www.sparkfun.com/datasheets/GPS/Modules/AN0003_v1.4.14_FlashOnly.pdf
 524 */
 525
 526 int gps_venus_init(void)
 527 {
 528         venus634_configure();
 529
 530         callout_init(&gps_timer, gps_venus_cb, NULL, GPS_PRIO);
 531         callout_schedule(&imuboard.intr_cm, &gps_timer, 2); /* every 2ms */
 532
 533         return 0;
 534 }
 535
 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)
 537 {
 538         (void)fs;
 539
 540     while(fat_read_dir(dd, dir_entry))
 541     {
 542         if(strcmp(dir_entry->long_name, name) == 0)
 543         {
 544             fat_reset_dir(dd);
 545             return 1;
 546         }
 547     }
 548
 549     return 0;
 550 }
 551
 552 static struct fat_file_struct* open_file_in_dir(struct fat_fs_struct* fs, struct fat_dir_struct* dd, const char* name)
 553 {
 554     struct fat_dir_entry_struct file_entry;
 555     if(!find_file_in_dir(fs, dd, name, &file_entry))
 556         return 0;
 557
 558     return fat_open_file(fs, &file_entry);
 559 }
 560
 561 static struct fat_file_struct *open_log_file(void)
 562 {
 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;
 569         int16_t i = 0;
 570         char name[16];
 571
 572         /* setup sd card slot */
 573         if (!sd_raw_init()) {
 574 #if SD_DEBUG
 575                 printf_P(PSTR("MMC/SD initialization failed\n"));
 576 #endif
 577                 return NULL;
 578         }
 579
 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,
 585 #else
 586                 0, 0,
 587 #endif
 588                 0);
 589
 590         if (!partition) {
 591                 /* If the partition did not open, assume the storage device
 592                  * is a "superfloppy", i.e. has no MBR.
 593                  */
 594                 partition = partition_open(sd_raw_read,
 595                         sd_raw_read_interval,
 596 #if SD_RAW_WRITE_SUPPORT
 597                         sd_raw_write,
 598                         sd_raw_write_interval,
 599 #else
 600                         0,
 601                         0,
 602 #endif
 603                         -1);
 604                 if (!partition) {
 605 #if SD_DEBUG
 606                         printf_P(PSTR("opening partition failed\n"));
 607 #endif
 608                         return NULL;
 609                 }
 610         }
 611
 612         /* open file system */
 613         fs = fat_open(partition);
 614         if (!fs) {
 615 #if SD_DEBUG
 616                 printf_P(PSTR("opening filesystem failed\n"));
 617 #endif
 618                 return NULL;
 619         }
 620
 621         /* open root directory */
 622         fat_get_dir_entry_of_path(fs, "/", &directory);
 623         dd = fat_open_dir(fs, &directory);
 624         if (!dd) {
 625 #if SD_DEBUG
 626                 printf_P(PSTR("opening root directory failed\n"));
 627 #endif
 628                 return NULL;
 629         }
 630
 631         /* print some card information as a boot message */
 632         //print_disk_info(fs);
 633
 634         printf("choose log file name\n");
 635         while (1) {
 636                 snprintf(name, sizeof(name), "log%.4d", i++);
 637                 if (!find_file_in_dir(fs, dd, name, &file_entry))
 638                         break;
 639         }
 640
 641         printf("create log file %s\n", name);
 642         if (!fat_create_file(dd, name, &file_entry)) {
 643                 printf_P(PSTR("error creating file: "));
 644         }
 645
 646         fd = open_file_in_dir(fs, dd, name);
 647         if (!fd) {
 648                 printf_P(PSTR("error opening "));
 649                 return NULL;
 650         }
 651
 652         return fd;
 653 }
 654
 655 int gps_loop(void)
 656 {
 657         struct fat_file_struct *fd = NULL;
 658         uint32_t ms;
 659         uint8_t flags;
 660         int16_t len;
 661         char buf[128];
 662
 663         if (1) {
 664                 fd = open_log_file();
 665                 if (fd == NULL)
 666                         printf("open log failed\r\n");
 667         }
 668
 669         while (1) {
 670
 671                 IRQ_LOCK(flags);
 672                 ms = global_ms;
 673                 IRQ_UNLOCK(flags);
 674
 675                 if (fd != NULL) {
 676
 677                         /* XXX copy */
 678                         len = snprintf(buf, sizeof(buf),
 679                                 "%"PRIu32" "
 680                                 "svnum %.2X lat %3.5f long %3.5f "
 681                                 "alt %3.5f sea_alt %3.5f\n",
 682                                 ms, gps_pos.sv_num,
 683                                 (double)gps_pos.latitude/10000000.,
 684                                 (double)gps_pos.longitude/10000000.,
 685                                 (double)gps_pos.altitude/100.,
 686                                 (double)gps_pos.sea_altitude/100.);
 687
 688                         if (fat_write_file(fd, (unsigned char *)buf, len) != len) {
 689                                 printf_P(PSTR("error writing to file\n"));
 690                                 return -1;
 691                         }
 692                 }
 693         }
 694
 695         return 0;
 696 }