1  // SPDX-FileCopyrightText: 2019 Anne Barela for Adafruit Industries
  2  //
  3  // SPDX-License-Identifier: MIT
  4  
  5  // Flora GPS + LED Pixel Code
  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 at the Adafruit electronics shop 
 15  // and help support open source hardware & software! -ada
 16  
 17  #include <Adafruit_GPS.h>
 18  #include <SoftwareSerial.h>
 19  #include "Adafruit_NeoPixel.h"
 20  
 21  Adafruit_GPS GPS(&Serial1);
 22  
 23  // Set GPSECHO to 'false' to turn off echoing the GPS data to the Serial console
 24  // Set to 'true' if you want to debug and listen to the raw GPS sentences
 25  #define GPSECHO false
 26  
 27  // this keeps track of whether we're using the interrupt
 28  // off by default!
 29  boolean usingInterrupt = false;
 30  
 31  //--------------------------------------------------|
 32  //                    WAYPOINTS                     |
 33  //--------------------------------------------------|
 34  //Please enter the latitude and longitude of your   |
 35  //desired destination:                              |
 36    #define GEO_LAT                44.995012
 37    #define GEO_LON               -93.228967
 38  //--------------------------------------------------|
 39  
 40  //--------------------------------------------------|
 41  //                    DISTANCE                      |
 42  //--------------------------------------------------|
 43  //Please enter the distance (in meters) from your   |
 44  //destination that you want your LEDs to light up:  |
 45    #define DESTINATION_DISTANCE   20
 46  //--------------------------------------------------|
 47  
 48  
 49  // Navigation location
 50  float targetLat = GEO_LAT;
 51  float targetLon = GEO_LON;
 52  
 53  // Trip distance
 54  float tripDistance;
 55  
 56  boolean isStarted = false;
 57  
 58  // Set the first variable to the NUMBER of pixels. 25 = 25 pixels in a row
 59  // second var is pin
 60  Adafruit_NeoPixel strip = Adafruit_NeoPixel(2, 6, NEO_GRB + NEO_KHZ800);
 61  
 62  
 63  uint8_t LED_Breathe_Table[]  = {   80,  87,  95, 103, 112, 121, 131, 141, 151, 161, 172, 182, 192, 202, 211, 220,
 64                228, 236, 242, 247, 251, 254, 255, 255, 254, 251, 247, 242, 236, 228, 220, 211,
 65                202, 192, 182, 172, 161, 151, 141, 131, 121, 112, 103,  95,  87,  80,  73,  66,
 66                 60,  55,  50,  45,  41,  38,  34,  31,  28,  26,  24,  22,  20,  20,  20,  20,
 67                 20,  20,  20,  20,  20,  20,  20,  20,  20,  20,  20,  20,  20,  20,  20,  20,
 68                 20,  20,  20,  20,  20,  20,  20,  20,  20,  20,  20,  20,  22,  24,  26,  28,
 69                 31,  34,  38,  41,  45,  50,  55,  60,  66,  73 };
 70  
 71  
 72  #define BREATHE_TABLE_SIZE (sizeof(LED_Breathe_Table))
 73  #define BREATHE_CYCLE    5000      /*breathe cycle in milliseconds*/
 74  #define BREATHE_UPDATE    (BREATHE_CYCLE / BREATHE_TABLE_SIZE)
 75  uint32_t lastBreatheUpdate = 0;
 76  uint8_t breatheIndex = 0;
 77  
 78  void setup()  
 79  {
 80    // connect at 115200 so we can read the GPS fast enough and echo without dropping chars
 81    // also spit it out
 82    Serial.begin(115200);
 83  
 84    // 9600 NMEA is the default baud rate for Adafruit MTK GPS's- some use 4800
 85    GPS.begin(9600);
 86    
 87    // uncomment this line to turn on RMC (recommended minimum) and GGA (fix data) including altitude
 88    GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
 89    // uncomment this line to turn on only the "minimum recommended" data
 90    //GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
 91    // For parsing data, we don't suggest using anything but either RMC only or RMC+GGA since
 92    // the parser doesn't care about other sentences at this time
 93    
 94    // Set the update rate
 95    GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);   // 1 Hz update rate
 96    // For the parsing code to work nicely and have time to sort thru the data, and
 97    // print it out we don't suggest using anything higher than 1 Hz
 98  
 99    delay(1000);
