Replies to This Discussion

3D RoboticsPermalink Reply by Chris Anderson on October 27, 2008 at 5:25am

Koen,

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?

• ▶ Reply

Permalink Reply by Koen Delvaux on October 27, 2008 at 6:26am

I have no idea about AVR architecture, but if you can hook up an IRQ on each character received in the uart, the code should work.
The "tricks" used are:
- minimise the number of instructions on each hit of the irq handler
- don't parse at the end, put parse as you go along
- only do the minimum on syntax checking, 'if' instructions are always slow but not always needed
- replace calculations with copy operations wherever possible, since they are a lot faster

My guestimate is that this will use about 1% CPU at 30MIPS for 2 NMEA sentences (GGA & RMC) at 5Hz update.

• ▶ Reply

Permalink Reply by Giles Barton-Owen on October 27, 2008 at 6:53am

I wrote one for the Arduino and it works...



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)?

• ▶ Reply

3D RoboticsPermalink Reply by Chris Anderson on October 27, 2008 at 7:09am

Giles,

Thanks for the code (and no, we don't have a dedicated code display widget here yet, although now that Ning supports Open Social I hope I can find a code snippet display widget).

Your code, and our earlier NMEA code for Arduino, uses the standard serial in buffer. The problem with this approach, and why we swtiched to SiRF binary, is that it uses a lot of CPU overhead and becomes a problem at 5HZ. What Koen did on the PIC was to go straight to the UART to skip the serial handling slowdown. I'm not sure if that will work on an Atmel.

Our solution for 5Hz was to use the Ublox chipset, which also has a binary mode.

• ▶ Reply

Permalink Reply by Giles Barton-Owen on October 27, 2008 at 7:13am

where is the code for binary parsing and also how is it different?

• ▶ Reply

3D RoboticsPermalink Reply by Chris Anderson on October 27, 2008 at 7:48am

The code is here. We grab the binary data right out of the serial buffer without the need for an ASCII parser.

• ▶ Reply

Permalink Reply by Koen Delvaux on October 27, 2008 at 9:05am

I looked at Ardupilot.cpp briefly and what I noticed:
- you're polling the serial port to see if characters come in
- you hook up things with delay routines to "wait" for characters

You really need to fetch the character under interrupt, or alternatively take up an instruction like "if (Serial.available() > 0) process_incoming_character();" in your main program loop. (with process_incoming_character being the function I posted)
But I heard from a colleague who likes to thinker with arduino that it supports serial interrupts, so you should be fine using that.

• ▶ Reply

Permalink Reply by Koen Delvaux on October 27, 2008 at 9:32am

I read a bit more of your code. At the danger of being a wiseass, I'm just gonna give you my impression.

I have to warn you that I'm no experienced embedded programmer, since this is only my second project. (the first one controls the curtains in my house, and is still working flawlessly after 8 years:)

I think I understand your approach, since this is also the way of programming I used when I made the curtain controller. But when I started on the autopilot, I realised I had to learn a few more things. And once I got up to par on the basics of interrupt and i/o processing, everything became very easy.

What you are trying to do:
- "I need some GPS data" -> ok let's wait for that
- "now I need to know if the switch is flipped on" -> ok, let's wait for the RC signals
- etc.

What you usually do in an embedded system:
- "Attention: we received serial data" -> ok, let's process that
- "Attention: we received an RC pulse" -> ok let's process that
- main loop: do our thing and keep doing it as fast as we can

The "attention" things are what people call IRQ's, it's like an event coming in and telling you to do something. So your basic approach could be something like:

start:
calculate where we are
calculate the direction we need to take
if (rc_input == autopilot_on)
rc_outputs = calculate what the rc outputs should be
else
rc_outputs = rc_inputs
go back to start

In this loop, you *never* insert wait-statements, you *never*¨do any IO. You just read the variables that hold the gps data, waypoint data and rc inputs. And you write to the variables that you use to store the rc outputs. That's it.

Now, how do you get external input? Simple: you set up an interrupt to trigger when something happens at the RC input. When it happens, you deal with it (e.g. update a variable that tells your main loop what RC pulses you received) and then quickly exit the interrupt. Same thing for the serial interrupt.
Now how do you create output pulses? You set up a cyclic timer. Each time the timer expires, you know you have to switch an output high or low and re-initialize the timer so it will expire again and hit the timer interrupt handler again. Of course you quickly calculate when you want it to expire, since this is how you control pulse lenght. To know what pulse lenght you need, you just check the variable your main loop uses to store the RC outputs it want to go to the servos.

In summary:
do all IO and timing in dedicated interrupt routines
put your code for navigation, calculating and blinking lights etc. in a continious main loop
interface between the two by means of variables that are read by the main loop and written by the irq (or vice versa)

I hope this helps some people who start in embedded programming, I know it took me a bit of time before I understood this.

• ▶ Reply

3D RoboticsPermalink Reply by Chris Anderson on October 27, 2008 at 9:41am

I think you're referring to Jordi, who wrote that code? If so, I think he was indeed planning to implement interrupt-driven serial, and may have already done something like what you recommend. But this looks like excellent help, so I'll make sure he sees it.

Many thanks!

• ▶ Reply

Permalink Reply by Ho-Chung Chang on May 11, 2009 at 9:39am

Thanks sharing this code. I have used dsPIC for my project. your code is good reference design for me.

• ▶ Reply