Marinus APRS Map on DUE/DigiX

// Marinus APRS Map Display
// Copyright 2012 Leigh L. Klotz, Jr.
// License: http://www.opensource.org/licenses/mit-license

#include <Adafruit_GFX.h>
#include <Adafruit_ILI9340.h> // Using 2.2" TFT ILI9340
#include <SPI.h>
#include <SD.h>
#include <MicroAPRS.h>        // Using MicroAPRS libs

#define SD_CS    87          // Chip select line for DigiX SD card
#define _cs 48
#define _rst 49
#define _dc 50

// LCD Size
// #define ILI9340_TFTHEIGHT 320
// #define ILI9340_TFTWIDTH  240

#define LCD_HEIGHT ILI9340_TFTHEIGHT
#define LCD_WIDTH ILI9340_TFTWIDTH
//#define LCD_HEIGHT 160
//#define LCD_WIDTH 128

// Colors
#define CALL_TEXT_COLOR ILI9340_RED
#define ICON_COLOR ILI9340_BLUE
#define LINE_COLOR ILI9340_BLUE

// APRS Buffers
#define BUFLEN (260)
char packet[BUFLEN];
int buflen = 0;

// Maps and calls
int last_map_n;
char last_call[10];
int last_pix_down;
int last_pix_right;

// MicroAPRS decoded fields
char *call, *posit, *pmsgTo, *pmsg;
char type, pmsgID;
long lat, lon;

// POI and Map info
// Location of center tile upper left corner
long POI_TL_LAT;
long POI_TL_LON;

int DEGREES_PER_PIXEL_LAT;
int DEGREES_PER_PIXEL_LON;

byte MAP_WIDTH_IN_TILES;
byte MAP_HEIGHT_IN_TILES;

Adafruit_ILI9340 tft = Adafruit_ILI9340(_cs, _dc, _rst);

//ArgentRadioShield  = MicroAPRS(&Serial);
// Serial3 Rx3 is pin besides 96
// Modem connected to Serial3 of DigiX
MicroAPRS microaprs = MicroAPRS(&Serial3);


#define FLOOR(a,b) (a < 0) ? (a/b)-1 : (a/b)

// Turn on/off DEUBGGING
#define DEBUG true


void setup(void) {

#ifdef DEBUG
      Serial.begin(9600);
        while (!Serial) ;
      Serial.println("DEBUG ON");;
#endif

  Serial3.begin(9600);

  tft.begin();
  tft.setRotation(0);
  tft.setTextWrap(true);
  tft.setCursor(0,0);
  tft.fillScreen(ILI9340_BLUE);
  tft.setTextColor(ILI9340_WHITE);
  tft.print(F("Marinus APRS Mapper\n\n(C) 2012 WA5ZNU\nMIT license\n\n"
          "Data imagery and map\n"
          "information provided\n"
          "by MapQuest,\n"
          "openstreetmap.org,\n"
          "and contributors:\nCC-BY-SA-2.0\n\n"));

  if (!SD.begin(SD_CS)) {
    tft.setTextColor(ILI9340_RED);
    tft.print(F("SD failed!"));
    return;
  }

  if (! readMapPOI()) {
        tft.setTextColor(ILI9340_RED);
        tft.print(F("Read POI failed!\n"));
        return;
  }


   tft.print(F("Drawing init map 0,0"));

   // Still not working ....
   //bmpDraw("foo.bmp", 0, 0);

  delay(2000);


  // start with center map.  inits last_map_n as well.

  drawMap(0, 0);
}

void loop() {
  while (Serial3.available()) {
    char ch = microaprs.read();
    if (ch == '\n') {
      packet[buflen] = 0;
      show_packet();
      buflen = 0;
    } else if ((ch > 31 || ch == 0x1c || ch == 0x1d || ch == 0x27) && buflen < BUFLEN) {
      // Mic-E uses some non-printing characters
      packet[buflen++] = ch;
    }
  }
}

