Commit 1f50ba51 authored by Ondřej Kučera's avatar Ondřej Kučera
Browse files

Revert "Open Cansat library for gps added"

This reverts commit 47424361
parent cfd512ed
#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;
}
#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