examples/develgps/develgps.c

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