void show_packet() {

// Moved to global variables
//  char *call, *posit, *pmsgTo, *pmsg;
//  char type, pmsgID;
//  long lat, lon;

#ifdef DEBUG
    Serial.print(F(" * "));
    Serial.println(packet);
#endif

  if (microaprs.decode_posit(packet, &call, &type, &posit, &lon, &lat, &pmsgTo, &pmsg, &pmsgID )) {
#ifdef DEBUG
      Serial.print(F(" call ")); Serial.print(call);
      Serial.print(F(" packet type ")); Serial.print(type);
      Serial.print(F(" posit ")); Serial.print(posit);
      Serial.print(F(" lat ")); Serial.print(lat, DEC);
      Serial.print(F(" lon ")); Serial.print(lon, DEC);
      Serial.println();
#endif

    long dy = (POI_TL_LAT - lat) / DEGREES_PER_PIXEL_LAT;
    long dx = (lon - POI_TL_LON) / DEGREES_PER_PIXEL_LON;

#ifdef DEBUG
      Serial.print(F(" down ")); Serial.print(dy, DEC);
      Serial.print(F(" right ")); Serial.println(dx, DEC);
#endif
    int map_down = FLOOR(dy, LCD_HEIGHT);
    int map_right = FLOOR(dx, LCD_WIDTH);

#ifdef DEBUG
      Serial.print(F(" map down ")); Serial.println(map_down, DEC);
      Serial.print(F(" map right ")); Serial.println(map_right, DEC);
#endif

    int pix_down = dy % LCD_HEIGHT;
    int pix_right = dx % LCD_WIDTH;

#ifdef DEBUG
      Serial.print(F(" in map down ")); Serial.print(pix_down, DEC);
      Serial.print(F(" in map right ")); Serial.println(pix_right, DEC);
#endif

    display(call, posit, map_down, map_right, pix_down, pix_right);
  } else {

#ifdef DEBUG
      Serial.println(F(" !!! Decode failed\n"));
#endif
  }
}

void display(char *call, char *posit, int map_down,
         int map_right, int pix_down, int pix_right) {
  // If the map is too many tiles away from the center, it's out of range so don't display.
  if ((abs(map_down) > MAP_HEIGHT_IN_TILES/2) || (abs(map_right) > MAP_WIDTH_IN_TILES/2))
      return;


  // Find the map number, and display it if it's not the current map.
  drawMap(map_down, map_right);

  // Find the spot within the map.  Adjust negative coordinates
  if (pix_right < 0) pix_right = LCD_WIDTH + pix_right;
  if (pix_down < 0) pix_down = LCD_HEIGHT + pix_down;

  // If it's the same callsign as last time, draw a line.
  if (strcmp(call, last_call) == 0) {
    if (last_pix_down != 0) {

      // Disable line drawing
      //tft.drawLine(last_pix_right, last_pix_down, pix_right, pix_down, LINE_COLOR);
    }
  } else {
    // If not the same callsign, draw the callsign and start a new spot.
    bmdrawtext(call, CALL_TEXT_COLOR, pix_right, pix_down);

    bmdrawtext(call, ILI9340_RED, 1,1 );

    tft.setCursor(1, 9);
    tft.setTextColor(ILI9340_BLUE);
    tft.print(lat);

    tft.setCursor(1,17);
    tft.setTextColor(ILI9340_BLUE);
    tft.print(lon);

    bmdrawtext(posit,ILI9340_BLUE, 1,26);


    // Remember the new callsign
    strlcpy(last_call, call, sizeof(last_call));
  }
  // Remember the last point we drew for this callsign.
  last_pix_down = pix_down;
  last_pix_right = pix_right;

  // Draw a simple icon at the position.
  tft.fillCircle(pix_right, pix_down, 2, ICON_COLOR);
}

