static void device_uart_raw_send_data(void* const _uart_raw,
const unsigned int id, const timestamp_t* const timestamp) {
device_uart_raw_t* uart_raw = (device_uart_raw_t*) _uart_raw;
unsigned char byte;
if (uart_light_receive_nb(uart_raw->uart_light, &byte) == UART_OK) {
data_package_t package = { id, "byte", DATA_TYPE_BYTE, &byte, timestamp };
uart_raw->data_out->new_data(uart_raw->data_out->parent, &package);
}
}
static void device_gps_nmea_send_data(void* const _gps_nmea,
const unsigned int id, const timestamp_t* const timestamp) {
device_gps_nmea_t* gps_nmea = (device_gps_nmea_t*) _gps_nmea;
uart_light_disable_rxint(gps_nmea->uart_light);
unsigned char byte;
while (uart_light_receive_nb(gps_nmea->uart_light, &byte) == UART_OK) {
if (byte > 127) { //filter out non ASCII characters (should not be there, but who knows..)
gps_nmea->parse_status = 0;
return;
}
if (byte == '$') { //found start of NMEA sentence
gps_nmea->parse_status = 1;
}
if (gps_nmea->parse_status != 0) {
data_package_t package = { id, "byte", DATA_TYPE_BYTE, &byte, timestamp };
gps_nmea->raw_data_out->new_data(gps_nmea->raw_data_out->parent,
&package);
}
if ((gps_nmea->parsed_data_out != &data_port_dummy)
|| (gps_nmea->sync_logger_time == GPS_SYNC_TIME)) { //parse only if data out assigned or sync activated
if (byte == '$') { //start of a NMEA sentence
device_gps_nmea_reset_parse_data(gps_nmea);
} else if (gps_nmea->parse_status != 0) { //valid char
if ((gps_nmea->parse_status < 3) || (gps_nmea->parse_status == 6)) { //every sentence starts with "GP"
gps_nmea->parse_status++;
} else if (gps_nmea->parse_status < 6) { //next three chars are the sentence type
gps_nmea->cur_sentence[gps_nmea->parse_status - 3] = byte;
gps_nmea->parse_status++;
} else if (byte == ',' || byte == '*' || byte == '\r') { //end of value markers
gps_nmea->parse_status++;
if (gps_nmea->parsed_data_out != &data_port_dummy) {
gps_nmea->parse_package.timestamp = timestamp;
device_gps_nmea_set_val_type(gps_nmea);
if (gps_nmea->parse_package.type == DATA_TYPE_SIMPLE_FLOAT) {
unsigned char i;
for (i = 0; i < (4 - gps_nmea->parsed_digits); i++) {
gps_nmea->parse_uint *= 10;
}
gps_nmea->parse_float.coefficient += cast_to_ulong(
gps_nmea->parse_uint);
}
if ((gps_nmea->parse_package.type != DATA_TYPE_INT)
|| gps_nmea->parsed_digits) {
if (gps_nmea->parse_package.val_name != nmea_unknown)
gps_nmea->parsed_data_out->new_data(
gps_nmea->parsed_data_out->parent,
&gps_nmea->parse_package);
}
device_gps_nmea_reset_parse_data(gps_nmea);
}
if (byte == '*') {
gps_nmea->parse_status = 1;
}
} else {
char digit = byte - '0';
if (gps_nmea->parsed_data_out != &data_port_dummy) {
if (digit >= 0 && digit <= 9) { //normal digit
if(gps_nmea->parsed_digits < 4 && gps_nmea->parse_package.type == DATA_TYPE_SIMPLE_FLOAT //Check if there are only 4 decimal places or truncate them to 4
|| gps_nmea->parse_package.type == DATA_TYPE_INT) {
gps_nmea->parse_uint *= 10;
gps_nmea->parse_uint += digit;
gps_nmea->parsed_digits++;
}
} else {
gps_nmea->parsed_digits = 0;
if (byte == '.') { //fixed point value -> treat as float with exponent 0
gps_nmea->parse_float.coefficient = mul34_17(
cast_to_ulong(gps_nmea->parse_uint), 10000);
gps_nmea->parse_uint = 0;
gps_nmea->parse_package.type = DATA_TYPE_SIMPLE_FLOAT;
gps_nmea->parse_package.data = &gps_nmea->parse_float;
} else if (byte != ' ') { //single character
gps_nmea->parse_package.type = DATA_TYPE_CHAR;
gps_nmea->parse_byte = byte;
gps_nmea->parse_package.data = &gps_nmea->parse_byte;
}
}
}
if (gps_nmea->sync_logger_time == GPS_SYNC_TIME) {
if (gps_nmea->parse_status == 7) { //first data field in a sentence
if ((gps_nmea->cur_sentence[0] == 'G')
& (gps_nmea->cur_sentence[1] == 'G')
& (gps_nmea->cur_sentence[2] == 'A')) {
if (digit >= 0 && digit <= 9) {
if (gps_nmea->time_parse_position == 0) {
gps_nmea->time_parsed = 0;
}
gps_nmea->time_parsed += digit
* time_multiplicators[gps_nmea->time_parse_position];
if (gps_nmea->time_parse_position == 5) { //seconds finished
gps_nmea->time_parsed_sec = gps_nmea->time_parsed;
gps_nmea->time_parsed = 0;
}
if (gps_nmea->time_parse_position == 7) {
timestamp_gen_regs_t* timestamp_gen = get_timestamp_gen();
timestamp_gen->timestamp.lpt_union.lpt = gps_nmea->time_parsed_sec;
unsigned long int lpt = mul34_17(get_peri_clock(),
gps_nmea->time_parsed);
lpt /= 10;
timestamp_gen->timestamp.hpt_union.hpt = lpt;
}
}
gps_nmea->time_parse_position++;
}
} else {
gps_nmea->time_parse_position = 0;
}
}
}
}
}
}
uart_light_enable_rxint(gps_nmea->uart_light);
}