size_t gnrc_netif_addr_from_str(const char *str, uint8_t *out)
{
/* Walk over str from the end. */
/* Take two chars a time as one hex value (%hhx). */
/* Leading zeros can be omitted. */
/* Every non-hexadimal character is a delimiter. */
/* Leading, tailing and adjacent delimiters are forbidden. */
const char *end_str = str;
uint8_t *out_end = out;
size_t count = 0;
int assert_cell = 1;
assert(out != NULL);
if ((str == NULL) || (str[0] == '\0')) {
return 0;
}
/* find end of string */
while (end_str[1]) {
++end_str;
}
while (end_str >= str) {
int a = 0, b = _dehex(*end_str--, -1);
if (b < 0) {
if (assert_cell) {
return 0;
}
else {
assert_cell = 1;
continue;
}
}
assert_cell = 0;
if (end_str >= str) {
a = _dehex(*end_str--, 0);
}
count++;
*out_end++ = (a << 4) | b;
}
if (assert_cell) {
return 0;
}
/* out is reversed */
while (out < --out_end) {
uint8_t tmp = *out_end;
*out_end = *out;
*out++ = tmp;
}
return count;
}
static int _parse_addr(uint8_t *out, size_t out_len, const char *in)
{
const char *end_str = in;
uint8_t *out_end = out;
size_t count = 0;
int assert_cell = 1;
if (!in || !*in) {
return 0;
}
while (end_str[1]) {
++end_str;
}
while (end_str >= in) {
int a = 0, b = _dehex(*end_str--, -1);
if (b < 0) {
if (assert_cell) {
return 0;
}
else {
assert_cell = 1;
continue;
}
}
assert_cell = 0;
if (end_str >= in) {
a = _dehex(*end_str--, 0);
}
if (++count > out_len) {
return 0;
}
*out_end++ = (a << 4) | b;
}
if (assert_cell) {
return 0;
}
/* out is reversed */
while (out < --out_end) {
uint8_t tmp = *out_end;
*out_end = *out;
*out++ = tmp;
}
return count;
}
int nmea_parse(char c) {
// avoid runaway sentences (>99 chars or >29 terms) and terms (>14 chars)
if ((n >= 100) || (_terms >= 30) || (_nt >= 15)) { _state = 0; }
// LF and CR always reset parser
if ((c == 0x0A) || (c == 0x0D)) { _state = 0; }
// '$' always starts a new sentence
if (c == '$') {
_gprmc_tag = 0;
_parity = 0;
_terms = 0;
_nt = 0;
_sentence[0] = c;
n = 1;
_state = 1;
return 0;
}
// parse other chars according to parser state
switch(_state) {
case 0:
// waiting for '$', do nothing
break;
case 1:
// decode chars after '$' and before '*' found
if (n < 7) {
// see if first seven chars match "$GPRMC,"
if (c == _GPRMC_TERM[n]) { _gprmc_tag++; }
}
// add received char to sentence
_sentence[n++] = c;
switch (c) {
case ',':
// ',' delimits the individual terms
(_term[_terms++])[_nt] = 0;
_nt = 0;
_parity = _parity ^ c;
break;
case '*':
// '*' delimits term and precedes checksum term
(_term[_terms++])[_nt] = 0;
_nt = 0;
_state++;
break;
default:
// all other chars between '$' and '*' are part of a term
(_term[_terms])[_nt++] = c;
_parity = _parity ^ c;
break;
}
break;
case 2:
// first char following '*' is checksum MSB
_sentence[n++] = c;
(_term[_terms])[_nt++] = c;
_parity = _parity - (16 * _dehex(c)); // replace with bitshift?
_state++;
break;
case 3:
// second char after '*' completes the checksum (LSB)
_sentence[n++] = c;
_sentence[n++] = 0;
(_term[_terms])[_nt++] = c;
(_term[_terms++])[_nt] = 0;
_state = 0;
_parity = _parity - _dehex(c);
// when parity is zero, checksum was correct!
if (_parity == 0) {
// accept all sentences, or only GPRMC datatype?
if ((!_gprmc_only) || (_gprmc_tag == 6)) {
// copy _sentence[] to f_sentence[]
while ((--n) >= 0) { f_sentence[n] = _sentence[n]; }
// copy all _terms[] to f_terms[]
for (f_terms=0; f_terms<_terms; f_terms++) {
_nt = 0;
while ((_term[f_terms])[_nt]) {
(f_term[f_terms])[_nt] = (_term[f_terms])[_nt];
_nt++;
}
(f_term[f_terms])[_nt] = 0;
}
// when sentence is of datatype GPRMC
if (_gprmc_tag == 6) {
// store values of relevant GPRMC terms
_gprmc_utc = _decimal(_term[1]);
_gprmc_status = (_term[2])[0];
// calculate signed degree-decimal value of latitude term
_gprmc_lat = _decimal(_term[3]) / 100.0;
_degs = floor(_gprmc_lat);
_gprmc_lat = (100.0 * (_gprmc_lat - _degs)) / 60.0;
_gprmc_lat += _degs;
// southern hemisphere is negative-valued
if ((_term[4])[0] == 'S') {
_gprmc_lat = 0.0 - _gprmc_lat;
}
// calculate signed degree-decimal value of longitude term
_gprmc_long = _decimal(_term[5]) / 100.0;
_degs = floor(_gprmc_long);
_gprmc_long = (100.0 * (_gprmc_long - _degs)) / 60.0;
_gprmc_long += _degs;
// western hemisphere is negative-valued
if ((_term[6])[0] == 'W') {
_gprmc_long = 0.0 - _gprmc_long;
}
_gprmc_speed = _decimal(_term[7]);
_gprmc_angle = _decimal(_term[8]);
}
// sentence accepted!
return 1;
}
}
break;
default:
_state = 0;
break;
}
return 0;
}
bool SimpleTelemetry::parseMessage(char c)
{
// Dont let sentences run away
if ((n >= MAXSENTENCE) || (_terms >= MAXTERMS) || (_nt >= MAXWORD))
{
/*
#ifdef DEBUG
debugPort->println("Runaway sentance. Resetting state. ");
debugPort->print("Sentence is: ");
debugPort->print(n);
debugPort->print(" chars ");
debugPort->print(_terms);
debugPort->print(" terms ");
debugPort->print(_nt);
debugPort->println(" words.");
#endif
*/
_state = 0;
n = 0;
_terms = 0;
_nt = 0;
}
// LF and CR always reset parser
if ((c == 0x0A) || (c == 0x0D))
{
/*
#ifdef DEBUG
debugPort->println("LF or CR received. Resetting state.");
#endif
*/
_state = 0;
}
// $ Always starts a new sentence
if (c == '$')
{
/*
#ifdef DEBUG
debugPort->println("New sentence.");
#endif
*/
//_sentence[0] = c;
_state = 1;
_terms = 0;
n = 1;
_nt = 0;
checksum = 0;
return 0;
}
switch (_state)
{
case 0 :
{
// Waiting for $. Do nothing
/*
#ifdef DEBUG
debugPort->println("Waiting for $.");
#endif
*/
break;
}
case 1 :
{
//_sentence[n++] = c;
n++;
switch (c)
{
// Delimits the terms in sentence
case ',' :
{
/*
#ifdef DEBUG
debugPort->print(",");
#endif
*/
(_term[_terms++])[_nt] = 0;
_nt = 0;
checksum = checksum ^ c;
break;
}
// End of terms
case '*' :
{
/*
#ifdef DEBUG
debugPort->print("*");
#endif
*/
(_term[_terms++])[_nt] = 0;
_nt = 0;
_state++;
break;
}
// All characters between $ and *
default :
{
/*
#ifdef DEBUG
debugPort->print(c);
#endif
*/
(_term[_terms])[_nt++] = c;
checksum = checksum ^ c;
break;
}
}
break;
}
// Checksum MSB
case 2 :
{
/*
#ifdef DEBUG
debugPort->print(c);
#endif
*/
checksum = checksum - (16 * _dehex(c));
_state++;
break;
}
// Sentence complete
case 3 :
{
/*
#ifdef DEBUG
debugPort->println(c);
#endif
*/
checksum = checksum - _dehex(c);
/*
#ifdef DEBUG
debugPort->print("Scentence complete. Checksum is: ");
debugPort->println(checksum, HEX);
#endif
*/
if (checksum == 0)
{
for (f_terms=0; f_terms<_terms; f_terms++)
{
_nt = 0;
while ((_term[f_terms])[_nt])
{
(f_term[f_terms])[_nt] = (_term[f_terms])[_nt];
_nt++;
}
(f_term[f_terms])[_nt] = 0;
}
_state = 0;
return 1;
}
break;
}
default :
{
_state = 0;
break;
}
}
return 0;
}
