Decodificare Nmea 0183 ecoscandaglio

Ciao e grazie della risposta,
Scopiazzando da esempi in rete ho scritto questo codice che decodifica la stringa e mi restituisce le due variabili che mi servono, funzionare funziona , il problema è la lentezza , teoricamente dovrei ricevere due dati al secondo ma in realtà ne decodifico uno ogni due secondi circa :frowning:

Secondo voi si puo' ottimizzare ?

#include <string.h>
#include <stdio.h>
#include <avr\io.h>
#include <avr\interrupt.h>
#include <Wire.h>
#define DIM 20
#define DIM2 80
#define DIM3 15

struct tokens {
 byte char_index;
 char array[DIM2];
 char *token[DIM3];
}  Sonar_tokens;

float Depth, Temp;

void setup()
{
Serial.begin(4800);

}

void loop() {

 read_Sonar();
  
 }
 
void read_Sonar() {
  
 while (Serial.available()>0) {
   
  if (read_tokens(Serial.read(), &Sonar_tokens)) {
    
   if (!strcmp(Sonar_tokens.token[1],"$SDDPT")) Depth = atof(Sonar_tokens.token[2]); Serial.println((int)(Depth*100));
   
   if (!strcmp(Sonar_tokens.token[1],"$SDMTW")) Temp = atof(Sonar_tokens.token[2]); Serial.println((int)(Temp*100));
 
  }
 }
}

int read_tokens (char character, struct tokens *buffer) {
 char i, *p, *token;
 if (character == '\r') {
  buffer->array[buffer->char_index] = 0;
  buffer->char_index = 0;
  if (!checksum(buffer->array)) return 0;
  p = buffer->array;
  i = 0;
  while (token = strtok_r(p, ",", &p)) buffer->token[++i] = token;
  return i;
 }
 if (character == '\n') {
  buffer->char_index = 0;
  return 0;
 }
 buffer->array[buffer->char_index] = character;
 if (buffer->char_index < DIM2) (buffer->char_index)++;
 return 0;
}

int checksum(char *str) {
 int j, len, xor1, xor2;
 len = strlen(str);
 if (str[0] != '

) return 0;
if (str[len-3] != '') return 0;
xor1 = 16
hex2int(str[len-2]) + hex2int(str[len-1]);
xor2 = 0;
for (j=1;j<len-3;j++) xor2 = xor2 ^ str[j];
if (xor1 != xor2) return 0;
return 1;
}
int hex2int(char a) {
if (a>='A' && a<='F') return 10+(a-'A');
if (a>='a' && a<='f') return 10+(a-'a');
if (a>='0' && a<='9') return a-'0';
return 0;
}