last commit before gitea mirror
This commit is contained in:
@@ -8,53 +8,157 @@
|
||||
#include "cm_nmea_decoder.h"
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#define MAX_NMEA_MSG_TYPES 2
|
||||
const char *nmea_msg_types[MAX_NMEA_MSG_TYPES] = {"$GPGLL", "$GPGFF"};
|
||||
typedef int (*nmeaDecoder_Evt_fpt)(cm_nmea_msg_t *inst, char *str);
|
||||
|
||||
static int findLenToChar(const char *str, const char character){
|
||||
static int findFirstChar(const char *str, const char Ch){
|
||||
|
||||
for(int i = 0; ; i++){
|
||||
|
||||
if(str[i] == Ch) return i;
|
||||
if(str[i] == '\0') return -1;
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
static int isPeriodInLine(char *str){
|
||||
|
||||
int i = 0;
|
||||
|
||||
while((str[i] != character) && (str[i] != '\0')) i++;
|
||||
while((str[i] != '.') && (str[i] != '\0') && (str[i] != ',')) i++;
|
||||
|
||||
if(str[i] == '\0' || str[i] == ',') return -1;
|
||||
|
||||
return i;
|
||||
}
|
||||
|
||||
static int sGPGLL_decode(cm_nmea_msg_t *inst, char *str){
|
||||
static int moveToNextLine(char **str){
|
||||
|
||||
printf("%s \r\n ",str);
|
||||
// Check if moved to next line or past the string.
|
||||
while((**str != ',') && (**str != '\0')){
|
||||
|
||||
//Move pointer until we meet a "," or NULL!
|
||||
while((str != NULL) && (*str != ',')) str++;
|
||||
// Count pointer up because we didnt came across ',' or end
|
||||
(*str)++;
|
||||
|
||||
}
|
||||
|
||||
// Throw error code if we found end and not a ','
|
||||
if(**str == '\0') return -1;
|
||||
|
||||
// Move past ','
|
||||
if (*str == ',') str++;
|
||||
(*str)++;
|
||||
|
||||
// Find comma
|
||||
int comma_idx = findLenToChar(str, '.');
|
||||
return 1;
|
||||
|
||||
// Replace with '\0'
|
||||
str[comma_idx] = '\0';
|
||||
}
|
||||
|
||||
// Decode first part of latitude
|
||||
inst->gpgll_msg.coordinates.lat_deg = atoi(str);
|
||||
static int decodeIntIntoStruct(uint8_t *inst, char *str, nmea_field_type type){
|
||||
|
||||
int isPeriod = isPeriodInLine(str);
|
||||
uint8_t next_val = 0;
|
||||
|
||||
int lineEnd = findFirstChar(str, ',');
|
||||
if (lineEnd < 0) return -1; // error check
|
||||
str[lineEnd] = '\0';
|
||||
|
||||
switch (type) {
|
||||
case NMEA_FIELD_UINT8_T:
|
||||
*((uint8_t*)inst) = (uint8_t)atoi(str);
|
||||
next_val = sizeof(uint8_t);
|
||||
break;
|
||||
case NMEA_FIELD_INT8_T:
|
||||
*((int8_t*)inst) = (int8_t)atoi(str);
|
||||
next_val = sizeof(int8_t);
|
||||
break;
|
||||
case NMEA_FIELD_UINT16_T:
|
||||
*((uint16_t*)inst) = (uint16_t)(isPeriod != -1 ? atoi(str) * 10000 + atoi(str + isPeriod + 1) : atoi(str));
|
||||
next_val = sizeof(uint16_t);
|
||||
break;
|
||||
case NMEA_FIELD_INT16_T:
|
||||
*((int16_t*)inst) = (int16_t)(isPeriod != -1 ? atoi(str) * 10000 + atoi(str + isPeriod + 1) : atoi(str));
|
||||
next_val = sizeof(int16_t);
|
||||
break;
|
||||
case NMEA_FIELD_UINT32_T:
|
||||
*((uint32_t*)inst) = (uint32_t)(isPeriod != -1 ? atoi(str) * 10000 + atoi(str + isPeriod + 1) : atoi(str));
|
||||
next_val = sizeof(uint32_t);
|
||||
break;
|
||||
case NMEA_FIELD_INT32_T:
|
||||
*((int32_t*)inst) = (int32_t)(isPeriod != -1 ? atoi(str) * 10000 + atoi(str + isPeriod + 1) : atoi(str));
|
||||
next_val = sizeof(int32_t);
|
||||
//printf("val: %d \r\n", *((int32_t*)inst));
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
str[lineEnd] = ','; // Restore
|
||||
|
||||
return next_val;
|
||||
}
|
||||
|
||||
|
||||
static int storeValIntoStruct(uint8_t *inst, char *str, nmea_field_type type){
|
||||
|
||||
|
||||
|
||||
if((type >= NMEA_FIELD_UINT8_T) && (type <= NMEA_FIELD_INT32_T)){
|
||||
|
||||
return decodeIntIntoStruct(inst, str, type);
|
||||
}
|
||||
|
||||
if(type == NMEA_FIELD_CHAR_T){
|
||||
|
||||
char *p = (void*)inst;
|
||||
|
||||
*p = *str;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
return 1;
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
static int sNMEA_decode(cm_nmea_msg_t *inst, char *str, int sentence_idx){
|
||||
|
||||
nmea_field_desc_t *p = &nmea_sentences[sentence_idx];
|
||||
|
||||
uint8_t *dst = (void*)inst;
|
||||
|
||||
int next_val = 0;
|
||||
|
||||
moveToNextLine(&str);
|
||||
|
||||
printf("%s \r\n", str);
|
||||
|
||||
for(int i = 0; i < p->field_type_size; i++){
|
||||
|
||||
next_val = storeValIntoStruct(dst, str, p->field_type[i]);
|
||||
|
||||
if(next_val == -1) return next_val;
|
||||
|
||||
dst += next_val;
|
||||
|
||||
|
||||
if (i < p->field_type_size - 1 && moveToNextLine(&str) == -1)
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
printf("lat: %d, lat_dir: %c, lon: %d, lon_dir: %c, utc: %d", inst->msg.gpgll.latitude, inst->msg.gpgll.lat_dir, inst->msg.gpgll.longitude, inst->msg.gpgll.lon_dir, inst->msg.gpgll.utc_time);
|
||||
|
||||
printf("deg: %d, str: %s \r\n ",comma_idx,str);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
nmeaDecoder_Evt_fpt nmea_decodeFuncs[1] = {
|
||||
(void *)&sGPGLL_decode,
|
||||
};
|
||||
|
||||
static int sNMEA_getMsgType(char *str){
|
||||
|
||||
for(int i = 0; i < (MAX_NMEA_MSG_TYPES-1); i++){
|
||||
for(int i = 0; i < (sizeof(nmea_sentences) / sizeof(nmea_sentences[0])); i++){
|
||||
|
||||
if(strcmp(nmea_msg_types[i], str)) return i;
|
||||
if(strncmp(nmea_sentences[i].nmea_sentence, str, 6) == 0) return nmea_sentences[i].nmea_sentence_type;
|
||||
};
|
||||
|
||||
return -1;
|
||||
@@ -75,8 +179,9 @@ int cm_nmea_stringDecode(cm_nmea_msg_t *inst, char *str){
|
||||
// Check if we found a valid NMEA message.
|
||||
if(nmea_msg_idx == -1) return nmea_msg_idx;
|
||||
|
||||
// Use the correct function to decode!
|
||||
if(nmea_msg_idx >= 0) nmea_decodeFuncs[nmea_msg_idx](inst,str);
|
||||
// Decode message:
|
||||
sNMEA_decode(inst,str,nmea_msg_idx);
|
||||
|
||||
|
||||
return nmea_msg_idx;
|
||||
|
||||
|
||||
Reference in New Issue
Block a user