imuboard/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 #include "sd_log.h"
  20
  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__) */
  24
  25 #define GPS_UART 0
  26
  27 #define GPS_PERIOD_MS 2
  28
  29 /*
  30   https://code.google.com/p/hardware-ntp/source/browse/trunk/pcb-1/venus634.c?r=12
  31  */
  32
  33 /* some useful defines */
  34 #define START1 0xA0
  35 #define START2 0xA1
  36 #define END1 0x0D
  37 #define END2 0x0A
  38
  39 /* proccessing states */
  40
  41 #define S_RESET 0
  42 #define S_POSTRESET 1
  43 #define S_START1 2
  44 #define S_START2 3
  45 #define S_LENGTH1 4
  46 #define S_LENGTH2 5
  47 #define S_DISCARD 6
  48 #define S_COPY 7
  49 #define S_ENDCOPY 8
  50 #define S_END 9
  51 #define S_END1 10
  52
  53 #define MAX_LEN 0x200
  54 struct gps_frame {
  55         uint16_t len;
  56         uint16_t cur_len;
  57         uint8_t data[MAX_LEN];
  58 };
  59
  60 static struct gps_frame rxframe;
  61
  62 static struct gps_pos gps_pos;
  63
  64 static struct callout gps_timer;
  65
  66 /* INPUT FORMATS */
  67
  68 /* packet types */
  69 /* 0x01 - System Restart */
  70 #define M_RESTART 0x01
  71 typedef struct {
  72         uint8_t start_mode; /* 01 = Hot; 02 = Warm; 03 = Cold */
  73         uint16_t utc_year; /* >= 1980 */
  74         uint8_t utc_month;
  75         uint8_t utc_day;
  76         uint8_t utc_hour;
  77         uint8_t utc_minute;
  78         uint8_t utc_second;
  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 */
  82 } m_restart_t;
  83
  84
  85 /* 0x02 - Query Software Version */
  86 #define M_QUERYVER 0x02
  87 typedef struct {
  88         uint8_t software_type; /* 01 = System Code */
  89 } m_queryver_t;
  90
  91
  92 /* 0x05 - Serial Port Configuration */
  93 #define M_SERCONF 0x05
  94 typedef struct {
  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 */
  99 } m_serconf_t;
 100
 101
 102
 103 /* 0x08 - nmea interval */
 104 #define M_NMEAINTERVAL 0x08
 105 typedef struct {
 106         uint8_t gga;
 107         uint8_t gsa;
 108         uint8_t gsv;
 109         uint8_t gll;
 110         uint8_t rmc;
 111         uint8_t vtg;
 112         uint8_t zda;
 113         uint8_t attributes; /*
 114                               0 - sram
 115                               1 - flash
 116                              */
 117
 118 } m_nmeainterval_t;
 119
 120
 121 /* 0x09 - Query Software Version */
 122 #define M_OUTPUT 0x09
 123 typedef struct {
 124         uint8_t msg_type; /*
 125                             00 = no output
 126                             01 = nmea output
 127                             02 = binary output
 128                            */
 129         uint8_t attributes; /*
 130                               0 - update ram
 131                               1 - update ram & flash
 132                             */
 133 } m_output_t;
 134
 135
 136 /* 0x0E - Update Rage */
 137 #define M_RATE 0x0E
 138 typedef struct {
 139         uint8_t rate; /* Hz */
 140         uint8_t attributes; /*
 141                               0 - update ram
 142                               1 - update ram & flash
 143                             */
 144 } m_rate_t;
 145
 146
 147
 148 /* 0x37 - configure waas */
 149 #define M_WAAS 0x37
 150 typedef struct {
 151         uint8_t enable; /* 01 = enable */
 152         uint8_t attributes;
 153 } m_waas_t;
 154
 155 /* 0x3C - nav mode */
 156 #define M_NAVMODE 0x3c
 157 typedef struct {
 158         uint8_t navmode; /* 00 = car
 159                              01 = pedestrian
 160                            */
 161         uint8_t attributes;
 162 } m_navmode_t;
 163
 164
 165 #define DBG_SEND
 166 #define DBG_RECV
 167
 168 void serial1_tx_cout(uint8_t c)
 169 {
 170         debug_printf("%.2X ", c);
 171         uart_send(GPS_UART, c);
 172 }
 173
 174 void venus634_send(uint8_t type, void *payload, uint16_t len)
 175 {
 176         uint8_t crc = 0, n;
 177
 178         debug_printf("SEND ");
 179
 180         /* now send the message */
 181         /* header */
 182         serial1_tx_cout(START1);
 183         serial1_tx_cout(START2);
 184         serial1_tx_cout(((len+1) >> 8) & 0xff);
 185         serial1_tx_cout((len+1) & 0xff);
 186         /* type and payload */
 187         serial1_tx_cout(type);
 188         crc ^= type;
 189         for (n = 0; n < len; n++) {
 190                 serial1_tx_cout(*(uint8_t *)payload);
 191                 crc ^= *(uint8_t *)payload;
 192                 payload++;
 193         }
 194         /* checksum and tail */
 195         serial1_tx_cout(crc);
 196         serial1_tx_cout(END1);
 197         serial1_tx_cout(END2);
 198
 199         debug_printf("\n");
 200 }
 201
 202 void venus634_restart(void)
 203 {
 204         m_restart_t restart;
 205
 206         memset(&restart,0,sizeof(m_restart_t));
 207         restart.start_mode = 0x03; /* COLD */
 208
 209         venus634_send(M_RESTART,&restart,sizeof(m_restart_t));
 210
 211         return;
 212 }
 213
 214 void venus634_config_serial(void)
 215 {
 216         m_serconf_t serconf;
 217
 218         memset(&serconf,0,sizeof(m_serconf_t));
 219         serconf.port = 0;
 220         serconf.baud = 4;
 221         serconf.update = 1;
 222
 223         venus634_send(M_SERCONF,&serconf,sizeof(m_serconf_t));
 224
 225         return;
 226 }
 227
 228
 229 void venus634_msg_type(void)
 230 {
 231         m_output_t output;
 232
 233         memset(&output,0,sizeof(m_output_t));
 234         output.msg_type = 0x02; /* binary msg */
 235         output.attributes = 0; /* ram, init may freeze when update in flash */
 236
 237         venus634_send(M_OUTPUT,&output,sizeof(m_output_t));
 238
 239         return;
 240 }
 241
 242
 243 void venus634_rate(void)
 244 {
 245         m_rate_t rate;
 246
 247         memset(&rate,0,sizeof(m_rate_t));
 248         rate.rate = 20;
 249         rate.attributes = 0; /* ram */
 250
 251         venus634_send(M_RATE,&rate,sizeof(m_rate_t));
 252
 253         return;
 254 }
 255
 256 /* Wide Area Augmentation System: use ground stations to increase the precision
 257  * of gps position */
 258 void venus634_waas(void)
 259 {
 260         m_waas_t waas;
 261
 262         memset(&waas,0,sizeof(m_waas_t));
 263         waas.enable = 1;
 264         waas.attributes = 0; /* ram */
 265
 266         venus634_send(M_WAAS,&waas,sizeof(m_waas_t));
 267
 268         return;
 269 }
 270
 271 /* Tell we are a car instead of a pedestrian */
 272 void venus634_navmode(void)
 273 {
 274         m_navmode_t navmode;
 275
 276         memset(&navmode,0,sizeof(m_navmode_t));
 277         navmode.navmode = 0; /* car */
 278         navmode.attributes = 0; /* ram */
 279
 280         venus634_send(M_NAVMODE,&navmode,sizeof(m_navmode_t));
 281
 282         return;
 283 }
 284
 285 /* frequency of NMEA messages */
 286 void venus634_nmea_interval(void)
 287 {
 288         m_nmeainterval_t nmeainterval;
 289
 290         memset(&nmeainterval,0,sizeof(m_nmeainterval_t));
 291
 292         /* set frequency for nmea: 1 = once every one position fix, 2 = once
 293          * every two position fix, ... */
 294         nmeainterval.gga = 1; /* GPGGA interval - GPS Fix Data*/
 295         nmeainterval.gsa = 1; /* GNSS DOPS and Active Satellites */
 296         nmeainterval.gsv = 1; /* GNSS Satellites in View */
 297         nmeainterval.gll = 1; /* Geographic Position - Latitude longitude */
 298         nmeainterval.rmc = 1; /* Recomended Minimum Specific GNSS Sentence */
 299         nmeainterval.vtg = 1; /* Course Over Ground and Ground Speed */
 300         nmeainterval.zda = 1; /* Time & Date*/
 301
 302         nmeainterval.attributes = 1; /* ram flash */
 303
 304         venus634_send(M_NMEAINTERVAL,&nmeainterval,sizeof(m_nmeainterval_t));
 305
 306         return;
 307 }
 308
 309 int8_t recv_cb(uint8_t byte)
 310 {
 311         uint16_t i;
 312
 313         /* bytes 0 and 1 are start bytes */
 314         if (rxframe.cur_len == 0) {
 315                 if (byte != START1) {
 316                         debug_printf("bad start1 %.2X\n", byte);
 317                         goto reset_buf;
 318                 }
 319         }
 320         else if (rxframe.cur_len == 1) {
 321                 if (byte != START2) {
 322                         debug_printf("bad start2 %.2X\n", byte);
 323                         goto reset_buf;
 324                 }
 325         }
 326         /* bytes 2 and 3 are the length of frame in network order */
 327         else if (rxframe.cur_len == 2) {
 328                 rxframe.len = (uint16_t)byte << 8;
 329         }
 330         else if (rxframe.cur_len == 3) {
 331                 rxframe.len |= (uint16_t)byte;
 332                 if (rxframe.len > MAX_LEN) {
 333                         debug_printf("bad len %d\n", rxframe.len);
 334                         goto reset_buf;
 335                 }
 336         }
 337         /* next bytes are data (the 4 below is the size of header) */
 338         else if ((rxframe.cur_len - 4) < rxframe.len) {
 339                 rxframe.data[rxframe.cur_len - 4] = byte;
 340         }
 341         /* then it's the crc */
 342         else if ((rxframe.cur_len - 4) == rxframe.len) {
 343                 uint8_t crc = 0;
 344
 345                 for (i = 0; i < rxframe.len; i++)
 346                         crc ^= rxframe.data[i];
 347                 if (byte != crc) {
 348                         debug_printf("invalid crc\n");
 349                         goto reset_buf;
 350                 }
 351
 352         }
 353         /* and the last 2 bytes are stop bytes */
 354         else if ((rxframe.cur_len - 4) == (rxframe.len + 1)) {
 355                 if (byte != END1) {
 356                         debug_printf("bad end1 %.2X\n", byte);
 357                         goto reset_buf;
 358                 }
 359         }
 360         else if ((rxframe.cur_len - 4) == (rxframe.len + 2)) {
 361                 if (byte != END2) {
 362                         debug_printf("bad end2 %.2X\n", byte);
 363                         goto reset_buf;
 364                 }
 365                 debug_printf("valid frame received\n");
 366                 rxframe.cur_len = 0;
 367                 return 0;
 368         }
 369         else /* should not happen */
 370                 goto reset_buf;
 371
 372         rxframe.cur_len ++;
 373         return 1;
 374
 375  reset_buf:
 376         rxframe.cur_len = 0;
 377         return 1;
 378 }
 379
 380 int recv_msg(void)
 381 {
 382         int ret;
 383         int16_t c;
 384
 385         while (1) {
 386                 /* XXX use select for timeout */
 387                 c = uart_recv(GPS_UART);
 388                 ret = recv_cb(c);
 389                 if (ret == 0)
 390                         return 0;
 391         }
 392 }
 393
 394 int wait_ack(int msg_type)
 395 {
 396         int ret;
 397
 398         while (1) {
 399                 ret = recv_msg();
 400                 if (ret < 0)
 401                         return -1;
 402
 403                 /* retry if it's not the expected message */
 404                 if (rxframe.data[0] != 0x83 && rxframe.data[0] != 0x84)
 405                         continue;
 406                 if (rxframe.data[1] != msg_type)
 407                         continue;
 408
 409                 if (rxframe.data[0] == 0x83)
 410                         printf_P(PSTR("ACK\n"));
 411                 else if (rxframe.data[0] == 0x84)
 412                         printf_P(PSTR("NACK\n"));
 413                 else
 414                         printf_P(PSTR("ZARB\n"));
 415                 break;
 416         }
 417
 418         return 0;
 419 }
 420
 421 static int decode_gps_pos(uint8_t *buf, uint16_t len)
 422 {
 423         struct gps_pos *pos = (struct gps_pos *)buf;
 424         uint8_t irq_flags;
 425
 426         if (len != sizeof(*pos))
 427                 return -1;
 428
 429         if (pos->msg_id != 0xA8)
 430                 return -1;
 431
 432         /* fix endianness inside the frame */
 433         pos->gps_week = ntohs(pos->gps_week);
 434         pos->tow = ntohl(pos->tow);
 435
 436         pos->latitude = ntohl(pos->latitude);
 437         pos->longitude = ntohl(pos->longitude);
 438         pos->altitude = ntohl(pos->altitude);
 439
 440         pos->sea_altitude = ntohl(pos->sea_altitude);
 441
 442         pos->gdop = ntohs(pos->gdop);
 443         pos->pdop = ntohs(pos->pdop);
 444         pos->hdop = ntohs(pos->hdop);
 445         pos->vdop = ntohs(pos->vdop);
 446         pos->tdop = ntohs(pos->tdop);
 447
 448         pos->ecef_vx = ntohl(pos->ecef_vx);
 449         pos->ecef_vy = ntohl(pos->ecef_vy);
 450         pos->ecef_vz = ntohl(pos->ecef_vz);
 451
 452         /* update global structure */
 453         IRQ_LOCK(irq_flags);
 454         memcpy(&gps_pos, pos, sizeof(gps_pos));
 455         IRQ_UNLOCK(irq_flags);
 456
 457         return 0;
 458 }
 459
 460 /* display current GPS position stored in the global variable */
 461 static void display_gps(void)
 462 {
 463         /* no need to call ntohs/ntohl because the frame is already fixed by
 464          * decode_gps_pos() */
 465         printf_P(PSTR("id %.2X mode %.2X svnum %.2X gpsw %.4X "
 466                         "tow %.10"PRIu32"\t"),
 467                 gps_pos.msg_id,
 468                 gps_pos.mode,
 469                 gps_pos.sv_num,
 470                 gps_pos.gps_week,
 471                 gps_pos.tow);
 472
 473         printf_P(PSTR("lat %.8"PRIx32" long %.8"PRIx32" alt %.8"PRIx32"\n"),
 474                 gps_pos.latitude,
 475                 gps_pos.longitude,
 476                 gps_pos.altitude);
 477
 478         printf_P(PSTR("gdop %3.3f pdop %3.3f hdop %3.3f vdop %3.3f "
 479                         "tdop %3.3f\n"),
 480                 (double)gps_pos.gdop/100.,
 481                 (double)gps_pos.pdop/100.,
 482                 (double)gps_pos.hdop/100.,
 483                 (double)gps_pos.vdop/100.,
 484                 (double)gps_pos.tdop/100.);
 485
 486         printf_P(PSTR("lat %3.5f long %3.5f alt %3.5f sea_alt %3.5f\n"),
 487                 (double)gps_pos.latitude/10000000.,
 488                 (double)gps_pos.longitude/10000000.,
 489                 (double)gps_pos.altitude/100.,
 490                 (double)gps_pos.sea_altitude/100.);
 491
 492         printf_P(PSTR("vx %3.3f vy %3.3f vz %3.3f\n"),
 493                 (double)gps_pos.ecef_vx/100.,
 494                 (double)gps_pos.ecef_vy/100.,
 495                 (double)gps_pos.ecef_vz/100.);
 496 }
 497
 498 /* called on timer event */
 499 static void gps_venus_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
 500 {
 501         static int16_t last_valid = 0;
 502         static uint8_t is_valid = 0;
 503         int16_t ms, diff;
 504         int16_t c;
 505         int8_t ret;
 506         uint8_t irq_flags;
 507
 508         (void)cm;
 509         (void)tim;
 510         (void)arg;
 511
 512         IRQ_LOCK(irq_flags);
 513         ms = global_ms;
 514         IRQ_UNLOCK(irq_flags);
 515
 516         while (1) {
 517                 c = uart_recv_nowait(GPS_UART);
 518                 if (c < 0) /* no more char */
 519                         break;
 520
 521                 ret = recv_cb(c);
 522                 if (ret == 0) {
 523                         decode_gps_pos(rxframe.data, rxframe.len);
 524                         if (0)
 525                                 display_gps();
 526
 527                         last_valid = ms;
 528                         is_valid = 1;
 529                 }
 530         }
 531
 532         diff = ms - last_valid;
 533         if (is_valid == 1 && diff > 100) {
 534
 535                 is_valid = 0;
 536                 /* update global structure */
 537                 IRQ_LOCK(irq_flags);
 538                 memset(&gps_pos, 0, sizeof(gps_pos));
 539                 gps_pos.latitude = 0xffffffff;
 540                 IRQ_UNLOCK(irq_flags);
 541         }
 542
 543         callout_schedule(cm, tim, GPS_PERIOD_MS);
 544 }
 545
 546 static void venus634_configure(void)
 547 {
 548         /* ask the GPS to reset */
 549         printf_P(PSTR("init..."));
 550         venus634_restart();
 551         wait_ack(M_RESTART);
 552
 553         /* it seems we must wait that the GPS is restarted, else it doesn't work
 554          * properly */
 555         wait_ms(500);
 556
 557         printf_P(PSTR("binmode..."));
 558         venus634_msg_type();
 559         wait_ack(M_OUTPUT);
 560
 561         printf_P(PSTR("waas..."));
 562         venus634_waas();
 563         wait_ack(M_WAAS);
 564
 565         printf_P(PSTR("rate..."));
 566         venus634_rate();
 567         wait_ack(M_RATE);
 568
 569         printf_P(PSTR("GPS configuration done !\n"));
 570 }
 571
 572 /*
 573   https://www.sparkfun.com/datasheets/GPS/Modules/AN0003_v1.4.14_FlashOnly.pdf
 574 */
 575
 576 int gps_venus_init(void)
 577 {
 578         venus634_configure();
 579
 580         callout_init(&gps_timer, gps_venus_cb, NULL, GPS_PRIO);
 581         callout_schedule(&imuboard.intr_cm, &gps_timer, GPS_PERIOD_MS); /* every 2ms */
 582
 583         return 0;
 584 }
 585
 586 void gps_get_pos(struct gps_pos *pos)
 587 {
 588         memcpy(pos, &gps_pos, sizeof(*pos));
 589 }
 590
 591 int gps_log(void)
 592 {
 593         uint32_t ms;
 594         uint8_t flags, prio;
 595         int16_t len;
 596         char buf[128];
 597         struct gps_pos pos;
 598
 599         IRQ_LOCK(flags);
 600         ms = global_ms;
 601         IRQ_UNLOCK(flags);
 602
 603         /* get position (prevent modification of gps pos during copy) */
 604         prio = callout_mgr_set_prio(&imuboard.intr_cm, GPS_PRIO);
 605         gps_get_pos(&pos);
 606         callout_mgr_restore_prio(&imuboard.intr_cm, prio);
 607
 608         /* XXX copy */
 609         len = snprintf(buf, sizeof(buf),
 610                 "%"PRIu32" "
 611                 "svnum %.2X lat %3.5f long %3.5f "
 612                 "alt %3.5f sea_alt %3.5f\n",
 613                 ms, gps_pos.sv_num,
 614                 (double)gps_pos.latitude / 10000000.,
 615                 (double)gps_pos.longitude / 10000000.,
 616                 (double)gps_pos.altitude / 100.,
 617                 (double)gps_pos.sea_altitude / 100.);
 618
 619
 620         if (sd_log_enabled()) {
 621                 if (sd_log_write(buf, len) != len) {
 622                         printf_P(PSTR("error writing to file\n"));
 623                         return -1;
 624                 }
 625         }
 626         else {
 627                 printf_P(PSTR("%s"), buf);
 628         }
 629
 630         return 0;
 631 }