Posted by Koen Delvaux on October 27, 2008 at 5:15am
I wanted to share this NMEA parser code, according to the DIYDrones "should I write a blog post" guide it is best to open a discussion topic about this. You will find the code in attachment.I have no idea if this is anywhere near efficient, but it works. The code should be self-explanatory and is designed to run on a dsPIC. I deliberately avoided unnecessary programming constructs (functions, loops, local scoped variables) to save on memory, processing time and code readability.
Replies
char serialBuffer[256] = "";
byte serialBufferMarker = 0;
float roll = 128 , pitch = 128 , yaw = 128 , hover = 0, function = 0;
char Identifier[] = {
'$','R','M','C'};
int count = 0;
boolean goodMessage = false;
char message[64] = "";
byte commaMarkers[32];
void setup()
{
Serial.begin(9600);
}
void loop()
{
int oldMarker = serialBufferMarker;
if(Serial.available() > 0)
{
for(int i = 0; i < Serial.available(); i++)
{
serialBuffer[serialBufferMarker] = Serial.read();
//Serial.print(serialBuffer[serialBufferMarker]);
if(serialBuffer[serialBufferMarker] == 13)
{
bufferTransfer(serialBufferMarker);
}
else
{
serialBufferMarker ++;
}
}
}
if(goodMessage == true)
{
digitalWrite(13,HIGH);
delay(100);
}
else
{
digitalWrite(13,LOW);
}
}
void parseMessage()
{
byte goodCount = 0;
Serial.print("%");
byte returnMarker;
byte dollarMarker;
//Find commas
int commaCount = 0;
for(int i = 0; i < 64; i++)
{
if(message[i] == 44)
{
commaMarkers[commaCount] = i;
commaCount++;
}
}
hover = 0;
yaw = 0;
roll = 0;
pitch = 0;
function = 0;
Serial.print(',');
Serial.print(commaMarkers[1],DEC);
Serial.print('@');
Serial.print(commaCount);
Serial.print(',');
/////****\\\\\
for(int i = 0; i < 6; i++)
{
if(message[i] == Identifier[i])
{
goodCount++;
}
}
if(goodCount >= 6)
{
Serial.print("<");
for(int i = 1; i <= commaCount; i++)
{
Serial.print("in for loop");
int j = 1;
while(isNum(message[commaMarkers[i] - j]) == true || message[commaMarkers[i] - j] == 46)
{
Serial.print('!');
if(isNum(message[commaMarkers[i] - j]) == true)
{
switch(i)
{
case 1:
hover = hover + (((message[commaMarkers[i] - j]) - 48) * (powerTen(j)));
break;
case 2:
yaw = yaw + (((message[commaMarkers[i] - j]) - 48) * (powerTen(j)));
;
break;
case 3:
roll = roll + (((message[commaMarkers[i] - j]) - 48) * (powerTen(j)));
;
break;
case 4:
pitch = pitch + (((message[commaMarkers[i] - j]) - 48) * (powerTen(j)));
;
break;
case 5:
function = function + (((message[commaMarkers[i] - j]) - 48) * (powerTen(j)));
;
break;
default:
break;
}
}
else
{
switch(i)
{
case 1:
hover = hover / (powerTen(j));
break;
case 2:
yaw = yaw / (powerTen(j));
;
break;
case 3:
roll = roll / (powerTen(j));
;
break;
case 4:
pitch = pitch / (powerTen(j));
;
break;
case 5:
function = function / (powerTen(j));
;
break;
default:
break;
}
}
j++;
}
if(message[commaMarkers[i] - j] == 45)
{
switch(i)
{
case 1:
hover = hover * -1;
break;
case 2:
yaw = yaw * -1;
break;
case 3:
roll = roll * -1;
break;
case 4:
pitch = pitch * -1;
break;
case 5:
function = function * -1;
break;
default:
break;
}
}
}
return;
}
}
boolean isNum(int num)
{
if(num > 47 && num < 58)
{
return true;
}
else
{
return false;
}
}
boolean bufferTransfer(byte returnMarker)
{
Serial.print('>');
byte dollarMarker;
byte i = returnMarker;
boolean stop1 = false;
while(serialBuffer[i] != 36 && i > 0)
{
i--;
}
if(serialBuffer[i] == 36)
{
dollarMarker = i;
}
else
{
return false;
}
Serial.print(returnMarker - dollarMarker);
for(byte j = 0; j < returnMarker - dollarMarker; j ++)
{
message[j] = serialBuffer[dollarMarker + j];
Serial.print(message[j]);
}
serialBufferMarker = 0;
parseMessage();
}
int powerTen(int power)
{
int num = 1;
int base = 10;
for(int i = 1; i < power; i++)
{
num = num * base;
}
return num;
}
It can haddle negatives and (untested) decimals.
I wrote it for and XBEE no RX-TX package type comms system.
BTW is there a way of putting this in a scrollable panel (like when you are writing it)?
Excellent---many thanks! Clever going straight to the Uart like that. We don't use PICs so I can't test it. Do you think it would work for AVRs, too?