Open Cansat library for gps added

src/OpenCansatGPS.cpp

+#include "OpenCansatGPS.h"
+#include <Wire.h>
+
+/*--- CONFIG ---*/
+// I2C address set
+static const uint8_t openCansat_gps_i2c_addr = 0x42;
+static const size_t init_ublox_buffer_size = 128;
+
+/**
+ * Check if timeout occured
+ */
+static inline bool isTimeOut(uint32_t from, uint32_t timeout)
+{
+    return (millis() - from) > timeout;
+}
+
+OpenCansatGPS::OpenCansatGPS()
+	: isReceiving(false),
+	  i2cAddr(openCansat_gps_i2c_addr),
+	  inputBuffer(static_cast<char*>(malloc(init_ublox_buffer_size))),
+	  inputBufferSize(init_ublox_buffer_size)
+{
+    resetValues();
+}
+
+void OpenCansatGPS::begin()
+{
+    Wire.begin();
+}
+
+void OpenCansatGPS::debugPrintOn()
+{
+	this->debugPrint = &SerialUSB;
+}
+
+uint16_t OpenCansatGPS::getYear()
+{
+	return (uint16_t)timeDate.year + 2000;
+}
+
+uint8_t OpenCansatGPS::getMonth()
+{
+	return timeDate.mth;
+}
+
+uint8_t OpenCansatGPS::getDay()
+{
+	return timeDate.day;
+}
+
+uint8_t OpenCansatGPS::getHour()
+{
+	return timeDate.hrs;
+}
+
+uint8_t OpenCansatGPS::getMinute()
+{
+	return timeDate.min;
+}
+
+uint8_t OpenCansatGPS::getSecond()
+{
+	return timeDate.sec;
+}
+
+uint8_t OpenCansatGPS::getNumberOfSatellites()
+{
+	return data.numSat;
+}
+
+double OpenCansatGPS::getLat()
+{
+	return data.lat;
+}
+
+double OpenCansatGPS::getLon()
+{
+	return data.lon;
+}
+
+double OpenCansatGPS::getAlt()
+{
+	return data.alt;
+}
+
+double OpenCansatGPS::getSpeed()
+{
+	return data.speed;
+}
+
+double OpenCansatGPS::getHdop()
+{
+	return data.hdop;
+}
+
+String OpenCansatGPS::getDateTimeString()
+{
+    return String(getYear()) + " " + String(getMonth()) + " " + String(getDay())
+           + " " +  String(getHour()) + " " +  String(getMinute()) + " " +  String(getSecond());
+}
+
+bool OpenCansatGPS::scan(uint32_t timeout)
+{
+    bool result = false;
+
+    uint32_t start = millis();
+
+    resetValues();
+
+    while (!isTimeOut(start, timeout))
+    {
+        if (!readLine()) {
+            continue;
+        }
+
+        parseLine(inputBuffer);
+
+        // Scan quit
+       /* if (isSeenLatLon && isTimeReceived && isSeenAlt
+            && data.numSat > 0)
+        {*/
+        if (isSeenLatLon && data.numSat > 0)
+        {
+            result = true;
+            break;
+        }
+    }
+
+    if (data.numSat > 0)
+    {
+    		debugPrint->println(String("[scan] num sats = ") + data.numSat);
+    }
+
+    if (isSeenLatLon)
+    {
+    		debugPrint->println(String("[scan] lat = ") + String(data.lat, 7));
+    		debugPrint->println(String("[scan] lon = ") + String(data.lon, 7));
+    }
+
+    return result;
+}
+
+
+void OpenCansatGPS::resetValues()
+{
+	timeDate = {0, 0, 0, 0, 0, 0};
+	data = {0, 0, 0, 0, 0, 0};
+
+    isSeenLatLon = false;
+    isSeenAlt = false;
+    isTimeReceived = false;
+}
+
+bool OpenCansatGPS::parseLine(const char * line)
+{
+    if (!computeCrc(line))
+    {
+        return false;
+    }
+
+    String data = line + 1;
+
+    data.remove(data.length() - 3, 3);  // Strip checksum *<hex><hex>
+
+    if (data.startsWith("GPGGA") || data.startsWith("GNGGA"))
+    {
+        return parseGPGGA(data);
+    }
+
+    if (data.startsWith("GPRMC") || data.startsWith("GNRMC"))
+    {
+        return parseGPRMC(data);
+    }
+
+    if (data.startsWith("GPGSV") || data.startsWith("GLGSV"))
+    {
+        return parseGPGSV(data);
+    }
+
+    if (data.startsWith("GNGLL"))
+    {
+    		return parseGNGLL(data);
+    }
+
+    debugPrint->println(String("No parse: ") + line);
+
+    return false;
+}
+
+bool OpenCansatGPS::readLine(uint32_t timeout)
+{
+    if (!inputBuffer)
+    {
+        return false;
+    }
+
+    uint32_t startTime = millis();
+    size_t counter = 0;
+    char c = 0;
+
+    char *ptr = inputBuffer;
+    *ptr = '\0';
+
+    while (!isTimeOut(startTime, timeout))
+    {
+        c = (char)read();
+        if (c == '$')
+        {
+            break;
+        }
+    }
+
+    if (c != '$')
+    {
+        return false;
+    }
+
+    *ptr++ = c;
+    ++counter;
+
+    c = 0;
+    while (!isTimeOut(startTime, timeout))
+    {
+        c = (char)read();
+        if (c == '\r')
+        {
+            continue;
+        }
+        if (c == '\n')
+        {
+            break;
+        }
+        if (counter < inputBufferSize - 1)
+        {
+            *ptr++ = c;
+            ++counter;
+        }
+    }
+
+    *ptr = '\0';
+
+    if (c != '\n')
+    {
+        return false;
+    }
+
+    // End transmission
+    if (isReceiving)
+    {
+        Wire.endTransmission();
+        isReceiving = false;
+    }
+
+    return true;
+}
+
+uint8_t OpenCansatGPS::read()
+{
+	// Begin transmission
+	if (!isReceiving)
+	{
+		Wire.beginTransmission(i2cAddr);
+	    	isReceiving = true;
+	}
+
+	// Read byte by byte
+    uint8_t b = 0xFF;
+    uint8_t nr_bytes = Wire.requestFrom(i2cAddr, 1, false);
+    if (nr_bytes == 1)
+    {
+        b = Wire.read();
+    }
+    return b;
+}
+
+bool OpenCansatGPS::computeCrc(const char* line)
+{
+    size_t len = strlen(line);
+    String crc1String = "";
+    uint8_t crc = 0;
+
+    // Compute crc from line
+    if (len < 4 or line[0] != '$' or line[len - 3] != '*')
+    {
+        	debugPrint->println("Invalid lengt or Begin/End character [$/*]");
+        return false;
+    }
+
+    for (size_t i = 1; i < len - 3; ++i)
+    {
+        crc ^= line[i];
+    }
+
+    // Get crc1 from line (before end char)
+    crc1String.operator +=(line[len - 2]);
+    crc1String.operator +=(line[len - 1]);
+    uint8_t crc1 = (int)strtol(crc1String.c_str(), 0, 16);
+
+    // Compare both crc, they have to be equal
+    if (crc != crc1)
+    {
+        	debugPrint->print("Invalid CRC: ");
+        	debugPrint->print(crc1, HEX);
+        	debugPrint->print(", expected: ");
+        	debugPrint->println(crc, HEX);
+        return false;
+    }
+
+    return true;
+}
+
+double OpenCansatGPS::convertToDecimalDeg(const String & data)
+{
+    double degMin = static_cast<int>(data.toFloat() / 100);
+
+    double min = fmod(static_cast<double>(degMin), 100.0);
+
+    return degMin + (min / 60);
+}
+
+String OpenCansatGPS::getField(const String & data, int index)
+{
+	int beginIndex = 0;
+	int maxIndex = 0;
+
+	for(int i = 0; i < index; i++)
+	{
+		beginIndex = data.indexOf(",", beginIndex+1);
+	}
+
+	maxIndex = data.indexOf(",", beginIndex+1);
+
+	if(maxIndex == -1)
+	{
+		maxIndex = data.length() - 1;
+	}
+
+	return beginIndex == -1 ? "" : data.substring(beginIndex + 1, maxIndex);
+}
+
+bool OpenCansatGPS::parseGPGGA(const String & line)
+{
+	debugPrint->println(String("Parse: ") + line);
+
+	data.lat 		= convertToDecimalDeg(getField(line, 2));
+	String directNS 	= getField(line, 3);
+	data.lon 		= convertToDecimalDeg(getField(line, 4));
+	String directEW 	= getField(line, 5);
+	String quality 	= getField(line, 6);
+	data.numSat   	= getField(line, 7).toInt();
+	data.hdop 		= getField(line, 8).toFloat();
+	String alt    	= getField(line, 9);
+	String altState 	= getField(line, 10);
+
+    if (quality.compareTo("0") != 0)
+    {
+        if (directNS.compareTo("S") == 0)
+        {
+        		data.lat = -data.lat;
+        }
+
+        if (directEW.compareTo("W") == 0)
+        {
+        		data.lon = -data.lon;
+        }
+
+        isSeenLatLon = true;
+
+        if (altState.compareTo("M") == 0)
+        {
+            data.alt = alt.toFloat();
+            isSeenAlt = true;
+        }
+    }
+    return true;
+}
+
+bool OpenCansatGPS::parseGNGLL(const String & line)
+{
+	debugPrint->println(String("Parse: ") + line);
+
+	data.lat 		= convertToDecimalDeg(getField(line, 1));
+    String directNS 	= getField(line, 2);
+    data.lon 		= convertToDecimalDeg(getField(line, 3));
+    String directEW 	= getField(line, 4);
+    String time  	= getField(line, 5);
+    String status 	= getField(line, 6);
+
+    debugPrint->println(String("[scan] status = ") + String(status));
+
+    if (status.compareTo("A") == 0)
+    {
+        if (directNS.compareTo("S") == 0)
+        {
+        		data.lat = -data.lat;
+        }
+
+        if (directEW.compareTo("W") == 0)
+        {
+        		data.lon = -data.lon;
+        }
+
+        isSeenLatLon = true;
+
+        debugPrint->println(String("[scan] dataLon = ") + String(data.lon));
+        debugPrint->println(String("[scan] dataLat = ") + String(data.lat));
+    }
+
+    if (time.length() == 9)
+    {
+    		timeDate.hrs = time.substring(0, 2).toInt() + 2;
+    		timeDate.min = time.substring(2, 4).toInt();
+    		timeDate.sec = time.substring(4, 6).toInt();
+
+    		debugPrint->println(String("[scan] datetime = ") + String(timeDate.hrs)
+    				+ " " + String(timeDate.min) + " " + String(timeDate.sec));
+    }
+
+    return true;
+}
+
+bool OpenCansatGPS::parseGPRMC(const String & line)
+{
+	debugPrint->println(String("Parse: ") + line);
+
+    String time  	= getField(line, 1);
+    String status 	= getField(line, 2);
+    data.lat 		= convertToDecimalDeg(getField(line, 3));
+    String directNS 	= getField(line, 4);
+    data.lon 		= convertToDecimalDeg(getField(line, 5));
+    String directEW 	= getField(line, 6);
+    data.speed 		= getField(line, 7).toDouble();
+    String date  	= getField(line, 9);
+    String posMode 	= getField(line, 12);
+
+    if (status.compareTo("A") == 0 && posMode.compareTo("N") != 0)
+    {
+        if (directNS.compareTo("S") == 0)
+        {
+        		data.lat = -data.lat;
+        }
+
+        if (directEW.compareTo("W") == 0)
+        {
+        		data.lon = -data.lon;
+        }
+
+        isSeenLatLon = true;
+    }
+
+    if (time.length() == 9 && date.length() == 6)
+    {
+		timeDate.day  = date.substring(0, 2).toInt();
+		timeDate.mth  = date.substring(2, 4).toInt();
+		timeDate.year = date.substring(4, 6).toInt();
+
+    		timeDate.hrs = time.substring(0, 2).toInt() + 2;
+    		timeDate.min = time.substring(2, 4).toInt();
+    		timeDate.sec = time.substring(4, 6).toInt();
+
+    		debugPrint->println(String("[scan] datetime = ") + getDateTimeString());
+    }
+
+    return true;
+}
+
+bool OpenCansatGPS::parseGPGSV(const String & line)
+{
+	debugPrint->println(String("Parse: ") + line);
+
+    data.numSat = getField(line, 3).toInt();
+
+    debugPrint->println(String("[scan] num sats = ") + data.numSat);
+
+    return true;
+}

