955 lines
28 KiB
C
955 lines
28 KiB
C
/****************************************************************************
|
|
*
|
|
* Copy right: 2024-, Copyrigths of EigenComm Ltd.
|
|
* File name: api_gnss.c
|
|
* Description: ec7xx openHAL Gnss source file
|
|
* History: Rev1.0 2024-11-12
|
|
*
|
|
****************************************************************************/
|
|
|
|
#include "api_gnss.h"
|
|
#include <stdarg.h>
|
|
#include <string.h>
|
|
|
|
#define boolstr(s) ((s) ? "true" : "false")
|
|
|
|
int hex2int(char c)
|
|
{
|
|
if (c >= '0' && c <= '9')
|
|
return c - '0';
|
|
if (c >= 'A' && c <= 'F')
|
|
return c - 'A' + 10;
|
|
if (c >= 'a' && c <= 'f')
|
|
return c - 'a' + 10;
|
|
return -1;
|
|
}
|
|
|
|
bool minmea_check(const char *sentence, bool strict)
|
|
{
|
|
uint8_t checksum = 0x00;
|
|
|
|
// A valid sentence starts with "$".
|
|
if (*sentence == '$')
|
|
sentence++;
|
|
|
|
// The optional checksum is an XOR of all bytes between "$" and "*".
|
|
while (*sentence && *sentence != '*' && isprint((unsigned char)*sentence))
|
|
checksum ^= *sentence++;
|
|
|
|
// If checksum is present...
|
|
if (*sentence == '*')
|
|
{
|
|
// Extract checksum.
|
|
sentence++;
|
|
int upper = hex2int(*sentence++);
|
|
if (upper == -1)
|
|
return false;
|
|
int lower = hex2int(*sentence++);
|
|
if (lower == -1)
|
|
return false;
|
|
int expected = upper << 4 | lower;
|
|
|
|
// Check for checksum mismatch.
|
|
if (checksum != expected)
|
|
return false;
|
|
}
|
|
else if (strict)
|
|
{
|
|
// Discard non-checksummed frames in strict mode.
|
|
return false;
|
|
}
|
|
|
|
// The only stuff allowed at this point is a newline.
|
|
while (*sentence == '\r' || *sentence == '\n')
|
|
{
|
|
sentence++;
|
|
}
|
|
|
|
if (*sentence)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool minmea_scan(const char *sentence, const char *format, ...)
|
|
{
|
|
bool result = false;
|
|
bool optional = false;
|
|
|
|
if (sentence == NULL)
|
|
return false;
|
|
|
|
va_list ap;
|
|
va_start(ap, format);
|
|
|
|
const char *field = sentence;
|
|
#define next_field() \
|
|
do \
|
|
{ \
|
|
/* Progress to the next field. */ \
|
|
while (minmea_isfield(*sentence)) \
|
|
sentence++; \
|
|
/* Make sure there is a field there. */ \
|
|
if (*sentence == ',') \
|
|
{ \
|
|
sentence++; \
|
|
field = sentence; \
|
|
} \
|
|
else \
|
|
{ \
|
|
field = NULL; \
|
|
} \
|
|
} while (0)
|
|
|
|
while (*format)
|
|
{
|
|
char type = *format++;
|
|
|
|
if (type == ';')
|
|
{
|
|
// All further fields are optional.
|
|
optional = true;
|
|
continue;
|
|
}
|
|
|
|
if (!field && !optional)
|
|
{
|
|
// Field requested but we ran out if input. Bail out.
|
|
goto parse_error;
|
|
}
|
|
|
|
switch (type)
|
|
{
|
|
case 'c':
|
|
{ // Single character field (char).
|
|
char value = '\0';
|
|
|
|
if (field && minmea_isfield(*field))
|
|
value = *field;
|
|
|
|
*va_arg(ap, char *) = value;
|
|
}
|
|
break;
|
|
|
|
case 'd':
|
|
{ // Single character direction field (int).
|
|
int value = 0;
|
|
|
|
if (field && minmea_isfield(*field))
|
|
{
|
|
switch (*field)
|
|
{
|
|
case 'N':
|
|
case 'E':
|
|
value = 1;
|
|
break;
|
|
case 'S':
|
|
case 'W':
|
|
value = -1;
|
|
break;
|
|
default:
|
|
goto parse_error;
|
|
}
|
|
}
|
|
|
|
*va_arg(ap, int *) = value;
|
|
}
|
|
break;
|
|
|
|
case 'f':
|
|
{ // Fractional value with scale (struct minmea_float).
|
|
int sign = 0;
|
|
int_least32_t value = -1;
|
|
int_least32_t scale = 0;
|
|
|
|
if (field)
|
|
{
|
|
while (minmea_isfield(*field))
|
|
{
|
|
if (*field == '+' && !sign && value == -1)
|
|
{
|
|
sign = 1;
|
|
}
|
|
else if (*field == '-' && !sign && value == -1)
|
|
{
|
|
sign = -1;
|
|
}
|
|
else if (isdigit((unsigned char)*field))
|
|
{
|
|
int digit = *field - '0';
|
|
if (value == -1)
|
|
value = 0;
|
|
if (value > (INT_LEAST32_MAX - digit) / 10)
|
|
{
|
|
/* we ran out of bits, what do we do? */
|
|
if (scale)
|
|
{
|
|
/* truncate extra precision */
|
|
break;
|
|
}
|
|
else
|
|
{
|
|
/* integer overflow. bail out. */
|
|
goto parse_error;
|
|
}
|
|
}
|
|
value = (10 * value) + digit;
|
|
if (scale)
|
|
scale *= 10;
|
|
}
|
|
else if (*field == '.' && scale == 0)
|
|
{
|
|
scale = 1;
|
|
}
|
|
else if (*field == ' ')
|
|
{
|
|
/* Allow spaces at the start of the field. Not NMEA
|
|
* conformant, but some modules do this. */
|
|
if (sign != 0 || value != -1 || scale != 0)
|
|
goto parse_error;
|
|
}
|
|
else
|
|
{
|
|
goto parse_error;
|
|
}
|
|
field++;
|
|
}
|
|
}
|
|
|
|
if ((sign || scale) && value == -1)
|
|
goto parse_error;
|
|
|
|
if (value == -1)
|
|
{
|
|
/* No digits were scanned. */
|
|
value = 0;
|
|
scale = 0;
|
|
}
|
|
else if (scale == 0)
|
|
{
|
|
/* No decimal point. */
|
|
scale = 1;
|
|
}
|
|
if (sign)
|
|
value *= sign;
|
|
|
|
*va_arg(ap, struct minmea_float *) = (struct minmea_float){value, scale};
|
|
}
|
|
break;
|
|
|
|
case 'i':
|
|
{ // Integer value, default 0 (int).
|
|
int value = 0;
|
|
|
|
if (field)
|
|
{
|
|
char *endptr;
|
|
value = strtol(field, &endptr, 10);
|
|
if (minmea_isfield(*endptr))
|
|
goto parse_error;
|
|
}
|
|
|
|
*va_arg(ap, int *) = value;
|
|
}
|
|
break;
|
|
|
|
case 's':
|
|
{ // String value (char *).
|
|
char *buf = va_arg(ap, char *);
|
|
|
|
if (field)
|
|
{
|
|
while (minmea_isfield(*field))
|
|
*buf++ = *field++;
|
|
}
|
|
|
|
*buf = '\0';
|
|
}
|
|
break;
|
|
|
|
case 't':
|
|
{ // NMEA talker+sentence identifier (char *).
|
|
// This field is always mandatory.
|
|
if (!field)
|
|
goto parse_error;
|
|
|
|
if (field[0] != '$')
|
|
goto parse_error;
|
|
for (int f = 0; f < 5; f++)
|
|
if (!minmea_isfield(field[1 + f]))
|
|
goto parse_error;
|
|
|
|
char *buf = va_arg(ap, char *);
|
|
memcpy(buf, field + 1, 5);
|
|
buf[5] = '\0';
|
|
}
|
|
break;
|
|
|
|
case 'D':
|
|
{ // Date (int, int, int), -1 if empty.
|
|
struct minmea_date *date = va_arg(ap, struct minmea_date *);
|
|
|
|
int d = -1, m = -1, y = -1;
|
|
|
|
if (field && minmea_isfield(*field))
|
|
{
|
|
// Always six digits.
|
|
for (int f = 0; f < 6; f++)
|
|
if (!isdigit((unsigned char)field[f]))
|
|
goto parse_error;
|
|
|
|
char dArr[] = {field[0], field[1], '\0'};
|
|
char mArr[] = {field[2], field[3], '\0'};
|
|
char yArr[] = {field[4], field[5], '\0'};
|
|
d = strtol(dArr, NULL, 10);
|
|
m = strtol(mArr, NULL, 10);
|
|
y = strtol(yArr, NULL, 10);
|
|
}
|
|
|
|
date->day = d;
|
|
date->month = m;
|
|
date->year = y;
|
|
}
|
|
break;
|
|
|
|
case 'T':
|
|
{ // Time (int, int, int, int), -1 if empty.
|
|
struct minmea_time *time_ = va_arg(ap, struct minmea_time *);
|
|
|
|
int h = -1, i = -1, s = -1, u = -1;
|
|
|
|
if (field && minmea_isfield(*field))
|
|
{
|
|
// Minimum required: integer time.
|
|
for (int f = 0; f < 6; f++)
|
|
if (!isdigit((unsigned char)field[f]))
|
|
goto parse_error;
|
|
|
|
char hArr[] = {field[0], field[1], '\0'};
|
|
char iArr[] = {field[2], field[3], '\0'};
|
|
char sArr[] = {field[4], field[5], '\0'};
|
|
h = strtol(hArr, NULL, 10);
|
|
i = strtol(iArr, NULL, 10);
|
|
s = strtol(sArr, NULL, 10);
|
|
field += 6;
|
|
|
|
// Extra: fractional time. Saved as microseconds.
|
|
if (*field++ == '.')
|
|
{
|
|
uint32_t value = 0;
|
|
uint32_t scale = 1000000LU;
|
|
while (isdigit((unsigned char)*field) && scale > 1)
|
|
{
|
|
value = (value * 10) + (*field++ - '0');
|
|
scale /= 10;
|
|
}
|
|
u = value * scale;
|
|
}
|
|
else
|
|
{
|
|
u = 0;
|
|
}
|
|
}
|
|
|
|
time_->hours = h;
|
|
time_->minutes = i;
|
|
time_->seconds = s;
|
|
time_->microseconds = u;
|
|
}
|
|
break;
|
|
|
|
case '_':
|
|
{ // Ignore the field.
|
|
}
|
|
break;
|
|
|
|
default:
|
|
{ // Unknown.
|
|
goto parse_error;
|
|
}
|
|
}
|
|
|
|
next_field();
|
|
}
|
|
|
|
result = true;
|
|
|
|
parse_error:
|
|
va_end(ap);
|
|
return result;
|
|
}
|
|
|
|
enum minmea_sentence_id minmea_sentence_id(const char *sentence, bool strict)
|
|
{
|
|
if (!minmea_check(sentence, strict))
|
|
return MINMEA_INVALID;
|
|
|
|
char type[6];
|
|
if (!minmea_scan(sentence, "t", type))
|
|
return MINMEA_INVALID;
|
|
|
|
if (!strcmp(type + 2, "GBS"))
|
|
return MINMEA_SENTENCE_GBS;
|
|
if (!strcmp(type + 2, "GGA"))
|
|
return MINMEA_SENTENCE_GGA;
|
|
if (!strcmp(type + 2, "GLL"))
|
|
return MINMEA_SENTENCE_GLL;
|
|
if (!strcmp(type + 2, "GSA"))
|
|
return MINMEA_SENTENCE_GSA;
|
|
if (!strcmp(type + 2, "GST"))
|
|
return MINMEA_SENTENCE_GST;
|
|
if (!strcmp(type + 2, "GSV"))
|
|
return MINMEA_SENTENCE_GSV;
|
|
if (!strcmp(type + 2, "RMC"))
|
|
return MINMEA_SENTENCE_RMC;
|
|
if (!strcmp(type + 2, "VTG"))
|
|
return MINMEA_SENTENCE_VTG;
|
|
if (!strcmp(type + 2, "ZDA"))
|
|
return MINMEA_SENTENCE_ZDA;
|
|
if (!strcmp(type + 2, "TXT"))
|
|
return MINMEA_SENTENCE_TXT;
|
|
return MINMEA_UNKNOWN;
|
|
}
|
|
|
|
bool minmea_parse_gbs(struct minmea_sentence_gbs *frame, const char *sentence)
|
|
{
|
|
// $GNGBS,170556.00,3.0,2.9,8.3,,,,*5C
|
|
char type[6];
|
|
if (!minmea_scan(sentence, "tTfffifff",
|
|
type,
|
|
&frame->time,
|
|
&frame->err_latitude,
|
|
&frame->err_longitude,
|
|
&frame->err_altitude,
|
|
&frame->svid,
|
|
&frame->prob,
|
|
&frame->bias,
|
|
&frame->stddev))
|
|
return false;
|
|
if (strcmp(type + 2, "GBS"))
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
// $GNRMC,071209.00,A,3110.94352,N,12136.37477,E,0.193,,070723,,,A,V*1E
|
|
// 0[0,69]$GNRMC,063116.00,A,3110.96251,N,12136.35722,E,0.069,,100723,,,A,V*13
|
|
bool minmea_parse_rmc(struct minmea_sentence_rmc *frame, const char *sentence)
|
|
{
|
|
// $GPRMC,081836,A,3751.65,S,14507.36,E,000.0,360.0,130998,011.3,E*62
|
|
char validity;
|
|
int latitude_direction;
|
|
int longitude_direction;
|
|
int variation_direction;
|
|
if (!minmea_scan(sentence, "tTcfdfdffDfd",
|
|
frame->type,
|
|
&frame->time,
|
|
&validity,
|
|
&frame->latitude, &latitude_direction,
|
|
&frame->longitude, &longitude_direction,
|
|
&frame->speed,
|
|
&frame->course,
|
|
&frame->date,
|
|
&frame->variation, &variation_direction))
|
|
return false;
|
|
if (strcmp(frame->type + 2, "RMC"))
|
|
return false;
|
|
|
|
frame->valid = (validity == 'A');
|
|
frame->latitude.value *= latitude_direction;
|
|
frame->longitude.value *= longitude_direction;
|
|
frame->variation.value *= variation_direction;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool minmea_parse_gga(struct minmea_sentence_gga *frame, const char *sentence)
|
|
{
|
|
// $GNGGA,084852.000,2236.9453,N,11408.4790,E,1,05,3.1,89.7,M,0.0,M,,*48
|
|
// $GPGGA,123519 ,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47
|
|
int latitude_direction;
|
|
int longitude_direction;
|
|
if (!minmea_scan(sentence, "tTfdfdiiffcfcf_",
|
|
frame->type,
|
|
&frame->time,
|
|
&frame->latitude, &latitude_direction,
|
|
&frame->longitude, &longitude_direction,
|
|
&frame->fix_quality,
|
|
&frame->satellites_tracked,
|
|
&frame->hdop,
|
|
&frame->altitude, &frame->altitude_units,
|
|
&frame->height, &frame->height_units, // 天线大地高
|
|
&frame->dgps_age))
|
|
return false;
|
|
if (strcmp(frame->type + 2, "GGA"))
|
|
return false;
|
|
|
|
frame->latitude.value *= latitude_direction;
|
|
frame->longitude.value *= longitude_direction;
|
|
|
|
return true;
|
|
}
|
|
// $GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39
|
|
// 2[138,69]:$GNGSA,A,3,06,13,15,195,05,29,30,199,07,11,20,18,1.54,0.87,1.27,1*07
|
|
// 3[207,47]:$GNGSA,A,3,05,11,,,,,,,,,,,1.54,0.87,1.27,3*0D
|
|
// 4[254,55]:$GNGSA,A,3,67,82,88,66,81,68,,,,,,,1.54,0.87,1.27,2*05
|
|
bool minmea_parse_gsa(struct minmea_sentence_gsa *frame, const char *sentence)
|
|
{
|
|
if (!minmea_scan(sentence, "tciiiiiiiiiiiiifff",
|
|
frame->type,
|
|
&frame->mode,
|
|
&frame->fix_type,
|
|
&frame->sats[0],
|
|
&frame->sats[1],
|
|
&frame->sats[2],
|
|
&frame->sats[3],
|
|
&frame->sats[4],
|
|
&frame->sats[5],
|
|
&frame->sats[6],
|
|
&frame->sats[7],
|
|
&frame->sats[8],
|
|
&frame->sats[9],
|
|
&frame->sats[10],
|
|
&frame->sats[11],
|
|
&frame->pdop,
|
|
&frame->hdop,
|
|
&frame->vdop))
|
|
return false;
|
|
if (strcmp(frame->type + 2, "GSA"))
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool minmea_parse_gll(struct minmea_sentence_gll *frame, const char *sentence)
|
|
{
|
|
// $GPGLL,3723.2475,N,12158.3416,W,161229.487,A,A*41$;
|
|
|
|
int latitude_direction;
|
|
int longitude_direction;
|
|
|
|
if (!minmea_scan(sentence, "tfdfdTc;c",
|
|
frame->type,
|
|
&frame->latitude, &latitude_direction,
|
|
&frame->longitude, &longitude_direction,
|
|
&frame->time,
|
|
&frame->status,
|
|
&frame->mode))
|
|
return false;
|
|
if (strcmp(frame->type + 2, "GLL"))
|
|
return false;
|
|
|
|
frame->latitude.value *= latitude_direction;
|
|
frame->longitude.value *= longitude_direction;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool minmea_parse_gst(struct minmea_sentence_gst *frame, const char *sentence)
|
|
{
|
|
// $GPGST,024603.00,3.2,6.6,4.7,47.3,5.8,5.6,22.0*58
|
|
char type[6];
|
|
|
|
if (!minmea_scan(sentence, "tTfffffff",
|
|
type,
|
|
&frame->time,
|
|
&frame->rms_deviation,
|
|
&frame->semi_major_deviation,
|
|
&frame->semi_minor_deviation,
|
|
&frame->semi_major_orientation,
|
|
&frame->latitude_error_deviation,
|
|
&frame->longitude_error_deviation,
|
|
&frame->altitude_error_deviation))
|
|
return false;
|
|
if (strcmp(type + 2, "GST"))
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool minmea_parse_gsv(struct minmea_sentence_gsv *frame, const char *sentence)
|
|
{
|
|
if (!minmea_scan(sentence, "tiii;iiiiiiiiiiiiiiii",
|
|
frame->type,
|
|
&frame->total_msgs,
|
|
&frame->msg_nr,
|
|
&frame->total_sats,
|
|
&frame->sats[0].nr,
|
|
&frame->sats[0].elevation,
|
|
&frame->sats[0].azimuth,
|
|
&frame->sats[0].snr,
|
|
&frame->sats[1].nr,
|
|
&frame->sats[1].elevation,
|
|
&frame->sats[1].azimuth,
|
|
&frame->sats[1].snr,
|
|
&frame->sats[2].nr,
|
|
&frame->sats[2].elevation,
|
|
&frame->sats[2].azimuth,
|
|
&frame->sats[2].snr,
|
|
&frame->sats[3].nr,
|
|
&frame->sats[3].elevation,
|
|
&frame->sats[3].azimuth,
|
|
&frame->sats[3].snr))
|
|
{
|
|
return false;
|
|
}
|
|
if (strcmp(frame->type + 2, "GSV"))
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool minmea_parse_vtg(struct minmea_sentence_vtg *frame, const char *sentence)
|
|
{
|
|
// $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48
|
|
// $GPVTG,156.1,T,140.9,M,0.0,N,0.0,K*41
|
|
// $GPVTG,096.5,T,083.5,M,0.0,N,0.0,K,D*22
|
|
// $GPVTG,188.36,T,,M,0.820,N,1.519,K,A*3F
|
|
|
|
char c_true, c_magnetic, c_knots, c_kph, c_faa_mode;
|
|
|
|
if (!minmea_scan(sentence, "t;fcfcfcfcc",
|
|
frame->type,
|
|
&frame->true_track_degrees,
|
|
&c_true,
|
|
&frame->magnetic_track_degrees,
|
|
&c_magnetic,
|
|
&frame->speed_knots,
|
|
&c_knots,
|
|
&frame->speed_kph,
|
|
&c_kph,
|
|
&c_faa_mode))
|
|
return false;
|
|
if (strcmp(frame->type + 2, "VTG"))
|
|
return false;
|
|
// values are only valid with the accompanying characters
|
|
if (c_true != 'T')
|
|
frame->true_track_degrees.scale = 0;
|
|
if (c_magnetic != 'M')
|
|
frame->magnetic_track_degrees.scale = 0;
|
|
if (c_knots != 'N')
|
|
frame->speed_knots.scale = 0;
|
|
if (c_kph != 'K')
|
|
frame->speed_kph.scale = 0;
|
|
frame->faa_mode = (enum minmea_faa_mode)c_faa_mode;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool minmea_parse_zda(struct minmea_sentence_zda *frame, const char *sentence)
|
|
{
|
|
// $GPZDA,201530.00,04,07,2002,00,00*60
|
|
if (!minmea_scan(sentence, "tTiiiii",
|
|
frame->type,
|
|
&frame->time,
|
|
&frame->date.day,
|
|
&frame->date.month,
|
|
&frame->date.year,
|
|
&frame->hour_offset,
|
|
&frame->minute_offset))
|
|
return false;
|
|
if (strcmp(frame->type + 2, "ZDA"))
|
|
return false;
|
|
|
|
// check offsets
|
|
if (abs(frame->hour_offset) > 13 ||
|
|
frame->minute_offset > 59 ||
|
|
frame->minute_offset < 0)
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool minmea_parse_txt(struct minmea_sentence_txt *frame, const char *sentence)
|
|
{
|
|
// $GPTXT,01,01,01,ANTENNA OK*35
|
|
if (!minmea_scan(sentence, "tiiis",
|
|
frame->type,
|
|
&frame->data1,
|
|
&frame->data2,
|
|
&frame->data3,
|
|
NULL))
|
|
return false;
|
|
if (strcmp(frame->type + 2, "TXT"))
|
|
return false;
|
|
if (strstr(sentence, "ANTENNA OK"))
|
|
frame->flag = 1;
|
|
return true;
|
|
}
|
|
|
|
uint16_t nmea_line_parse(char *line, gnss_data_t *gnss)
|
|
{
|
|
char *head = NULL;
|
|
char *token = NULL;
|
|
uint8_t ret = 0;
|
|
uint16_t num = 0;
|
|
enum minmea_sentence_id type_id = minmea_sentence_id(line, false);
|
|
switch (type_id)
|
|
{
|
|
case MINMEA_SENTENCE_GLL:
|
|
{
|
|
struct minmea_sentence_gll frame;
|
|
if (minmea_parse_gll(&frame, line))
|
|
{
|
|
#if GNSS_DEBUG_PRINTF
|
|
printf("%s %d:%d:%d:%d\r\n",
|
|
frame.type,
|
|
frame.time.hours,
|
|
frame.time.minutes,
|
|
frame.time.seconds,
|
|
frame.time.microseconds);
|
|
printf("\t latitude:%d/%d,longitude %d/%d\r\n",
|
|
frame.latitude.value, frame.latitude.scale,
|
|
frame.longitude.value, frame.longitude.scale);
|
|
printf("\t status %c, mode %c\r\n", frame.status, frame.mode);
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
#if GNSS_DEBUG_PRINTF
|
|
printf("$xxGLL sentence is not parsed\r\n");
|
|
#endif
|
|
}
|
|
}
|
|
break;
|
|
case MINMEA_SENTENCE_GSA: // 当前卫星信息
|
|
{
|
|
struct minmea_sentence_gsa frame;
|
|
if (minmea_parse_gsa(&frame, line))
|
|
{
|
|
#if GNSS_DEBUG_PRINTF
|
|
printf("%s\t[P%d,H%d,V%d]:", frame.type, frame.pdop, frame.hdop, frame.vdop);
|
|
for (int i = 0; i < 12; i++)
|
|
{
|
|
if (frame.sats[i])
|
|
{
|
|
printf("%d ", frame.sats[i]);
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
#if GNSS_DEBUG_PRINTF
|
|
printf("$xxGSA sentence is not parsed\r\n");
|
|
#endif
|
|
}
|
|
}
|
|
break;
|
|
case MINMEA_SENTENCE_GGA: // 全球定位信息
|
|
{
|
|
struct minmea_sentence_gga frame;
|
|
if (minmea_parse_gga(&frame, line))
|
|
{
|
|
gnss->altitude = frame.altitude.value;
|
|
gnss->tracked = frame.satellites_tracked;
|
|
gnss->quality = frame.fix_quality;
|
|
gnss->hdop = frame.hdop.value;
|
|
gnss->time.hours = frame.time.hours;
|
|
gnss->time.minutes = frame.time.minutes;
|
|
gnss->time.seconds = frame.time.seconds;
|
|
gnss->latitude = frame.latitude.value;
|
|
gnss->longitude = frame.longitude.value;
|
|
gnss->latitude_scale = frame.latitude.scale;
|
|
gnss->longitude_scale = frame.longitude.scale;
|
|
gnss->altitude_scale = frame.altitude.scale;
|
|
|
|
#if GNSS_DEBUG_PRINTF
|
|
printf("%s\t%d:%d:%d:%d",
|
|
frame.type,
|
|
frame.time.hours,
|
|
frame.time.minutes,
|
|
frame.time.seconds,
|
|
frame.time.microseconds);
|
|
printf("\tlatitude:%d.%d,longitude:%d.%d,altitude:%d.%d\r\n",
|
|
(frame.latitude.value / frame.latitude.scale), (frame.latitude.value % frame.latitude.scale),
|
|
(frame.longitude.value / frame.longitude.scale), (frame.longitude.value % frame.longitude.scale),
|
|
(frame.altitude.value / frame.altitude.scale), (frame.altitude.value % frame.altitude.scale));
|
|
printf("\thdop:%d,tracked:%d,quality:%d\r\n", frame.hdop.value, frame.satellites_tracked, frame.fix_quality);
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
#if GNSS_DEBUG_PRINTF
|
|
printf("$xxGGA sentence is not parsed\r\n");
|
|
#endif
|
|
}
|
|
}
|
|
break;
|
|
case MINMEA_SENTENCE_GSV: // 可见卫星信息
|
|
{
|
|
struct minmea_sentence_gsv frame;
|
|
if (minmea_parse_gsv(&frame, line))
|
|
{
|
|
}
|
|
}
|
|
break;
|
|
case MINMEA_SENTENCE_RMC:
|
|
{
|
|
struct minmea_sentence_rmc frame;
|
|
if (minmea_parse_rmc(&frame, line)) // 推荐定位信息
|
|
{
|
|
if (!frame.valid)
|
|
{
|
|
gnss->valid = 0;
|
|
// gIsFix = false;
|
|
return;
|
|
}
|
|
// gIsFix = true;
|
|
gnss->valid = 1;
|
|
|
|
// gnss->date = frame.date;
|
|
gnss->time.year = frame.date.year;
|
|
gnss->time.month = frame.date.month;
|
|
gnss->time.day = frame.date.day;
|
|
gnss->time.hours = frame.time.hours;
|
|
gnss->time.minutes = frame.time.minutes;
|
|
gnss->time.seconds = frame.time.seconds;
|
|
gnss->speed = frame.speed.value;
|
|
// 在此处通知到任务
|
|
#if GNSS_DEBUG_PRINTF
|
|
printf("%s\t%02d.%02d.%02d %d:%d:%d ",
|
|
frame.type,
|
|
frame.date.year,
|
|
frame.date.month,
|
|
frame.date.day,
|
|
frame.time.hours,
|
|
frame.time.minutes,
|
|
frame.time.seconds);
|
|
// 311096596/100000,1213635808/100000,51/1000
|
|
printf("\tlatitude:%d.%d,longitude:%d.%d,speed:%d.%d\r\n",
|
|
(frame.latitude.value / frame.latitude.scale), (frame.latitude.value % frame.latitude.scale),
|
|
(frame.longitude.value / frame.longitude.scale), (frame.longitude.value % frame.longitude.scale),
|
|
(frame.speed.value / frame.speed.scale), (frame.speed.value % frame.speed.scale));
|
|
printf("scale = %d\r\n", frame.latitude.scale);
|
|
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
#if GNSS_DEBUG_PRINTF
|
|
printf("$xxRMC sentence is not parsed\r\n");
|
|
#endif
|
|
}
|
|
}
|
|
break;
|
|
case MINMEA_SENTENCE_VTG:
|
|
{
|
|
struct minmea_sentence_vtg frame;
|
|
if (minmea_parse_vtg(&frame, line))
|
|
{
|
|
#if GNSS_DEBUG_PRINTF
|
|
printf("%s: true track degrees = %d\r\n",
|
|
frame.type,
|
|
frame.true_track_degrees);
|
|
printf("\t magnetic track degrees = %d\r\n",
|
|
frame.magnetic_track_degrees);
|
|
printf("\t speed knots = %d\r\n",
|
|
frame.speed_knots);
|
|
printf("\t speed kph = %d\r\n",
|
|
frame.speed_kph);
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
#if GNSS_DEBUG_PRINTF
|
|
printf("$xxVTG sentence is not parsed\r\n");
|
|
#endif
|
|
}
|
|
}
|
|
break;
|
|
case MINMEA_SENTENCE_ZDA:
|
|
{
|
|
struct minmea_sentence_zda frame;
|
|
if (minmea_parse_zda(&frame, line))
|
|
{
|
|
#if GNSS_DEBUG_PRINTF
|
|
printf("%s: %d:%d:%d %02d.%02d.%d UTC%+03d:%02d",
|
|
frame.type,
|
|
frame.time.hours,
|
|
frame.time.minutes,
|
|
frame.time.seconds,
|
|
frame.date.day,
|
|
frame.date.month,
|
|
frame.date.year,
|
|
frame.hour_offset,
|
|
frame.minute_offset);
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
#if GNSS_DEBUG_PRINTF
|
|
printf("$xxZDA sentence is not parsed\r\n");
|
|
#endif
|
|
}
|
|
}
|
|
break;
|
|
case MINMEA_SENTENCE_TXT:
|
|
{
|
|
struct minmea_sentence_txt frame;
|
|
if (minmea_parse_txt(&frame, line))
|
|
{
|
|
#if GNSS_DEBUG_PRINTF
|
|
printf("%s: %d:%d:%d %d",
|
|
frame.type,
|
|
frame.data1,
|
|
frame.data2,
|
|
frame.data3,
|
|
frame.flag);
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
#if GNSS_DEBUG_PRINTF
|
|
printf("$xxTXT sentence is not parsed\r\n");
|
|
#endif
|
|
}
|
|
}
|
|
break;
|
|
case MINMEA_INVALID:
|
|
{
|
|
#if GNSS_DEBUG_PRINTF
|
|
printf("$xxxxx sentence is invalid\r\n");
|
|
#endif
|
|
}
|
|
break;
|
|
default:
|
|
{
|
|
#if GNSS_DEBUG_PRINTF
|
|
printf("$xxxxx sentence is not parsed\r\n");
|
|
#endif
|
|
}
|
|
break;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int gnss_parse(char *str, gnss_data_t *data)
|
|
{
|
|
if (str == NULL || strlen(str) < 6)
|
|
return 0;
|
|
char *head = NULL;
|
|
char *nmea_line[NMEA_ITEMS_MAX];
|
|
int num = 0, line = 0;
|
|
const char s[2] = "\n";
|
|
head = strtok(str, s);
|
|
while (head != NULL)
|
|
{
|
|
nmea_line[line] = head;
|
|
num += strlen(nmea_line[line]);
|
|
head = strtok(NULL, s);
|
|
line++;
|
|
}
|
|
num += line;
|
|
data->items = line;
|
|
data->size = num;
|
|
struct minmea_sentence_gsa frame;
|
|
for (int i = 0; i < line; i++)
|
|
{
|
|
nmea_line_parse(nmea_line[i], data);
|
|
}
|
|
return data->valid;
|
|
}
|