1  // SPDX-FileCopyrightText: 2019 Anne Barela for Adafruit Industries
  2  //
  3  // SPDX-License-Identifier: MIT
  4  
  5  // Test code for Adafruit Flora GPS modules
  6  //
  7  // This code shows how to listen to the GPS module in an interrupt
  8  // which allows the program to have more 'freedom' - just parse
  9  // when a new NMEA sentence is available! Then access data when
 10  // desired.
 11  //
 12  // Tested and works great with the Adafruit Flora GPS module
 13  // ------> http://adafruit.com/products/1059
 14  // Pick one up today atAdafruit https://www.adafruit.com/
 15  // and help support open source hardware & software! 
 16  
 17  #include <Adafruit_GPS.h>
 18  #include <Adafruit_NeoPixel.h>
 19  #include <SoftwareSerial.h>
 20  #include <TimeLib.h>
 21  #include <Wire.h>
 22  #include <Adafruit_Sensor.h>
 23  #include <Adafruit_LSM303DLH_Mag.h>
 24  
 25  
 26  Adafruit_GPS GPS(&Serial1);
 27  /* Assign a unique ID to this sensor at the same time */
 28  Adafruit_LSM303DLH_Mag_Unified mag = Adafruit_LSM303DLH_Mag_Unified(12345);
 29  
 30  // Set GPSECHO to 'false' to turn off echoing the GPS data to the Serial console
 31  // Set to 'true' if you want to debug and listen to the raw GPS sentences
 32  #define GPSECHO false
 33  
 34  // this keeps track of whether we're using the interrupt
 35  // off by default!
 36  boolean usingInterrupt = false;
 37  
 38  //--------------------------------------------------|
 39  //                    WAYPOINT                      |
 40  //--------------------------------------------------|
 41  //Please enter the latitude and longitude of your   |
 42  //desired destination:                              |
 43  #define GEO_LAT                48.009551
 44  #define GEO_LON               -88.771131
 45  //--------------------------------------------------|
 46  //Your NeoPixel ring may not line up with ours.     |
 47  //Enter which NeoPixel led is your top LED (0-15).  |
 48  #define TOP_LED                1
 49  //--------------------------------------------------|
 50  //Your compass module may not line up with ours.    |
 51  //Once you run compass mode, compare to a separate  |
 52  //compass (like one found on your smartphone).      |
 53  //Point your TOP_LED north, then count clockwise    |
 54  //how many LEDs away from TOP_LED the lit LED is    |
 55  #define LED_OFFSET             0
 56  //--------------------------------------------------|
 57  
 58  // Navigation location
 59  float targetLat = GEO_LAT;
 60  float targetLon = GEO_LON;
 61  
 62  // Trip distance
 63  float tripDistance;
 64  
 65  Adafruit_NeoPixel strip = Adafruit_NeoPixel(16, 6, NEO_GRB + NEO_KHZ800);
 66  
 67  // Offset hours from gps time (UTC)
 68  //const int offset = 1;   // Central European Time
 69  //const int offset = -4;  // Eastern Daylight Time (USA)
 70  const int offset = -5;  // Central Daylight Time (USA)
 71  //const int offset = -8;  // Pacific Standard Time (USA)
 72  //const int offset = -7;  // Pacific Daylight Time (USA)
 73  
 74  int topLED = TOP_LED;
 75  int compassOffset = LED_OFFSET;
 76  
 77  int lastMin = 16;
 78  int lastHour = 16;
 79  int startLED = 0;
 80  int startLEDlast = 16;
 81  int lastCombined = 0;
 82  int start = 0;
 83  int mode = 0;
 84  int lastDir = 16;
 85  int dirLED_r = 0;
 86  int dirLED_g = 0;
 87  int dirLED_b = 255;
 88  int compassReading;
 89  
 90  // Calibration offsets
 91  float magxOffset = 2.55;
 92  float magyOffset = 27.95;
 93  
 94  // Pushbutton setup
 95  int buttonPin = 10;             // the number of the pushbutton pin
 96  int buttonState;               // the current reading from the input pin
 97  int lastButtonState = HIGH;    // the previous reading from the input pin
 98  long buttonHoldTime = 0;         // the last time the output pin was toggled
 99  long buttonHoldDelay = 2500;      // how long to hold the button down