// find the map file and load it if it's not the current map.
void drawMap(int map_down, int map_right) {
  char name[] = "map####.bmp";
  map_down += MAP_HEIGHT_IN_TILES/2;
  map_right += MAP_WIDTH_IN_TILES/2;
  int n = map_right * 100 + map_down;

  // Do not check for last map, draw maps on every Rx
  //if (n != last_map_n) {
    char *p = name+3;
    *p++ = '0' + map_right / 10;
    *p++ = '0' + map_right % 10;
    *p++ = '0' + map_down / 10;
    *p++ = '0' + map_down % 10;
    bmpDraw(name, 0, 0);

    // Set the map number and reset callsign and last icon positions.
    last_map_n = n;
    last_call[0] = 0;
    last_pix_down = -1;
    last_pix_right = -1;
  //}
}

void bmdrawtext(char *text, uint16_t color, byte x, byte y) {
  tft.setCursor(x, y);
  tft.setTextColor(color);
  tft.setTextWrap(true);
  tft.print(text);
}

boolean readMapPOI() {
  File poi;
  char *fn = "POI.CSV";
  if ( (poi = SD.open("POI.CSV") ) == NULL) {
    tft.print("Error reading fn :");
    tft.print(fn);
    return false;
  }

  // skip first line of header text
  while (poi.read() != '\n') {
    ;
  }


  if (LCD_WIDTH != poi.parseInt()) {
    tft.print("Width mismatch");
    return false;
  }

  if (LCD_HEIGHT != poi.parseInt()) {
    tft.print("Height mismatch");
    return false;
  }


  {
    char qra[16];
    // skip comma after previous field
    poi.read();
    // read QRA string
    qra[poi.readBytesUntil(',', qra, sizeof(qra))] = 0;
    byte zoom =  poi.parseInt();
    tft.print(F("\n"));
    tft.print(qra);
    tft.print(F(" zoom "));
    tft.println(zoom, DEC);
  }

  POI_TL_LAT = poi.parseInt();
  POI_TL_LON = poi.parseInt();
  DEGREES_PER_PIXEL_LAT = poi.parseInt();
  DEGREES_PER_PIXEL_LON = poi.parseInt();
  MAP_WIDTH_IN_TILES = poi.parseInt();
  MAP_HEIGHT_IN_TILES = poi.parseInt();

  poi.close();
  return true;
}


/***************************************************
  Written by Limor Fried/Ladyada for Adafruit Industries.
  MIT license, all text above must be included in any redistribution
 ****************************************************/

// This function opens a Windows Bitmap (BMP) file and
// displays it at the given coordinates.  It's sped up
// by reading many pixels worth of data at a time
// (rather than pixel by pixel).  Increasing the buffer
// size takes more of the Arduino's precious RAM but
// makes loading a little faster.  20 pixels seems a
// good balance.

#define BUFFPIXEL 40