100    // Ask for firmware version
101    Serial1.println(PMTK_Q_RELEASE);
102    
103      // Start up the LED strip
104    strip.begin();
105  
106    // Update the strip, to start they are all 'off'
107    strip.show();
108  }
109  
110  uint32_t timer = millis();
111  
112  void loop()                     // run over and over again
113  {
114    // read data from the GPS in the 'main loop'
115    char c = GPS.read();
116    // if you want to debug, this is a good time to do it!
117    if (GPSECHO)
118        if (c) Serial.print(c);
119    
120    // if a sentence is received, we can check the checksum, parse it...
121    if (GPS.newNMEAreceived()) {
122      // a tricky thing here is if we print the NMEA sentence, or data
123      // we end up not listening and catching other sentences! 
124      // so be very wary if using OUTPUT_ALLDATA and trytng to print out data
125      //Serial.println(GPS.lastNMEA());   // this also sets the newNMEAreceived() flag to false
126    
127      if (!GPS.parse(GPS.lastNMEA()))   // this also sets the newNMEAreceived() flag to false
128        return;  // we can fail to parse a sentence in which case we should just wait for another
129    }
130    
131  
132      if (GPS.fix) {
133        //Serial.print("Location: ");
134        //Serial.print(GPS.latitude, 2); Serial.print(GPS.lat);
135        //Serial.print(", "); 
136        //Serial.print(GPS.longitude, 2); Serial.println(GPS.lon);
137        
138        float fLat = decimalDegrees(GPS.latitude, GPS.lat);
139        float fLon = decimalDegrees(GPS.longitude, GPS.lon);
140        
141        if (!isStarted) {
142          isStarted = true;
143          tripDistance = (double)calc_dist(fLat, fLon, targetLat, targetLon);
144        }
145        
146        //Uncomment below if you want your Flora to navigate to a certain destination.  Then modify the headingDirection function.
147        /*if ((calc_bearing(fLat, fLon, targetLat, targetLon) - GPS.angle) > 0) {
148          headingDirection(calc_bearing(fLat, fLon, targetLat, targetLon)-GPS.angle);
149         }
150        else {
151          headingDirection(calc_bearing(fLat, fLon, targetLat, targetLon)-GPS.angle+360);
152        }*/
153        
154        headingDistance((double)calc_dist(fLat, fLon, targetLat, targetLon));
155        //Serial.print("Distance Remaining:"); Serial.println((double)calc_dist(fLat, fLon, targetLat, targetLon));
156        
157      }
158    //}
159    
160  }
161  
162  int calc_bearing(float flat1, float flon1, float flat2, float flon2)
163  {
164    float calc;
165    float bear_calc;
166  
167    float x = 69.1 * (flat2 - flat1); 
168    float y = 69.1 * (flon2 - flon1) * cos(flat1/57.3);
169  
170    calc=atan2(y,x);
171  
172    bear_calc= degrees(calc);
173  
174    if(bear_calc<=1){
175      bear_calc=360+bear_calc; 
176    }
177    return bear_calc;
178  }
179  
180  void headingDirection(float heading) 
181  {
182    //Use this part of the code to determine which way you need to go.
183    //Remember: this is not the direction you are heading, it is the direction to the destination (north = forward).
184    if ((heading > 348.75)||(heading < 11.25)) {
185      Serial.println("  N");
186      //Serial.println("Forward");
187    }
188    
189    if ((heading >= 11.25)&&(heading < 33.75)) {
190      Serial.println("NNE");
191      //Serial.println("Go Right");
192    }  
193    
194    if ((heading >= 33.75)&&(heading < 56.25)) {
195      Serial.println(" NE");
196      //Serial.println("Go Right");
197    }
198    
199    if ((heading >= 56.25)&&(heading < 78.75)) {
200      Serial.println("ENE");
201      //Serial.println("Go Right");
202    }
203    
204    if ((heading >= 78.75)&&(heading < 101.25)) {
205      Serial.println("  E");
206      //Serial.println("Go Right");
207    }
208    
209    if ((heading >= 101.25)&&(heading < 123.75)) {
210      Serial.println("ESE");
211      //Serial.println("Go Right");
212    }
213    
214    if ((heading >= 123.75)&&(heading < 146.25)) {
215      Serial.println(" SE");
216      //Serial.println("Go Right");
217    }
218    
219    if ((heading >= 146.25)&&(heading < 168.75)) {
220      Serial.println("SSE");
221      //Serial.println("Go Right");
222    }
223    
224    if ((heading >= 168.75)&&(heading < 191.25)) {
225      Serial.println("  S");
226      //Serial.println("Turn Around");
227    }
228    
229    if ((heading >= 191.25)&&(heading < 213.75)) {
230      Serial.println("SSW");
231      //Serial.println("Go Left");
232    }
233    
234    if ((heading >= 213.75)&&(heading < 236.25)) {
235      Serial.println(" SW");
236      //Serial.println("Go Left");
237    }
238    
239    if ((heading >= 236.25)&&(heading < 258.75)) {
240      Serial.println("WSW");
241      //Serial.println("Go Left");
242    }
243    
244    if ((heading >= 258.75)&&(heading < 281.25)) {
245      Serial.println("  W");
246      //Serial.println("Go Left");
247    }
248    
249    if ((heading >= 281.25)&&(heading < 303.75)) {
250      Serial.println("WNW");
251      //Serial.println("Go Left");
252    }
253    
254    if ((heading >= 303.75)&&(heading < 326.25)) {
255      Serial.println(" NW");
256      //Serial.println("Go Left");
257    }
258    
259    if ((heading >= 326.25)&&(heading < 348.75)) {
260      Serial.println("NWN");
261      //Serial.println("Go Left");
262    }
263  }
264  
265  void headingDistance(float fDist)
266  {
267    //Use this part of the code to determine how far you are away from the destination.
268    //The total trip distance (from where you started) is divided into five trip segments.
269   Serial.println(fDist);
270   if ((fDist >= DESTINATION_DISTANCE)) { // You are now within 5 meters of your destination.
271      //Serial.println("Trip Distance: 1");
272      //Serial.println("Arrived at destination!");
273      int i;
274      for (i=0; i < strip.numPixels(); i++) {
275        strip.setPixelColor(i, 0, 0, 0);
276      }  
277      strip.show();   // write all the pixels out
278    }
279  
280  
281    if ((fDist < DESTINATION_DISTANCE)) { // You are now within 5 meters of your destination.
282      //Serial.println("Trip Distance: 0");
283      //Serial.println("Arrived at destination!");
284      breath();
285    }
286    
287  }
288  
289  unsigned long calc_dist(float flat1, float flon1, float flat2, float flon2)
290  {
291    float dist_calc=0;
292    float dist_calc2=0;
293    float diflat=0;
294    float diflon=0;
295  
296    diflat=radians(flat2-flat1);
297    flat1=radians(flat1);
298    flat2=radians(flat2);
299    diflon=radians((flon2)-(flon1));
300  
301    dist_calc = (sin(diflat/2.0)*sin(diflat/2.0));
302    dist_calc2= cos(flat1);
303    dist_calc2*=cos(flat2);
304    dist_calc2*=sin(diflon/2.0);
305    dist_calc2*=sin(diflon/2.0);
306    dist_calc +=dist_calc2;
307  
308    dist_calc=(2*atan2(sqrt(dist_calc),sqrt(1.0-dist_calc)));
309  
310    dist_calc*=6371000.0; //Converting to meters
311    return dist_calc;
312  }
313  
314  // Convert NMEA coordinate to decimal degrees
315  float decimalDegrees(float nmeaCoord, char dir) {
316    uint16_t wholeDegrees = 0.01*nmeaCoord;
317    int modifier = 1;
318  
319    if (dir == 'W' || dir == 'S') {
320      modifier = -1;
321    }
322    
323    return (wholeDegrees + (nmeaCoord - 100.0*wholeDegrees)/60.0) * modifier;
324  }
325  
326  void breath()
327  {
328    uniformBreathe(LED_Breathe_Table, BREATHE_TABLE_SIZE, BREATHE_UPDATE, 127, 127, 127);
329  }
330  
331  void uniformBreathe(uint8_t* breatheTable, uint8_t breatheTableSize, uint16_t updatePeriod, uint16_t r, uint16_t g, uint16_t b)
332  {
333    int i;
334  
335    uint8_t breatheBlu;
336    
337    if ((millis() - lastBreatheUpdate) > updatePeriod) {
338      lastBreatheUpdate = millis();
339  
340      
341      for (i=0; i < strip.numPixels(); i++) {
342        breatheBlu = (b * breatheTable[breatheIndex]) / 256;
343        strip.setPixelColor(i, 0, 0, breatheBlu);
344      }
345      strip.show();   
346      
347      breatheIndex++;
348      if (breatheIndex > breatheTableSize) {
349        breatheIndex = 0;
350      }   
351    }
352  }