src/OpenCansatGPS.h

+#ifndef _OPEN_CANSAT_GPS_H
+#define _OPEN_CANSAT_GPS_H
+
+#include <WString.h>
+#include <stdint.h>
+
+class OpenCansatGPS
+{
+public:
+/*--- CONSTRUCTOR ---*/
+	/**
+	 * Non parametric constructor
+	 */
+	OpenCansatGPS();
+
+/*--- PUBLIC METHODS ---*/
+	/**
+	 * Begin of GPS i2c communication
+	 */
+    void begin();
+
+	/**
+	 * Switch on debug print
+	 */
+    void debugPrintOn();
+
+    /*
+     * Get methods for time, date and gps data
+     */
+    uint16_t getYear();
+    uint8_t getMonth();
+    uint8_t getDay();
+    uint8_t getHour();
+    uint8_t getMinute();
+    uint8_t getSecond();
+    uint8_t getNumberOfSatellites();
+    double getLat();
+    double getLon();
+    double getAlt();
+    double getSpeed();
+    double getHdop();
+
+	/**
+	 *  Get date and time in string format
+	 */
+    String getDateTimeString();
+
+    /**
+     * Read UBlox device, reading is end when fix is seen,
+     * or when timeout has been reached.
+     */
+    bool scan(uint32_t timeout = 10000);
+
+private:
+/*--- PRIVATE VARIABLES ---*/
+    // Recognize of receiving/not receiving state
+    bool	 		isReceiving;
+    // Recognize if time is already received
+    bool        isTimeReceived;
+    // Recognize if lat and lon are seen
+    bool        isSeenLatLon;
+    // Recognize if Alt is seen
+    bool        isSeenAlt;
+
+    // OpenCansat i2c addr storage
+    uint8_t 		i2cAddr;
+    char*   		inputBuffer;
+    size_t		inputBufferSize;
+
+    // Optional debug print
+    Stream*     debugPrint;
+
+/*--- DATA STRUCTURES ---*/
+    struct gpsTimeAndDate
+    {
+    		uint16_t year;
+    		uint8_t mth;
+    		uint8_t day;
+    		uint8_t hrs;
+    		uint8_t min;
+    		uint8_t sec;
+    };
+
+    struct gpsData
+    {
+    		double lat;
+    		double lon;
+    		uint8_t numSat;
+    	    double speed;
+    	    double alt;
+    	    double hdop;
+    };
+
+    // GPS date and time
+    gpsTimeAndDate timeDate;
+
+    // GPS data
+    gpsData data;
+
+/*--- PRIVATE METHODS ---*/
+    /**
+     * Reset of variables to 0
+     */
+    void resetValues();
+
+    bool parseLine(const char * line);
+
+    /**
+     * Read one NMEA frame
+     */
+    bool readLine(uint32_t timeout = 10000);
+
+    /**
+     * Begin transmission and read one byte
+     */
+    uint8_t read();
+
+    /**
+     *  Compute and verify checksum CRC for line
+     *
+     *  Line must start with '$' and end with '*'
+     */
+    bool computeCrc(const char * line);
+
+    /**
+     * Convert lat/long degree format to decimal degrees
+     */
+    double convertToDecimalDeg(const String & data);
+
+    /**
+     * Get field of specific index separator = ","
+     */
+    String getField(const String & data, int index);
+
+    /**
+     * Read the coordinates using GPGGA
+     *
+     * 2    lat     .. ddmm.mmmmm  .. Latitude (degrees & minutes)
+     * 3    NS      .. char        .. North/South indicator
+     * 4    long    .. dddmm.mmmmm .. Longitude (degrees & minutes)
+     * 5    EW      .. char        .. East/West indicator
+     * 6    quality .. digit       .. Quality indicator for position fix: 0 No Fix, 6 Estimated, 1 Auto GNSS, 2 Diff GNSS
+     * 7    numSV   .. num         .. Number of satellites used
+     * 8    hdop    .. num         .. Horizontal Dilution of Precision
+     * 9    alt     .. num         .. Altitude above mean sea level
+     */
+    bool parseGPGGA(const String & line);
+
+    /**
+     * Read the coordinates using GPRMC
+     *
+     * 1    time    .. hhmmss.ss   .. UTC time
+     * 2    status  .. char        .. Status, V = Navigation receiver warning, A = Data valid