void bmpDraw(char *filename, uint8_t x, uint8_t y) {

  File     bmpFile;
  int      bmpWidth, bmpHeight;   // W+H in pixels
  uint8_t  bmpDepth;              // Bit depth (currently must be 24)
  uint32_t bmpImageoffset;        // Start of image data in file
  uint32_t rowSize;               // Not always = bmpWidth; may have padding
  uint8_t  sdbuffer[3*BUFFPIXEL]; // pixel buffer (R+G+B per pixel)
  uint8_t  buffidx = sizeof(sdbuffer); // Current position in sdbuffer
  boolean  goodBmp = false;       // Set to true on valid header parse
  boolean  flip    = true;        // BMP is stored bottom-to-top
  int      w, h, row, col;
  uint8_t  r, g, b;
  uint32_t pos = 0, startTime = millis();

  if((x >= tft.width()) || (y >= tft.height())) return;

  // Open requested file on SD card
  if ((bmpFile = SD.open(filename)) == NULL) {
    // Serial.print(F(" File not found"));
    return;
  }

  // Parse BMP header
  if(read16(bmpFile) == 0x4D42) { // BMP signature
    int bmpSize = read32(bmpFile);
    // Serial.print(" File size: "); Serial.println(bmpSize);
    (void)read32(bmpFile); // Read & ignore creator bytes
    bmpImageoffset = read32(bmpFile); // Start of image data
    // Serial.print(" Image Offset: "); Serial.println(bmpImageoffset, DEC);
    // Read DIB header
    int bmpHeaderSize = read32(bmpFile);
    // Serial.print(" Header size: "); Serial.println(bmpHeaderSize);
    bmpWidth  = read32(bmpFile);
    bmpHeight = read32(bmpFile);
    if(read16(bmpFile) == 1) { // # planes -- must be '1'
      bmpDepth = read16(bmpFile); // bits per pixel
      // Serial.print(" Bit Depth: "); Serial.println(bmpDepth);
      if((bmpDepth == 24) && (read32(bmpFile) == 0)) { // 0 = uncompressed

        goodBmp = true; // Supported BMP format -- proceed!
        // Serial.print(" Image size: ");
        // Serial.print(bmpWidth);
        // Serial.print('x');
        // Serial.println(bmpHeight);

        // BMP rows are padded (if needed) to 4-byte boundary
        rowSize = (bmpWidth * 3 + 3) & ~3;

        // If bmpHeight is negative, image is in top-down order.
        // This is not canon but has been observed in the wild.
        if(bmpHeight < 0) {
          bmpHeight = -bmpHeight;
          flip      = false;
        }

        // Crop area to be loaded
        w = bmpWidth;
        h = bmpHeight;
        if((x+w-1) >= tft.width())  w = tft.width()  - x;
        if((y+h-1) >= tft.height()) h = tft.height() - y;

        // Set TFT address window to clipped image bounds
        tft.setAddrWindow(x, y, x+w-1, y+h-1);

        for (row=0; row<h; row++) { // For each scanline...

          // Seek to start of scan line.  It might seem labor-
          // intensive to be doing this on every line, but this
          // method covers a lot of gritty details like cropping
          // and scanline padding.  Also, the seek only takes
          // place if the file position actually needs to change
          // (avoids a lot of cluster math in SD library).
          if(flip) // Bitmap is stored bottom-to-top order (normal BMP)
            pos = bmpImageoffset + (bmpHeight - 1 - row) * rowSize;
          else     // Bitmap is stored top-to-bottom
            pos = bmpImageoffset + row * rowSize;
          if(bmpFile.position() != pos) { // Need seek?
            bmpFile.seek(pos);
            buffidx = sizeof(sdbuffer); // Force buffer reload
          }

          for (col=0; col<w; col++) { // For each pixel...
            // Time to read more pixel data?
            if (buffidx >= sizeof(sdbuffer)) { // Indeed
              bmpFile.read(sdbuffer, sizeof(sdbuffer));
              buffidx = 0; // Set index to beginning
            }

            // Convert pixel from BMP to TFT format, push to display
            b = sdbuffer[buffidx++];
            g = sdbuffer[buffidx++];
            r = sdbuffer[buffidx++];
            tft.pushColor(tft.Color565(r,g,b));
          } // end pixel
        } // end scanline
        // Serial.print(" Loaded in ");
        // Serial.print(millis() - startTime);
        // Serial.println(" ms");
      } // end goodBmp
    }
  }

  bmpFile.close();
  if(!goodBmp) {
    // Serial.println(" BMP format not recognized.");
  }
}

// These read 16- and 32-bit types from the SD card file.
// BMP data is stored little-endian, Arduino is little-endian too.
// May need to reverse subscript order if porting elsewhere.

uint16_t read16(File f) {
  uint16_t result;
  ((uint8_t *)&result)[0] = f.read(); // LSB
  ((uint8_t *)&result)[1] = f.read(); // MSB
  return result;
}

uint32_t read32(File f) {
  uint32_t result;
  ((uint8_t *)&result)[0] = f.read(); // LSB
  ((uint8_t *)&result)[1] = f.read();
  ((uint8_t *)&result)[2] = f.read();
  ((uint8_t *)&result)[3] = f.read(); // MSB
  return result;
}