boards/ek-lm3s1968/examples/gps/main.c

   1 #include <cpu/irq.h>
   2
   3 #include <drv/lcd_rit128x96.h>
   4 #include <drv/timer.h>
   5 #include <drv/ser.h>
   6 #include <drv/flash_lm3s.h>
   7 #include <drv/kbd.h>
   8
   9 #include <kern/proc.h>
  10 #include <kern/sem.h>
  11
  12 #include <net/nmea.h>
  13
  14 #include <gfx/font.h>
  15 #include <gfx/text.h>
  16 #include <gfx/gfx_p.h> /* BM_PLOT() */
  17 #include <gui/menu.h>
  18 #include <math.h>
  19
  20 #include "compass.h"
  21
  22 /* OLED/GUI stuff */
  23 #include "logo.c"
  24 #define KEY_MASK (K_UP | K_DOWN | K_LEFT | K_RIGHT | K_OK)
  25 extern Font font_gohu;
  26 static uint8_t raster[RAST_SIZE(LCD_WIDTH, LCD_HEIGHT)];
  27 static Bitmap lcd_bitmap;
  28
  29 #define SCRSVR_TIME 60000
  30 static ticks_t scrsvr_timestamp;
  31 static bool is_lcd_off;
  32
  33 /* Serial and NMEA stuff */
  34 static Serial ser_port;
  35 static nmeap_context_t nmea;
  36 static NmeaGga gga;
  37 static NmeaRmc rmc;
  38 static NmeaVtg vtg;
  39 static bool nmea_update;
  40
  41 static long prev_b;
  42 static long lat, lon;
  43 static long target_lat, target_lon;
  44
  45 /* Storage stuff */
  46 #define GPS_POS_MAGIC 0xdeadbeef
  47 static FlashLM3S flash;
  48
  49 static void flash_load_target(void)
  50 {
  51         uint32_t magic;
  52
  53         kfile_seek(&flash.fd, -FLASH_PAGE_SIZE_BYTES, KSM_SEEK_END);
  54         kfile_read(&flash.fd, &magic, sizeof(magic));
  55         if (magic == GPS_POS_MAGIC)
  56         {
  57                 kfile_read(&flash.fd, &target_lat, sizeof(target_lat));
  58                 kfile_read(&flash.fd, &target_lon, sizeof(target_lon));
  59         }
  60 }
  61
  62 static void flash_save_target(void)
  63 {
  64         const uint32_t magic = GPS_POS_MAGIC;
  65
  66         kfile_seek(&flash.fd, -FLASH_PAGE_SIZE_BYTES, KSM_SEEK_END);
  67         kfile_write(&flash.fd, &magic, sizeof(magic));
  68         kfile_write(&flash.fd, &target_lat, sizeof(target_lat));
  69         kfile_write(&flash.fd, &target_lon, sizeof(target_lon));
  70         kfile_flush(&flash.fd);
  71 }
  72
  73 /* Status LED management */
  74 static void led_init(void)
  75 {
  76         SYSCTL_RCGC2_R = SYSCTL_RCGC2_GPIOG;
  77         (void)SYSCTL_RCGC2_R;
  78         GPIO_PORTG_DIR_R = 0x04;
  79         GPIO_PORTG_DEN_R = 0x04;
  80 }
  81
  82 INLINE void led_on(void)
  83 {
  84         GPIO_PORTG_DATA_R |= 0x04;
  85 }
  86
  87 INLINE void led_off(void)
  88 {
  89         GPIO_PORTG_DATA_R &= ~0x04;
  90 }
  91
  92 /* Display management */
  93 INLINE void video_off(void)
  94 {
  95         unsigned long delta =
  96                 (long)ticks_to_ms(timer_clock_unlocked()) -
  97                 (long)scrsvr_timestamp;
  98
  99         if (!is_lcd_off && delta > SCRSVR_TIME)
 100         {
 101                 rit128x96_off();
 102                 is_lcd_off = true;
 103         }
 104 }
 105
 106 INLINE void video_on(void)
 107 {
 108         if (is_lcd_off)
 109         {
 110                 rit128x96_on();
 111                 is_lcd_off = false;
 112         }
 113         scrsvr_timestamp = ticks_to_ms(timer_clock_unlocked());
 114 }
 115
 116 INLINE void repaint(Bitmap *bm)
 117 {
 118         rit128x96_blitBitmap(bm);
 119 }
 120
 121 INLINE keymask_t keypad_peek(void)
 122 {
 123         keymask_t key = kbd_peek();
 124
 125         if (key & KEY_MASK)
 126         {
 127                 if (is_lcd_off)
 128                         key = 0;
 129                 video_on();
 130         }
 131         return key;
 132 }
 133
 134 /* Status LED thread */
 135 static void NORETURN led_process(void)
 136 {
 137         while (1)
 138         {
 139                 video_off();
 140                 if (!nmea_update)
 141                 {
 142                         timer_delay(1000);
 143                         continue;
 144                 }
 145                 led_on();
 146                 timer_delay(100);
 147                 led_off();
 148                 nmea_update = false;
 149         }
 150 }
 151
 152 /* NMEA parser */
 153 static void nmea_callback(nmeap_context_t *context, void *data, void *user_data)
 154 {
 155         (void)context;
 156         (void)data;
 157         (void)user_data;
 158
 159         lat = (long)gga.latitude;
 160         lon = (long)gga.longitude;
 161
 162         nmea_update = true;
 163 }
 164
 165 static void NORETURN ser_process(void)
 166 {
 167         while (1)
 168         {
 169                 nmea_poll(&nmea, &ser_port.fd);
 170                 kfile_clearerr(&ser_port.fd);
 171         }
 172 }
 173
 174 /* Target position screen */
 175 static void target(Bitmap *bm)
 176 {
 177         const long STEP = 10000000,
 178                 MAX_LAT = 90000000, MIN_LAT = -90000000,
 179                 MAX_LON = 180000000, MIN_LON = -180000000;
 180         long step = STEP, target;
 181         int row = 0, pos = 1;
 182         keymask_t key;
 183
 184         gfx_bitmapClear(bm);
 185         video_on();
 186         text_xprintf(bm, 0, 0,
 187                 STYLEF_UNDERLINE | TEXT_CENTER | TEXT_FILL,
 188                 "Target position");
 189         while (1)
 190         {
 191                 if (!is_lcd_off)
 192                 {
 193                         text_xprintf(bm, 3, 0, TEXT_FILL,
 194                                         "Lat:  %02ld.%06ld %c %s",
 195                                         ABS(target_lat) / 1000000L,
 196                                         ABS(target_lat) % 1000000,
 197                                         target_lat >= 0 ? 'N' : 'S',
 198                                         row == 0 ? "<-" : "");
 199                         text_xprintf(bm, row ? 4 : 6, 0,
 200                                         TEXT_FILL | TEXT_CENTER, " ");
 201                         text_xprintf(bm, row ? 6 : 4, 0,
 202                                         TEXT_FILL, "%*c",
 203                                         (step < 1000000L) ?
 204                                                 pos + 7 : pos + 6, '^');
 205                         text_xprintf(bm, 5, 0, TEXT_FILL,
 206                                         "Lon: %03ld.%06ld %c %s",
 207                                         ABS(target_lon) / 1000000L,
 208                                         ABS(target_lon) % 1000000L,
 209                                         target_lon >= 0 ? 'E' : 'W',
 210                                         row == 1 ? "<-" : "");
 211                         repaint(bm);
 212                 }
 213                 key = keypad_peek();
 214                 if (key & K_UP)
 215                 {
 216                         if (row == 0)
 217                         {
 218                                 target = target_lat + step;
 219                                 if (target <= MAX_LAT)
 220                                 {
 221                                         if (target_lat < 0 && target > 0)
 222                                                 target_lat = ABS(target_lat) +
 223                                                                 step;
 224                                         else
 225                                                 target_lat = target;
 226                                 }
 227                         }
 228                         else
 229                         {
 230                                 target = target_lon + step;
 231                                 if (target <= MAX_LON)
 232                                 {
 233                                         if (target_lon < 0 && target > 0)
 234                                                 target_lon = ABS(target_lon) +
 235                                                                 step;
 236                                         else
 237                                                 target_lon = target;
 238                                 }
 239                         }
 240                 }
 241                 else if (key & K_DOWN)
 242                 {
 243                         if (row == 0)
 244                         {
 245                                 target = target_lat - step;
 246                                 if (target >= MIN_LAT)
 247                                 {
 248                                         if (target_lat > 0 && target < 0)
 249                                                 target_lat = -ABS(target_lat) -
 250                                                                 step;
 251                                         else
 252                                                 target_lat = target;
 253                                 }
 254                         }
 255                         else
 256                         {
 257                                 target = target_lon - step;
 258                                 if (target >= MIN_LON)
 259                                 {
 260                                         if (target_lon > 0 && target < 0)
 261                                                 target_lon = -ABS(target_lon) -
 262                                                                 step;
 263                                         else
 264                                                 target_lon = target;
 265                                 }
 266                         }
 267                 }
 268                 else if (key & K_LEFT)
 269                 {
 270                         if (step < STEP)
 271                         {
 272                                 step *= 10;
 273                                 pos--;
 274                         }
 275                 }
 276                 else if (key & K_RIGHT)
 277                 {
 278                         if (step >= 10)
 279                         {
 280                                 step /= 10;
 281                                 pos++;
 282                         }
 283                 }
 284                 else if (key & K_OK)
 285                 {
 286                         if (row++ == 1)
 287                                 break;
 288                          /* Move to longigude */
 289                 }
 290                 cpu_relax();
 291         }
 292         flash_save_target();
 293 }
 294
 295 /* Compass management */
 296 static void draw_compass(Bitmap *bm)
 297 {
 298         const int R = LCD_HEIGHT / 3, R_SMALL = 10;
 299         long x, y, x1, y1, x2, y2, d;
 300         int i;
 301
 302         d = distance(lat / 1E6, lon / 1E6, target_lat / 1E6, target_lon / 1E6);
 303
 304         x = (long)((float)R * (1 + sin(deg2rad((float)prev_b))));
 305         y = (long)((float)R * (1 - cos(deg2rad((float)prev_b))));
 306
 307         x1 = R - R_SMALL + (long)((float)R_SMALL *
 308                 (1 + sin(deg2rad((float)prev_b - 120.0))));
 309         y1 = R - R_SMALL + (long)((float)R_SMALL *
 310                 (1 - cos(deg2rad((float)prev_b - 120.0))));
 311         x2 = R - R_SMALL + (long)((float)R_SMALL *
 312                 (1 + sin(deg2rad((float)prev_b + 120.0))));
 313         y2 = R - R_SMALL + (long)((float)R_SMALL *
 314                 (1 - cos(deg2rad((float)prev_b + 120.0))));
 315
 316         gfx_bitmapClear(bm);
 317         /* Print direction heading and degrees */
 318         text_xprintf(bm, 0, 5, 0, "%s", "N");
 319         text_xprintf(bm, 0, 0, TEXT_RIGHT, "%s", compass_heading(prev_b));
 320         text_xprintf(bm, 1, 0, TEXT_RIGHT, "%ld deg.", prev_b);
 321         /* Print distance */
 322         text_xprintf(bm, 2, 0, TEXT_RIGHT, "%ld %s",
 323                         d >= 1000 ? d / 1000 : d,
 324                         d >= 1000 ? "Km" : "m");
 325         /* Print current and target position */
 326         text_xprintf(bm, 6, 0, TEXT_FILL, "%02ld.%06ld%c%03ld.%06ld%c",
 327                         ABS(lat) / 1000000L,
 328                         ABS(lat) % 1000000,
 329                         lat >= 0 ? 'N' : 'S',
 330                         ABS(lon) / 1000000L,
 331                         ABS(lon) % 1000000L,
 332                         lon >= 0 ? 'E' : 'W');
 333         text_xprintf(bm, 7, 0, TEXT_FILL, "%02ld.%06ld%c%03ld.%06ld%c",
 334                         ABS(target_lat) / 1000000L,
 335                         ABS(target_lat) % 1000000,
 336                         target_lat >= 0 ? 'N' : 'S',
 337                         ABS(target_lon) / 1000000L,
 338                         ABS(target_lon) % 1000000L,
 339                         target_lon >= 0 ? 'E' : 'W');
 340         /* Draw the circle */
 341         for (i = 0; i < 360; i++)
 342                 BM_PLOT(bm,
 343                         (long)((float)R * (1 + sin(deg2rad((float)i)))),
 344                         (long)((float)R * (1 - cos(deg2rad((float)i)))));
 345         /* Draw the needle */
 346         gfx_rectFill(bm, R - 2, R - 2, R + 2, R + 2);
 347         gfx_line(bm, R, R, x1, y1);
 348         gfx_line(bm, R, R, x2, y2);
 349         gfx_line(bm, x1, y1, x, y);
 350         gfx_line(bm, x2, y2, x, y);
 351
 352         repaint(bm);
 353 }
 354
 355 static void compass(Bitmap *bm)
 356 {
 357         long b, inc;
 358
 359         video_on();
 360         while (1)
 361         {
 362                 if (!is_lcd_off)
 363                 {
 364                         b = bearing(lat / 1E6, lon / 1E6,
 365                                         target_lat / 1E6, target_lon / 1E6);
 366                         inc = ABS(b - prev_b) < 360 - ABS(b - prev_b) ?  1 : -1;
 367                         /* Compass animation */
 368                         if (b < prev_b)
 369                                 inc = -inc;
 370                         while (prev_b != b)
 371                         {
 372                                 prev_b = (prev_b + inc) % 360;
 373                                 if (prev_b < 0)
 374                                         prev_b = 359;
 375                                 if (!(prev_b % 5))
 376                                         draw_compass(bm);
 377                                 if (keypad_peek() & KEY_MASK)
 378                                         return;
 379                         }
 380                         draw_compass(bm);
 381                 }
 382                 cpu_relax();
 383                 if (keypad_peek() & KEY_MASK)
 384                         return;
 385         }
 386 }
 387
 388 /* GPS receiver status */
 389 static const char *gps_fix[] =
 390 {
 391         "invalid",
 392         "GPS",
 393         "DGPS",
 394         "PPS",
 395         "RTK",
 396         "float-RTK",
 397         "estimated",
 398         "manual",
 399         "simulation",
 400 };
 401
 402 static void gps_data(Bitmap *bm)
 403 {
 404         struct tm tm;
 405         char buf[32];
 406
 407         video_on();
 408         gfx_bitmapClear(bm);
 409         while (1)
 410         {
 411                 if (!is_lcd_off)
 412                 {
 413                         if (!rmc.time)
 414                         {
 415                                 text_xprintf(bm, 3, 0,
 416                                         TEXT_CENTER | TEXT_FILL, "No GPS data");
 417                         }
 418                         else
 419                         {
 420                                 gmtime_r(&rmc.time, &tm);
 421
 422                                 text_xprintf(bm, 0, 0, TEXT_FILL,
 423                                                 "Lat. %ld.%06ld%c",
 424                                                 ABS(lat) / 1000000L,
 425                                                 ABS(lat) % 1000000,
 426                                                 lat >= 0 ? 'N' : 'S');
 427                                 text_xprintf(bm, 1, 0, TEXT_FILL,
 428                                                 "Lon. %ld.%06ld%c",
 429                                                 ABS(lon) / 1000000L,
 430                                                 ABS(lon) % 1000000L,
 431                                                 lon >= 0 ? 'E' : 'W');
 432                                 text_xprintf(bm, 2, 0, TEXT_FILL,
 433                                                 "Alt. %d", gga.altitude);
 434                                 text_xprintf(bm, 3, 0, TEXT_FILL,
 435                                                 "Speed: %d", vtg.km_speed);
 436                                 if (gga.quality < countof(gps_fix))
 437                                         text_xprintf(bm, 4, 0, TEXT_FILL,
 438                                                         "Fix: %s",
 439                                                         gps_fix[gga.quality]);
 440                                 else
 441                                         text_xprintf(bm, 4, 0, TEXT_FILL,
 442                                                         "Fix: %d", gga.quality);
 443                                 text_xprintf(bm, 5, 0, TEXT_FILL,
 444                                                 "Satellites: %d",
 445                                                 gga.satellites);
 446                                 strftime(buf, sizeof(buf),
 447                                                 "Date: %Y-%m-%d %a", &tm);
 448                                 text_xprintf(bm, 6, 0, TEXT_FILL, "%s", buf);
 449                                 strftime(buf, sizeof(buf),
 450                                                 "Time: %H:%M:%S (UTC)", &tm);
 451                                 text_xprintf(bm, 7, 0, TEXT_FILL, "%s", buf);
 452                         }
 453                         repaint(bm);
 454                 }
 455                 if (keypad_peek() & KEY_MASK)
 456                         break;
 457                 cpu_relax();
 458         }
 459 }
 460
 461 /* BeRTOS screen */
 462 static void about(Bitmap *bm)
 463 {
 464         gfx_bitmapClear(bm);
 465         video_on();
 466         text_xprintf(bm, 7, 0,
 467                 STYLEF_UNDERLINE | TEXT_CENTER | TEXT_FILL,
 468                 "http://www.bertos.org");
 469         repaint(bm);
 470         if (!is_lcd_off)
 471         {
 472                 const uint8_t *p = &logo[BITMAP_HEADER_SIZE];
 473                 uint8_t h = logo[BITMAP_HEIGHT_OFFSET];
 474                 uint8_t w = logo[BITMAP_WIDTH_OFFSET];
 475                 uint8_t r;
 476
 477                 for (r = 0; r < h; r++)
 478                 {
 479                         rit128x96_blitRaw(p,
 480                                 (128 - w) / 2, 70 - r, w, 1);
 481                         p += w / 2;
 482                 }
 483         }
 484         while (!(keypad_peek() & KEY_MASK))
 485                 cpu_relax();
 486 }
 487
 488 static struct MenuItem main_items[] =
 489 {
 490         {(const_iptr_t)"Target", 0, (MenuHook)target, (iptr_t)&lcd_bitmap},
 491         {(const_iptr_t)"Compass", 0, (MenuHook)compass, (iptr_t)&lcd_bitmap},
 492         {(const_iptr_t)"GPS data", 0, (MenuHook)gps_data, (iptr_t)&lcd_bitmap},
 493         {(const_iptr_t)"About...", 0, (MenuHook)about, (iptr_t)&lcd_bitmap},
 494         {(const_iptr_t)0, 0, NULL, (iptr_t)NULL}
 495 };
 496
 497 static struct Menu main_menu =
 498 {
 499         main_items, "DevelGPS v0.1", MF_STICKY | MF_SAVESEL, &lcd_bitmap, 0
 500 };
 501
 502 static void init(void)
 503 {
 504         IRQ_ENABLE;
 505
 506         kdbg_init();
 507         timer_init();
 508         proc_init();
 509
 510         scrsvr_timestamp = ticks_to_ms(timer_clock_unlocked());
 511         led_init();
 512
 513         flash_lm3sInit(&flash);
 514         flash_load_target();
 515
 516         ser_init(&ser_port, SER_UART1);
 517         ser_setbaudrate(&ser_port, 38400);
 518
 519         nmeap_init(&nmea, NULL);
 520         nmeap_addParser(&nmea, "GPGGA", nmea_gpgga, nmea_callback, &gga);
 521         nmeap_addParser(&nmea, "GPRMC", nmea_gprmc, nmea_callback, &rmc);
 522         nmeap_addParser(&nmea, "GPVTG", nmea_gpvtg, nmea_callback, &vtg);
 523
 524         rit128x96_init();
 525         gfx_bitmapInit(&lcd_bitmap, raster, LCD_WIDTH, LCD_HEIGHT);
 526         gfx_setFont(&lcd_bitmap, &font_gohu);
 527         repaint(&lcd_bitmap);
 528
 529         kbd_init();
 530 }
 531
 532 int main(void)
 533 {
 534         init();
 535
 536         proc_new(led_process, NULL, KERN_MINSTACKSIZE * 2, NULL);
 537         proc_new(ser_process, NULL, KERN_MINSTACKSIZE * 2, NULL);
 538
 539         while (1)
 540         {
 541                 iptr_t res = menu_handle(&main_menu);
 542                 if (res != MENU_TIMEOUT)
 543                         video_on();
 544                 cpu_relax();
 545         }
 546 }