gnss: update gnss code
Change-Id: Id906b93eb619bcbebc11469338647a3fdc47076f
diff --git a/mbtk/libmbtk_gnss/mbtk_gnss.c b/mbtk/libmbtk_gnss/mbtk_gnss.c
index 11bbfd3..3e22cf7 100755
--- a/mbtk/libmbtk_gnss/mbtk_gnss.c
+++ b/mbtk/libmbtk_gnss/mbtk_gnss.c
@@ -23,6 +23,7 @@
#include <netinet/in.h>
#include <fcntl.h>
#include <sys/epoll.h>
+#include <time.h>
#include "mbtk_gnss_inter.h"
#include "mbtk_log.h"
@@ -39,6 +40,12 @@
static pthread_mutex_t gnss_mutex;
static gnss_err_enum gnss_result;
+#if 1//MBTK_GNSS_LOCATION_INFO
+static mbtk_gnss_location_info_t locl_info;
+
+extern long timezone;
+#endif
+
static int sock_read(int fd, uint8 *msg, int data_len)
{
memset(msg, 0, data_len);
@@ -127,11 +134,375 @@
return;
}
+#if 1//MBTK_GNSS_LOCATION_INFO
+static int strstr_n(const char *s1, const char *s2)
+{
+ int n;
+ int strlen = 0;
+
+ if(*s2)
+ {
+ while(*s1)
+ {
+ for(n = 0; *(s1+n) == *(s2 + n); n++)
+ {
+ if(!*(s2 + n + 1))
+ {
+ strlen++;
+ return strlen;
+ }
+ }
+ s1++;
+ strlen++;
+ }
+ return 0;
+ }
+
+ return 0;
+}
+
+static int str2int( const char* head, const char* end )
+{
+ int result = 0;
+ int len = end - head;
+
+ for(; len > 0; len--, head++)
+ {
+ int c;
+ if (head >= end)
+ {
+ goto Fail;
+ }
+
+ c = *head - '0';
+ if ((unsigned)c >= 10)
+ {
+ goto Fail;
+ }
+ result = result * 10 + c;
+ }
+ return result;
+Fail:
+ return -1;
+}
+
+static double str2float( const char* head, const char* end )
+{
+ int len = end - head;
+ char temp[16];
+
+ if(len >= (int)sizeof(temp))
+ {
+ return 0;
+ }
+
+ memcpy( temp, head, len );
+ temp[len] = 0;
+ return strtod(temp, NULL);
+}
+
+static mbtk_token nmea_tokenizer_get(mbtk_nmeatokenizer* t, int index)
+{
+ mbtk_token tok;
+ static const char* dummy = "";
+
+ if (index < 0 || index >= t->count)
+ {
+ tok.head = tok.end = dummy;
+ }
+ else
+ {
+ tok = t->tokens[index];
+ }
+ return tok;
+}
+
+static int nmea_tokenizer_init(mbtk_nmeatokenizer* t, const char* head, const char* end, int param_num)
+{
+ int count = 0;
+ const char* p = head;
+ const char* q = end;
+ const char* tmp = NULL;
+ // the initial '$' is optional
+ if (p < q && p[0] == '$')
+ {
+ p += 1;
+ }
+ else
+ {
+ return -1;
+ }
+
+ //find '*',del '*25\r\n'
+ // get rid of checksum at the end of the sentecne
+ if (q >= p + 5 && q[-5] == '*')
+ {
+ q -= 5;
+ }
+ else
+ {
+ return -1;
+ }
+
+ while (p <= q)
+ {
+ tmp = memchr(p, ',', q-p);
+ if (tmp == NULL)
+ {
+ tmp = q;
+ }
+ // if (q > p) {
+ // q >= p include empty token: ,,
+ if (tmp >= p)
+ {
+ if (count < MAX_NMEA_TOKENS)
+ {
+ t->tokens[count].head = p;
+ t->tokens[count].end = tmp;
+ count += 1;
+ }
+ }
+
+ if (tmp <= q)
+ {
+ tmp += 1;
+ }
+
+ p = tmp;
+ }
+
+ if(count != param_num)
+ {
+ LOGD("count [%d], param_num [%d]", count, param_num);
+ return -1;
+ }
+
+ t->count = count;
+ return count;
+}
+
+static int nmea_update_date_time(mbtk_gnss_location_info_t* locl_info, mbtk_token date, mbtk_token time)
+{
+ char tmp_char[4] = {0};
+ struct tm tmp_time;
+
+ memset(&tmp_time, 0x0, sizeof(struct tm));
+ if (date.head + 6 > date.end)
+ {
+ LOGD("date get fail");
+ return -1;
+ }
+
+ memcpy(tmp_char, date.head, 2);
+ tmp_time.tm_mday = atoi(tmp_char);
+ memcpy(tmp_char, date.head + 2, 2);
+ tmp_time.tm_mon = atoi(tmp_char) - 1;
+ memcpy(tmp_char, date.head + 4, 2);
+ tmp_time.tm_year = 100 + atoi(tmp_char);
+
+ if (time.head + 6 > time.end)
+ {
+ LOGD("time get fail");
+ return -1;
+ }
+
+ memcpy(tmp_char, time.head, 2);
+ tmp_time.tm_hour = atoi(tmp_char);
+ memcpy(tmp_char, time.head + 2, 2);
+ tmp_time.tm_min = atoi(tmp_char);
+ memcpy(tmp_char, time.head + 4, 2);
+ tmp_time.tm_sec = atoi(tmp_char);
+ tmp_time.tm_isdst = -1;
+
+#if MBTK_GNSS_LOG_ENABLED
+ LOGD("data:%d-%d-%d %d:%d:%d", tmp_time.tm_year + 1900,
+ tmp_time.tm_mon,
+ tmp_time.tm_mday,
+ tmp_time.tm_hour,
+ tmp_time.tm_min,
+ tmp_time.tm_sec);
+#endif
+
+ time_t _t = mktime(&tmp_time);//parse location tmp_time
+#if MBTK_GNSS_LOG_ENABLED
+ LOGD("time: %ld", _t);
+#endif
+ tzset(); // auto set tz
+ _t = _t - timezone;
+ locl_info->timestamp = (int64_t)_t;
+#if MBTK_GNSS_LOG_ENABLED
+ LOGD("timestamp: %ld, %ld", locl_info->timestamp, timezone);
+#endif
+ return 0;
+}
+
+static int nmea_update_bearing(mbtk_gnss_location_info_t* locl_info, mbtk_token bearing)
+{
+ mbtk_token tok = bearing;
+
+ if (tok.head >= tok.end)
+ {
+ LOGD("bearing get fail");
+ return -1;
+ }
+
+ locl_info->bearing = str2float(tok.head, tok.end);
+ return 0;
+}
+
+static int nmea_update_speed(mbtk_gnss_location_info_t* locl_info, mbtk_token speed)
+{
+ mbtk_token tok = speed;
+
+ if (tok.head >= tok.end)
+ {
+ LOGD("speed get fail");
+ return -1;
+ }
+
+ locl_info->speed = str2float(tok.head, tok.end);
+ return 0;
+}
+
+static int nmea_update_latlong(mbtk_gnss_location_info_t* locl_info, mbtk_token latitude, mbtk_token longitude)
+{
+ double lat, lon;
+ mbtk_token tok;
+ tok = latitude;
+ if (tok.head + 6 > tok.end)
+ {
+ LOGD("latitude get fail");
+ return -1;
+ }
+ locl_info->latitude = str2float(tok.head, tok.end);
+
+ tok = longitude;
+ if (tok.head + 6 > tok.end)
+ {
+ LOGD("longitude get fail");
+ return -1;
+ }
+ locl_info->longitude = str2float(tok.head, tok.end);
+ return 0;
+}
+
+static int nmea_update_altitude(mbtk_gnss_location_info_t* locl_info, mbtk_token altitude)
+{
+ mbtk_token tok = altitude;
+
+ if (tok.head >= tok.end)
+ {
+ LOGD("altitude get fail");
+ return -1;
+ }
+
+ locl_info->altitude = str2float(tok.head, tok.end);
+ return 0;
+}
+
+static int ind_nmea_parse(const char *data, int data_len, mbtk_gnss_location_info_t* locl_info)
+{
+ int ret;
+ mbtk_nmeatokenizer tzer = {0};
+ if(strstr_n(data + 3, "RMC"))
+ {
+#if MBTK_GNSS_LOG_ENABLED
+ LOGD("ind data_len: [%d] data: %s", data_len, data);
+#endif
+ ret = nmea_tokenizer_init(&tzer, data, data + data_len, NMEA_RMC_PARAM_NUM);
+ if(ret < 0)
+ {
+ LOGD("nmea_tokenizer_init fail");
+ return -1;
+ }
+
+ mbtk_token tok_time = nmea_tokenizer_get(&tzer,1);
+ mbtk_token tok_fixStatus = nmea_tokenizer_get(&tzer,2);
+ mbtk_token tok_speed = nmea_tokenizer_get(&tzer,7);
+ mbtk_token tok_bearing = nmea_tokenizer_get(&tzer,8);
+ mbtk_token tok_date = nmea_tokenizer_get(&tzer,9);
+
+ if(tok_fixStatus.head[0] == 'A')
+ {
+ ret = nmea_update_date_time(locl_info, tok_date, tok_time);
+ if(ret < 0)
+ {
+ LOGD("nmea_update_date_time fail");
+ return -1;
+ }
+ locl_info->flags |= GNSS_LOCATION_HAS_TIMESTAMP;
+
+ ret = nmea_update_bearing(locl_info, tok_bearing);
+ if(ret < 0)
+ {
+ LOGD("nmea_update_bearing fail");
+ return -1;
+ }
+ locl_info->flags |= GNSS_LOCATION_HAS_BEARING;
+
+ ret = nmea_update_speed(locl_info, tok_speed);
+ if(ret < 0)
+ {
+ LOGD("nmea_update_speed fail");
+ return -1;
+ }
+ locl_info->flags |= GNSS_LOCATION_HAS_SPEED;
+ }
+ }
+ else if(strstr_n(data + 3, "GGA"))
+ {
+#if MBTK_GNSS_LOG_ENABLED
+ LOGD("ind data_len: [%d] data: %s", data_len, data);
+#endif
+ ret = nmea_tokenizer_init(&tzer, data, data + data_len, NMEA_GGA_PARAM_NUM);
+ if(ret < 0)
+ {
+ LOGD("nmea_tokenizer_init fail");
+ return -1;
+ }
+
+ mbtk_token tok_latitude = nmea_tokenizer_get(&tzer,2);
+ mbtk_token tok_longitude = nmea_tokenizer_get(&tzer,4);
+ mbtk_token tok_isPix = nmea_tokenizer_get(&tzer,6);
+ mbtk_token tok_altitude = nmea_tokenizer_get(&tzer,9);
+
+ if(tok_isPix.head[0] > '0' && tok_isPix.head[0] < '9')
+ {
+ ret = nmea_update_latlong(locl_info, tok_latitude, tok_longitude);
+ if(ret < 0)
+ {
+ LOGD("nmea_update_latlong fail");
+ return -1;
+ }
+ locl_info->flags |= GNSS_LOCATION_HAS_LAT_LONG;
+
+ ret = nmea_update_altitude(locl_info, tok_altitude);
+ if(ret < 0)
+ {
+ LOGD("nmea_update_altitude fail");
+ return -1;
+ }
+ locl_info->flags |= GNSS_LOCATION_HAS_ALTITUDE;
+ }
+ }
+ else
+ {
+#if MBTK_GNSS_LOG_ENABLED
+ LOGD("ind data_len: [%d] data: %s", data_len, data);
+#endif
+ }
+ return 0;
+}
+#endif
+
+
static void gnss_rsp_process(const char *data, int data_len)
{
int index = 0;
char buff[GNSS_BUFF_SIZE];
int buff_len = 0;
+ int ret = 0;
+ int tag_len = 0;
while(index < data_len) {
if(data[index] == MBTK_IND_START_FLAG) {
memset(buff, 0, sizeof(buff));
@@ -140,11 +511,22 @@
buff[buff_len] = '\0';
if(memcmp(MBTK_IND_LOCATION_TAG, buff, strlen(MBTK_IND_LOCATION_TAG)) == 0) {
if(gnss_cb) {
- gnss_cb(MBTK_GNSS_IND_LOCATION, buff, buff_len);
+#if 1//MBTK_GNSS_LOCATION_INFO
+ tag_len = strlen(MBTK_IND_LOCATION_TAG);
+ ret = ind_nmea_parse(buff + tag_len, buff_len - tag_len, &locl_info);
+#if MBTK_GNSS_LOG_ENABLED
+ LOGD("gnss_cb: [%p], locl_info.flags [%d]", gnss_cb, locl_info.flags);
+#endif
+ if(locl_info.flags == GNSS_LOCATION_HAS_ALL) {
+ gnss_cb(MBTK_GNSS_IND_LOCATION, &locl_info, sizeof(mbtk_gnss_location_info_t));
+ locl_info.flags = 0;
+ }
+#endif
}
} else if(memcmp(MBTK_IND_NMEA_TAG, buff, strlen(MBTK_IND_NMEA_TAG)) == 0) {
if(gnss_cb) {
- gnss_cb(MBTK_GNSS_IND_NMEA, buff, buff_len);
+ tag_len = strlen(MBTK_IND_NMEA_TAG);
+ gnss_cb(MBTK_GNSS_IND_NMEA, buff + tag_len, buff_len - tag_len);
}
} else {
LOGD("RSP[len - %d] : %s", buff_len, buff);