100  
101  // the following variables are long's because the time, measured in miliseconds,
102  // will quickly become a bigger number than can be stored in an int.
103  long lastDebounceTime = 0;     // the last time the output pin was toggled
104  long debounceDelay = 50;       // the debounce time; increase if the output flickers
105  long menuDelay = 2500;
106  long menuTime;
107  
108  float fLat = 0.0;
109  float fLon = 0.0;
110  
111  void setup()
112  {
113    // connect at 115200 so we can read the GPS fast enough and echo without dropping chars
114    // also spit it out
115    Serial.begin(115200);
116    Serial.println("Adafruit GPS library basic test!");
117  
118    // 9600 NMEA is the default baud rate for Adafruit MTK GPS's- some use 4800
119    GPS.begin(9600);
120    // uncomment this line to turn on RMC (recommended minimum) and GGA (fix data) including altitude
121    GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
122    // uncomment this line to turn on only the "minimum recommended" data
123    //GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
124    // For parsing data, we don't suggest using anything but either RMC only or RMC+GGA since
125    // the parser doesn't care about other sentences at this time
126    // Set the update rate
127    GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); // 1 Hz update rate
128    // For the parsing code to work nicely and have time to sort thru the data, and
129    // print it out we don't suggest using anything higher than 1 Hz
130  
131    // Request updates on antenna status, comment out to keep quiet
132    GPS.sendCommand(PGCMD_ANTENNA);
133  
134    /* Initialise the sensor */
135    if(!mag.begin())
136    {
137      /* There was a problem detecting the LSM303 ... check your connections */
138      Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
139      while(1);
140    }
141    // Ask for firmware version
142    Serial1.println(PMTK_Q_RELEASE);
143  
144    strip.begin();
145    strip.show(); // Initialize all pixels to 'off'
146  
147    // Make input & enable pull-up resistors on switch pins for pushbutton
148    pinMode(buttonPin, INPUT);
149    digitalWrite(buttonPin, HIGH); 
150  }
151  
152  uint32_t gpsTimer = millis();
153  uint32_t startupTimer = millis();
154  uint32_t compassTimer = millis();
155  
156  void loop() // run over and over again
157  {
158    compassCheck();
159    // read the state of the switch into a local variable:
160    int buttonState = digitalRead(buttonPin);
161  
162    if (buttonState == LOW) {
163      buttonCheck();
164    }
165  
166    lastButtonState = buttonState;
167  
168    //Serial.println(buttonState);
169    // read data from the GPS in the 'main loop'
170    char c = GPS.read();
171    // if you want to debug, this is a good time to do it!
172    if (GPSECHO)
173      if (c) Serial.print(c);
174    // if a sentence is received, we can check the checksum, parse it...
175    if (GPS.newNMEAreceived()) {
176      // a tricky thing here is if we print the NMEA sentence, or data
177      // we end up not listening and catching other sentences!
178      // so be very wary if using OUTPUT_ALLDATA and trytng to print out data
179      Serial.println(GPS.lastNMEA()); // this also sets the newNMEAreceived() flag to false
180      if (!GPS.parse(GPS.lastNMEA())) // this also sets the newNMEAreceived() flag to false
181        return; // we can fail to parse a sentence in which case we should just wait for another
182    }
183  
184    // if millis() or timer wraps around, we'll just reset it
185    if (gpsTimer > millis()) gpsTimer = millis();
186  
187    if (start == 0) {
188      if (GPS.fix) {
189        // set the Time to the latest GPS reading
190        setTime(GPS.hour, GPS.minute, GPS.seconds, GPS.day, GPS.month, GPS.year);
191        delay(50);
192        adjustTime(offset * SECS_PER_HOUR);
193        delay(500);
194        tripDistance = (double)calc_dist(fLat, fLon, targetLat, targetLon);
195        start = 1;
196      }
197    }    
198    // approximately every 60 seconds or so, update time
199    if ((millis() - gpsTimer > 60000) && (start == 1)) {
200      gpsTimer = millis(); // reset the timer
201      if (GPS.fix) {
202        // set the Time to the latest GPS reading
203        setTime(GPS.hour, GPS.minute, GPS.seconds, GPS.day, GPS.month, GPS.year);
204        delay(50);
205        adjustTime(offset * SECS_PER_HOUR);
206        delay(500);
207      }
208    }
209  
210    if (GPS.fix) {
211      fLat = decimalDegrees(GPS.latitude, GPS.lat);
212      fLon = decimalDegrees(GPS.longitude, GPS.lon);
213    }
214  
215    if (mode == 0) {
216      clockMode();
217    }
218  
219    if (mode == 1) {
220      navMode();
221    }
222  
223    if (mode == 2) {
224      compassMode();
225    }
226  }
227  
228  // Fill the dots one after the other with a color
229  void colorWipe(uint32_t c, uint8_t wait) {
230    for(uint16_t i=0; i<strip.numPixels(); i++) {
231      strip.setPixelColor(i, c);
232      strip.show();
233      delay(wait);
234    }
235  }
236  
237  void buttonCheck() {
238    menuTime = millis();
239    int buttonState = digitalRead(buttonPin);
240    if (buttonState == LOW && lastButtonState == HIGH) {
241      buttonHoldTime = millis();
242    }
243  
244    if (buttonState == LOW && lastButtonState == LOW) {
245      if ((millis() - buttonHoldTime) > buttonHoldDelay) {
246  
247        if(mode == 2) {
248          mode = 0;
249          lastMin = 16;
250          lastHour = 16;
251          colorWipe(strip.Color(0, 0, 0), 20);
252          buttonHoldTime = millis();
253        } 
254        else {
255          mode = mode + 1;
256          colorWipe(strip.Color(0, 0, 0), 20);
257          buttonHoldTime = millis();
258        }
259      }
260    }
261  }
262  
263  void clockMode() {
264    if (start == 1) {
265      strip.setPixelColor(startLEDlast, strip.Color(0, 0, 0));
266      strip.show();
267  
268      float gpsMin = (minute() + (second()/60.0));
269      unsigned int ledMin = 0;
270      int minTemp = 0;
271      minTemp = topLED - (gpsMin + 1.875)/3.75;
272  
273      if (minTemp < 0) {
274        ledMin = minTemp + 16;
275      } 
276      else {
277        ledMin = minTemp;
278      }
279  
280      float gpsHour = (hour() + (minute()/60.0));
281      if (gpsHour > 12) { 
282        gpsHour = gpsHour - 12; 
283      }
284      unsigned int ledHour = 0;
285      int hourTemp = 0;
286      hourTemp = topLED - (gpsHour + .375)/.75;
287  
288      if (hourTemp < 0) {
289        ledHour = hourTemp + 16;
290      } 
291      else {
292        ledHour = hourTemp;
293      }
294  
295      if ((ledHour == ledMin) && (lastCombined == 0)) {
296        strip.setPixelColor(lastHour, strip.Color(0, 0, 0));
297        strip.setPixelColor(lastMin, strip.Color(0, 0, 0));
298        strip.setPixelColor(ledHour, strip.Color(255, 0, 255));
299        strip.show();
300        lastCombined = 1;
301        lastHour = ledHour;
302        lastMin = ledMin;
303      } 
304      else {
305        if (lastHour != ledHour) {
306          strip.setPixelColor(lastHour, strip.Color(0, 0, 0));
307          strip.setPixelColor(ledHour, strip.Color(255, 50, 0));
308          strip.show();
309          lastHour = ledHour;
310        }
311        if (lastMin != ledMin) {
312          strip.setPixelColor(lastMin, strip.Color(0, 0, 0));
313          strip.setPixelColor(ledMin, strip.Color(200, 200, 0));
314          if (lastCombined == 1) {
315            strip.setPixelColor(ledHour, strip.Color(255, 0, 0));
316            lastCombined = 0;
317          }
318          strip.show();
319          lastMin = ledMin;
320        }
321      }   
322    } 
323    else {
324      // if millis() or timer wraps around, we'll just reset it
325      if (startupTimer > millis()) startupTimer = millis();
326  
327      // approximately every 10 seconds or so, update time
328      if (millis() - startupTimer > 200) {
329        startupTimer = millis(); // reset the timer
330        if (startLED == 16) {
331          startLED = 0;
332        }
333        strip.setPixelColor(startLEDlast, strip.Color(0, 0, 0));
334        strip.setPixelColor(startLED, strip.Color(0, 255, 0));
335        strip.show();
336        startLEDlast = startLED;
337        startLED++;
338        //delay(200);
339      }
340    }
341  }
342  
343  void navMode() {
344    if (start == 1) {
345  
346      compassCheck();
347  
348      headingDistance((double)calc_dist(fLat, fLon, targetLat, targetLon));
349  
350      if ((calc_bearing(fLat, fLon, targetLat, targetLon) - compassReading) > 0) {
351        compassDirection(calc_bearing(fLat, fLon, targetLat, targetLon)-compassReading);
352      } 
353      else {
354        compassDirection(calc_bearing(fLat, fLon, targetLat, targetLon)-compassReading+360);
355      }
356  
357    } 
358    else {
359      // if millis() or timer wraps around, we'll just reset it
360      if (startupTimer > millis()) startupTimer = millis();
361  
362      // approximately every 10 seconds or so, update time
363      if (millis() - startupTimer > 200) {
364        startupTimer = millis(); // reset the timer
365        if (startLED == 16) {
366          startLED = 0;
367        }
368        strip.setPixelColor(startLEDlast, strip.Color(0, 0, 0));
369        strip.setPixelColor(startLED, strip.Color(0, 0, 255));
370        strip.show();
371        startLEDlast = startLED;
372        startLED++;
373      }
374    }
375  }
376  
377  int calc_bearing(float flat1, float flon1, float flat2, float flon2)
378  {
379    float calc;
380    float bear_calc;
381  
382    float x = 69.1 * (flat2 - flat1); 
383    float y = 69.1 * (flon2 - flon1) * cos(flat1/57.3);
384  
385    calc=atan2(y,x);
386  
387    bear_calc= degrees(calc);
388  
389    if(bear_calc<=1){
390      bear_calc=360+bear_calc; 
391    }
392    return bear_calc;
393  }
394  void headingDistance(int fDist)
395  {
396    //Use this part of the code to determine how far you are away from the destination.
397    //The total trip distance (from where you started) is divided into five trip segments.
398    float tripSegment = tripDistance/5;
399  
400    if (fDist >= (tripSegment*4)) {
401      dirLED_r = 255;
402      dirLED_g = 0;
403      dirLED_b = 0;
404    }
405  
406    if ((fDist >= (tripSegment*3))&&(fDist < (tripSegment*4))) {
407      dirLED_r = 255;
408      dirLED_g = 0;
409      dirLED_b = 0;
410    }
411  
412    if ((fDist >= (tripSegment*2))&&(fDist < (tripSegment*3))) {
413      dirLED_r = 255;
414      dirLED_g = 255;
415      dirLED_b = 0;
416    }
417  
418    if ((fDist >= tripSegment)&&(fDist < (tripSegment*2))) {
419      dirLED_r = 255;
420      dirLED_g = 255;
421      dirLED_b = 0;
422    }
423  
424    if ((fDist >= 5)&&(fDist < tripSegment)) {
425      dirLED_r = 255;
426      dirLED_g = 255;
427      dirLED_b = 0;
428    }
429  
430    if ((fDist < 5)) { // You are now within 5 meters of your destination.
431      //Serial.println("Arrived at destination!");
432      dirLED_r = 0;
433      dirLED_g = 255;
434      dirLED_b = 0;
435    }
436  }
437  
438  
439  unsigned long calc_dist(float flat1, float flon1, float flat2, float flon2)
440  {
441    float dist_calc=0;
442    float dist_calc2=0;
443    float diflat=0;
444    float diflon=0;
445  
446    diflat=radians(flat2-flat1);
447    flat1=radians(flat1);
448    flat2=radians(flat2);
449    diflon=radians((flon2)-(flon1));
450  
451    dist_calc = (sin(diflat/2.0)*sin(diflat/2.0));
452    dist_calc2= cos(flat1);
453    dist_calc2*=cos(flat2);
454    dist_calc2*=sin(diflon/2.0);
455    dist_calc2*=sin(diflon/2.0);
456    dist_calc +=dist_calc2;
457  
458    dist_calc=(2*atan2(sqrt(dist_calc),sqrt(1.0-dist_calc)));
459  
460    dist_calc*=6371000.0; //Converting to meters
461    return dist_calc;
462  }
463  
464  // Convert NMEA coordinate to decimal degrees
465  float decimalDegrees(float nmeaCoord, char dir) {
466    uint16_t wholeDegrees = 0.01*nmeaCoord;
467    int modifier = 1;
468  
469    if (dir == 'W' || dir == 'S') {
470      modifier = -1;
471    }
472  
473    return (wholeDegrees + (nmeaCoord - 100.0*wholeDegrees)/60.0) * modifier;
474  }
475  
476  void compassMode() {
477    dirLED_r = 0;
478    dirLED_g = 0;
479    dirLED_b = 255;
480    compassDirection(compassReading);   
481  }
482  
483  void compassCheck() {
484    // if millis() or timer wraps around, we'll just reset it
485    if (compassTimer > millis()) compassTimer = millis();
486  
487    // approximately every 10 seconds or so, update time
488    if (millis() - compassTimer > 50) {
489      /* Get a new sensor event */
490      sensors_event_t event; 
491      mag.getEvent(&event);
492  
493      float Pi = 3.14159;
494  
495      compassTimer = millis(); // reset the timer
496  
497      // Calculate the angle of the vector y,x
498      float heading = (atan2(event.magnetic.y + magyOffset,event.magnetic.x + magxOffset) * 180) / Pi;
499  
500      // Normalize to 0-360
501      if (heading < 0)
502      {
503        heading = 360 + heading;
504      }
505      compassReading = heading; 
506    }  
507  }  
508  
509  void compassDirection(int compassHeading) 
510  {
511    //Serial.print("Compass Direction: ");
512    //Serial.println(compassHeading);
513  
514    unsigned int ledDir = 2;
515    int tempDir = 0;
516    //Use this part of the code to determine which way you need to go.
517    //Remember: this is not the direction you are heading, it is the direction to the destination (north = forward).
518  
519    if ((compassHeading > 348.75)||(compassHeading < 11.25)) {
520        tempDir = topLED;
521    }
522    for(int i = 1; i < 16; i++){
523      float pieSliceCenter = 45/2*i;
524      float pieSliceMin = pieSliceCenter - 11.25;
525      float pieSliceMax = pieSliceCenter + 11.25;
526      if ((compassHeading >= pieSliceMin)&&(compassHeading < pieSliceMax)) {
527        if (mode == 2 ) {
528          tempDir = topLED - i;
529        } 
530        else {
531          tempDir = topLED + i;
532        }
533      }
534    }
535  
536    if (tempDir > 15) {
537      ledDir = tempDir - 16;
538    }
539  
540    else if (tempDir < 0) {
541      ledDir = tempDir + 16;
542    } 
543    else {
544      ledDir = tempDir;
545    }
546  
547    if (mode == 1) {
548      ledDir = ledDir + compassOffset;
549      if (ledDir > 15) {
550        ledDir = ledDir - 16;
551      }
552    } 
553    else {
554      ledDir = ledDir + compassOffset;
555      if (ledDir > 15) {
556        ledDir = ledDir - 16;
557      }
558    }
559  
560    if (lastDir != ledDir) {
561      strip.setPixelColor(lastDir, strip.Color(0, 0, 0));
562      strip.setPixelColor(ledDir, strip.Color(dirLED_r, dirLED_g, dirLED_b));
563      strip.show();
564      lastDir = ledDir;
565    }
566  }