For modern microprocessors with 8 byte IEEE floating point numbers, the spherical law of cosines can be used to compute distance between 2 latitude longitude points.  The law of cosines can be expressed in Excel as:

=ACOS((COS(LAT1)*COS(LAT2)*COS((-1*LON2)-(-1*CLON2)))+(SIN(LAT1)*SIN(LAT2)))*r

Whatever units you use for r, the radius of the earth,  will be the units of the returned value.  So, for example, if you want the returned distance in miles, you would use the value 3,959, the mean radius of the earth in miles.  You can also use 6,371 for kilometers or 20,903,520 for feet.  For Deathpod3000, since we are only dealing with small distances and need smallish scale, I used feet.   To play with in Excel, miles is nice because you can run some simple and easy sanity checks.

All of the latitude and longitude values in the formula must be in radians.  It is easy to convert degrees to radians with: =Degrees*PI()/180.  Substitute any cell for Degrees and you’ll get back radians.  Similarly, you can convert back from radians to degrees with:  =Radians*180/PI().  Substitute any cell for Radians to convert to degrees.

I created columns in my spreadsheet for Degrees and Radians since I’m used to dealing with latitude and longitude in degrees, but the formulas need radians.  I also created rows for each of the latitude and longitude values and a cell for the value r so I could easily change from miles to feet and back.  The result looks like this:

Latitude Longitude Degrees Radians Convert

You can see one of the radians to degrees conversion formulas just above the spreadsheet.

OK, now we’re ready to do the distance calculation.  As I said before, 1 degree of latitude is 60 nautical miles or roughly 69 statute miles.  Let’s change lat2 to be 1 degree less than lat1 and make lon1 and lon2 the same.  This should be 2 points directly north/south of each other and about 69 miles apart.  Now, let’s put in the law of cosines distance formula.  You can see the formula just above the sheet.

Law of Cosines Distance

69 miles!  Just what we were expecting.  So that works great for Excel, javascript etc.  For 8 bit microcontrollers with 4 byte floats, however, the formula breaks down for small distances.  The precision of 4 byte floats causes too much error.  There is a slightly more complex formula that does work well at small distances called the haversine formula.  It takes a bit more work, but we should get the same distance we got for the spherical law of cosines.  Here is the haversine formula:

r is the same radius of the earth.
dLat = lat2 – lat1
dLon = lon2 = lon1
a =  (sin(dlat/2))^2+cos(lat1)*cos(lat2)*(sin(dlon/2))^2
c=2*(atan2(sqrt(1-a), sqrt(a))  (Note that parameters are reversed from the “normal” atan2 function)
distance = r * c

So, let’s add each of these to the spreadsheet and see if we get the same distance value we got before.

Excel Haversine Formula Distance

Bingo!  The distance reported by the spherical law of cosines formula and the haversine formula are the same.  If you’re just using excel or writing in C# on a modern computer, you can use the spherical law of cosines formula and be done.  If you’re using something like an atmega8, atmega32, pic or similar, it takes a bit more work, but the haversine formula will be more accurate for the short distances you’ll probably be navigating so go with that.

Ok, so now we know how far away from our waypoint we are, but we don’t yet know what direction to head.  The formula for determining bearing to a waypoint is:

=MOD(ATAN2((COS(lat1)*SIN(lat2))-(SIN(lat1)*COS(lat1)*COS(lon2-lon1)), SIN(lon2-lon1)*COS(lat2)),2*PI())

First, remember that Excel has the atan2 parameters reversed so this is the Excel formula, but the atmega32 C code I’ll show in the next post will have the parameters the other way around.  I don’t know why the Excel atan2 function is different from the way everyone else implemented the function, but we have to live with it.  Second, notice the MOD() function with 2*pi().  The reason for this is we want our heading to be between 0-360 degrees or between 0 and 2 pi radians.  What this does is divide the result by 2 pi (360 degrees) and give us back the remainder.  If the result of the atan2 function is less than 2 pi it simply returns the number.  If it is greater, it divides and gives the remainder, which will always then be between 0 and 2 pi (0-360 degrees).   Here is a simple example using degrees.  Let’s say the atan2 function returns 380 degrees.  Dividing by 360 degrees gives a value of 1 with a remainder of 20.  The mod function discards the 1 and return 20.  380 degrees on a compass is the same as 20 degrees (20 degrees past 360).  So, the mod fuction wrapped around the atan2 function makes sure that the bearing is always between 0-360 degrees.

Now, all the trig functions in Excel use radians and atan2 is no exception so the formula above gives us the bearing in radians.  To convert to degrees we use the same formula shown above for lat/long:

Radians*180/PI()

So, let’s add this to our spreadsheet.  Now, point 2, our waypoint, has the same longitude as point 1 so these 2 points are directly north/south of each other.  Latitudes start at the equator and grow larger the farther north or south from the equator you go.  In this case, the latitude is near Denver, Colorado in the northern hemisphere, so larger values mean farther north and smaller values mean farther south.  Point 2, the waypoint, is 1 degree of latitude less than point 1, current position.  This means that the waypoint should be due south of current location at a distance of 69 miles.  Let’s look at the spreadsheet and see what we get.

Bearing in Excel

The bearing in radians was pi, which when converted to degrees is 180 degrees.  Perfect.  We can try some other simple tests to make sure it works in other cases as well.  Due east would have latitude the same, but smaller longitude.  Let’s try that.

Due East

Rounding the result gives 90 degrees, what we expected.  You can try some other tests, for example put in the lat/lon of Los Angeles and New York City and see if the bearing makes sense.

You can download the Excel spreadsheet I used in the examples here:  DistanceAndBearingToWaypoint.xls

Deathpod3000 uses an Atmel AVR Atmega32 and is programmed with the free gcc C compiler.  The code is very similar, but there are some differences, like swapping parameter order for atan2.  I also created a relative bearing function to compute a -180 to 180 degree “steering angle” using current heading and bearing to waypoint.  This allows fairly easily proportional steering.  In the next post I’ll show and explain the code for doing all of this in C.

Deathpod3000 taking off at the start line

RAD and Deathpod3000 (at 2:30 remaining)  first round runs at Sparkfun Autonomous Vehicle Competition

Deathpod3000 Top View

There were some requests on the Sparkfun site for the specs of the vehicles so I thought I’d list them.  All the info is in the build posts, but the design changed along the way so here is what we ended up with: (Click More…)

• Duratrax Evader EXT electric r/c car
• Atmel AVR Atmega32 running at 8mhz
• 40 pin AVR development board
• 16X2 character LCD
• Locosys 20031 3.3v 5hz GPS (running at 2 only 2hz)
• 5v to 3.3v Logic Level Converter
• Honeywell HMC6352 i2c compass
• Atmel AVR Atmega8
• Sharp GP2Y0A710YK0F infrared distance sensor
• Breadboard
• Plywood and hardware

I only added the Atmega8 because I used all the a2d pins of the Atmega32 to connect the LCD and then, at the last minute, added the ir distance sensors that needed a2d pins.  I also used the breadboard because I added the Atmega8 for the ir sensors so late.

Deathpod3000 Front View

I used the free Atmel AVR Studio IDE  with the open source gcc WinAVR C compiler for development and an STK500 for prototyping and AVR programming.  I used Pascal Stang’s free AVRLib for much of the low-level interfacing such as uart, buffers, nmea parsing, i2c and a2d.  There’s a lot more in AVRlib than what I used as well.  It’s a fantastic library for getting a jumpstart on virtually any AVR project, though it does add some bloat so it isn’t well suited to the smaller AVRs.  I wrote the navigation, servo and UI software from scratch with help from internet research for timing, distance and bearing formulas, and inturrupt pin and i2c examples.

I learned a lot during this build.  The navigation stuff was pretty interesting.  Learning how to use AVRs as both i2c master and slave was really worthwhile.  The smaller AVRs are limited in terms of things like number of pins and peripherals, but they have all the processing horsepower of the bigger AVRs.  With an ATTINY or Atmega8 costing $2-4, a cheap and powerful approach to many projects would be to use 1 AVR as main processor to handle UI and higher level functions and 1 or more AVRs as i2c or spi slaves to read sensors, parse gps data, drive servos, etc.

Deathpod3000 at the finish line

There were 20 teams – 17 rovers and 3 planes.  The rovers were going first and the planes last.  Each team got 3 tries with the fastest time winning.  There was a pit area inside for prepping the vehicles.  Here is a pic of us getting ready in the pits.

In the pits

Unfortunately, we didn’t get video of the first run, but here is a video of our practice run.  The real run was very similar.

I decided to try to just make it around the first try.  I set the program to go 30% throttle all the way.  30% is somewhere between a walk and a jog.  There was immediate drama in trial 1.  It headed too far to the right because of gps error and it started following the curb in the wrong direction.  There were gasps from the crowd.  But, after about 3 or 4 feet, it turned around and headed down the course.  The crowd cheered!  It had to steer to follow along the curb by the pond, again because of gps error, but it  made all the way around and crossed the finish line without any other problems.  Our time was just over 2 minutes.

Here are a few pics of some of the other vehicles.

Most of round 1 ended up like this:

The big “tank” decided to go off-roading:

One of the planes ended up in a tree and the fire department had to come get the plane down.

Plane stuck in a tree

Through round 1 we were the only vehicle to successfully make it around the course.  Round 2 started and it was going pretty much like round 1.  I decided to crank up the speed to 50% throttle, which is basically like a medium run.  It cuts the speed down as it approaches a waypoint or if it detect an obstacle.

Here we are at the start line for trial 2:

We shot video of the second run, but because we had to run to keep up, it is really shaky.  Here is video of the beginning.  You can see the start, the first turn and you can see it avoiding the curb.

The drama in run 2 came at the pond.  The car headed straight at the pond at full speed.  Because it was going faster, it came within 3 inches of smashing into the curb before it detected the curb, cut the wheels hard left, did a 180 spin and stopped.  Then it started up again headed down the course.

It  completed the course and shaved 30 seconds off our first run at 1 minute 28 seconds.

Deathpod3000 crossing the finish line

The editor in chief of Make magazine came out for the event and he interviewed us after round 2.

Through round 2 we were still the only vehicle to complete the course.  In round 3 we upped the speed to 70% throttle and unfortunately that was too fast.  It couldn’t turn in time after detecting the curb and we smashed head-first.

The final run of the final vehicle – an airplane – was the only other successful run.  They had a time of 39 seconds, much faster than our 1:28 and they took first place.  Here is a pic of the winning team.

We got 2nd place and also won the Engineer’s Choice award, voted on by Sparkfun’s engineers.

The whole day was a blast.  Thanks to Sparkfun for sponsoring such a fun competition.  They are planning to make this an annual event so get planning for next year.  I’m sure they’ll make some changes to make it even better.  (Hopefully put air and ground vehicles in separate divisions…)

If you’ve enjoyed reading the blog, please leave a comment.  Right now I need a few days to recover!

I connected this sonar to the i2c bus I was already using for the compass.  One of the cool things about i2c is you can connect many devices to just 2 wires on the microcontroller and communicate with each device based on its address.  I was getting distance readings in inches and it was perfect down to 1″ distance.  Max range was about 4 feet.

Unfortunately, the 45 degree cone of the sonar is both left and right and up and down.  When I put the sonar on the car, it was detecting the ground.  I tried tilting it up and I even tried building a shield to block the sonar reflections from the ground.

Sonar Shield

The shield extended the range of the sonar, but it still had trouble detecting the curb and the car couldn’t turn fast enough to avoid objects.  The range was only 18-24″ being so low to the ground and by the time the car turned the wheels, it had already hit the object.  I decided sonar wasn’t going to work.

The other option for object detection (other than a $3,000 laser, which I felt violated the spirit of the competition) is infrared.  I’ve used ir detectors in the past, and they are very accurate, but usually don’t have the range of sonar and they detect objects in a straight, pencil beam line.  The main obstacle to avoid in the race is the curb, which is a linear obstacle extending out basically to infinity so the almost laser-like detection probably won’t be a problem for the race.  I found some newer, long-range ir sensors I hadn’t seen before.  The Sharp GP2Y0A710YK0F ir sensor claims to detect objects from 40″ to 200″.

I bought a couple and did some testing.  I was getting maybe 4-5 feet of real-world range and putting it right next to the ground was no problem since the ir light beam is so narrow.  Now for the problem.  I needed a full 8 bit port for the LCD and the only port I wasn’t using at least one pin in was port A, the port that has all the analog-to-digital conversion pins.  The ir sensor outputs an analog voltage, higher voltage means close object.  So, my choices were to try to totally rewire the LCD (14 wires – ugh) or figure out another way to connect the sensors.

I had a spare Atmega8 laying around and I knew AVRs could be i2c slaves as well as master so I decided to try to program the Atmega8 to read the sensors and report back the analog voltage values to the main chip through i2c.  I initially had some problems, but was eventually able to get it working using AVRLib.  Here is the code in the Atmega8 to read the Sharp ir distance sensors:

DDRC = 0×00; //Set a2d port as input
PORTC &= 0×00;  //turn off pull-up resistors
a2dInit();  //Initialize AVRLib A2d
a2dSetPrescaler(ADC_PRESCALE_DIV128); //most precise
a2dSetReference(ADC_REFERENCE_AVCC); //Use 5v ref
ir[0] = a2dConvert10bit(0);  //Read a2d pin 0

That’s it.  I actually have 2 sensors so I read both values.  Now I had to make the Atmega8 return the readings from the 2 sensors whenever the master asked for them.

First in the main function, you have to set things up.

i2cInit(); // Initialize i2c AVRlib
//Set slave address to 2
i2cSetLocalDeviceAddr(2, TRUE);
//Set slave receive function to our function
//We don’t actually use this
i2cSetSlaveReceiveHandler( i2cSlaveReceiveService );
//This function will be called when a master requests data
i2cSetSlaveTransmitHandler( i2cSlaveTransmitService );

When the master sends a “get data” request to device 2, our i2cSlaveTransmitService function will be called and that will send data back to the master.  Here is the code in the transmit function.

u08 i2cSlaveTransmitService(u08 transmitDataLengthMax, u08* transmitData)
{
transmitData[0] = ir[0] >> 8;
outBuffer[0] = transmitData[0];
transmitData[1] = ir[0] & 0x00FF;
outBuffer[1] = transmitData[1];
transmitData[2] = ir[1] >> 8;
outBuffer[2] = transmitData[2];
transmitData[3] = ir[1] & 0x00FF;
outBuffer[3] = transmitData[3];
sent = 1;
return 4;
}

I decided to go most significant byte first, so it strips that out of the int and puts it into the transmit buffer, then puts the least signifigant byte in the transmit buffer.  It repeats the operation for the 2nd sensor reading and then return that there are 4 bytes to send to the master.

On the master side, the code is very similar to the compass reading code.

void GetIR()
{
u08 in[4];
i2cMasterReceive(2, 4, &in[0]);
ir1 = in[0];
ir1 = ir1 << 8;
ir1 += in[1];
ir2 = in[2];
ir2 = ir2 << 8;
ir2 += in[3];
}

It reads from device 2 and basically does the reverse of what is in the slave to re-build the int from the MSB and LSB.  So, now I can read the ir sensors in the main program by calling this function.

Unfortunately, the front of the rc car where I can mount sensors is angled so I had to build a platform for the sensors.  I used a hinge and some screws to make it adjustable so I could dial in just the right angle to detect the low curbs, but not the ground.  It isn’t pretty, but it works.

Deathpod with ir sensors

One other thing I found when I was testing at the track is if the waypoint “moves” because of GPS error to someplace the car can’t reach (like, say, into the pond) even if you avoid the curb, the car won’t complete the course because it will never move to the next waypoint.  I came up with a couple of strategies to overcome this and implemented the simplest one.  If it determines it is on a line between the current waypoint and the next waypoint, it will drop the current waypoint and move on to the next one.  It should move along the curb with the ir sensor trying to get to the current waypoint and then at some point decide it can just abandon it and continue on.

That’s going to have to be it.  I wasn’t sure I could get the ir sensors on and working in time, but I made it.  We did some testing in the street outside the house and it looks promising.  Race day is here so we’ll just have to take what we have and hope for the best.

First up, some pics of the final board construction.

Board1

I’m using a board from Sparkfun that includes powersupply, db9 serial port and rs232 conversion, in circuit programming port, 8 mhz crystal and plenty of room to solder our connections.  I decided to connect everything with ribbon cables so I can re-use things on other projects if I need to.  Since I originally thought I wouldn’t be using anything requiring analog to digital conversion, I needed an entire 8 pin port for the LCD and I needed pins from the other ports for things like i2c, serial and input pin interrup, I used port A, which is also the a2d pins.  This turned out to be a headache later on.

The pic above shows the board after I soldered wires to connect the LCD.  I tested this first because it can help with debugging other problems.  Next I soldered connections for serial and power to go to the gps, i2c and the servos.  I included one more 3 pin servo connection wired to one of the interrupt pins, which I’ll talk about later.  I also used this connection to provide power for the servos, which comes from the rc battery, separate from the board power.

Board2

After soldering and testing all the connections I bought some thin, but stiff plywood and bolts/screws to use to attach everything to the car.  I cut out a rectangle of plywood big enough to hold the boards and parts, but small enough to allow removing the rc car battery.  There is a plastic cover on the steering servo in the front of the car and I decided to use this for mounting.  I drilled holes through the plasic cover and then used small bolts to attach the plywood to the cover.

Construction

Then I used screws to attach the boards to the plywood.  The compass was a little tricky since it didn’t have any mounting holes and it needs to be in a consistent orientation.  I decided to superglue the compass to the plywood away from the main board and midway between the motor controller and the steering servo to try to minimize magnetic interference.

With the very real possibility of ending up in a pond, I decided I needed a way to kill the throttle if the car headed for water.  I thought I could use the rc receiver to read the pulses from the transmitter and only run if the car detected pulses.  I connected once channel of the rc receiver to one of the INT pins on the Atmega32.  The receiver channels are wired just like servos and provide the exact same pulse timing that you would send to a servo.  The 3 wires are ground, power and signal.  I connected the signal wire to the INT1 pin on the Atmega32 and set it up to trigger an interrupt on any change (high to low or low to high) on the pin.  Here is the code to do this:

//Set up external interrupt for reading receiver
cbi(EXTINTDIR, EXTINT);
sbi(PORTD, PD3);
MCUCR = 1<<ISC10;
GIMSK = 1<<INT1;

I previously defined EXTINTDIR as DDRD and  EXINT as PD3.  The first two lines set up the pin as input and enable the internal pull-up resistor.  The next line sets INT1 as trigger interrupt on any change.  There are a number of different conditions you can have trigger the interrupt including low to high, high to low, low and any change.  See the Atmega32 datasheet for which bits to set in MCUCR for each type.   The last line sets the bit in the global interrupt mask for INT1, which enables the interrupt pin.

The code to service the interrupt is very simple since all I need is a counter that increments once every time there is a change on the pin indicating a servo pulse on or off.  I set up a global int variable called cntValue and start it at 0.  I decided to set it back to 0 after 20,000 interrupts.  We only care about change in the value, not the specific value itself so overflows don’t matter.

//External Interrupt Handler
ISR(INT1_vect)  /* signal handler for external interrupt int0 */
{
//We just increment this variable whenever there is a state
change
//We store the previous value each cycle of our main navigation
loop
//Then check current value against the previous value to see if
//We have rc transmitter signal.  If not, safety kill.

cntValue++;
if(cntValue > 20000)
{
cntValue = 0;
}
}

In the main navigation loop, we compare the current value to the value stored during the last time through the loop and if it is the same we kill the throttle.  Basically, when the transmitter is on, no matter what state the controls are in, the car goes, when the transmitter is off or if it goes beyond signal range, the car stops.

OK, so now the main board is wired up and attached to the car and the kill switch will hopefully keep us out of the pond should things go really bad.  Now, we need some way to avoid obstacles.  The next post will be about using sonar to do that.

Sparkfun

We walked the car around the building and set waypoints at the corners.  After starting the car, it tried to cut the first corner too short and headed for the curb.  We moved the car over and after heading around the turn, it went straight down the side.  It had a couple more curb incidents  and it wouldn’t have made a complete lap on its own.

We decided to try more waypoints and it only made the problem worse.  There is a pond on one side of the course and Deathpod apparently was feeling a little warm because it really wanted to go for a swim.  After a number of attempts, it didn’t make it all the way around the building without incident once.  The major problem was gps error and some relatively tight straightaways.

I don’t think I mentioned that I connected the r/c receiver to a pin on the atmega32 so I can read the rc pulses as a failsafe.  It doesn’t actually do anything with the rc pulses other than count them.  If the count keeps going up, it keeps driving.  If the rc pulses stop, the car cuts throttle.  This way, when Deathpod decides to go for a swim, I can stop the car before he gets to the water by simply turning off the rc transmitter.

My original plan was to navigate using only gps and compass, but it became clear that the car will need abstacle avoidance.  If there are cars in the parking lot, Deathpod will need to go around them.  More importantly, it will need to stay within the curbs to keep it from going into the pond!  I have both sonar and infrared distance sensors.  I think sonar will be better for general obstacle avoidance, but I don’t know if it will work for curbs.  In the next post I will describe how I read the rc servo pulses from the receiver and then I will talk about connecting and testing the sonars and infrared sensors.

Even with 5hz updates, the gps isn’t going to be able to give us accurate heading data during a turn.  The reason is a gps figures out heading based on a line drawn from where you were during the last update to where you are this update.  In a turn, as the car goes around in a circle, the nose is always pointing beyond this line so the gps heading will always lag behind the true direction the car is pointing.

The way to get an accurate heading reading in a turn is with a compass.  Our UART (regular serial port) is already being used by the gps, so we need a compass with some other interface.  The Atmega32 has both spi and i2c synchronous serial.  Sparkfun carries a digital compass chip with an i2c interface mounted on a small board with spots to solder pins at standard .1″ spacing.  The Honywell HMC6352 mounted on a breakout board requires only 4 connections:  5v, GND, clock (SCL) and data (SDA).

I2c is a 2 wire synchronous serial interface.  It is actually a bus since it can accomodate multiple slave devices and a master device on only 2 wires.  I2c was originally developed by Philips and to get around  licensing costs, other manufacturers have released their own implementations.  Smbus is compatible with i2c and is used on many motherboards to communicate with temperature sensors.  Atmel calls it TWI (Two Wire Interface).

The two wires  that make up the i2c bus are clock and data.  The master drives the clock and both master and slave transmit using the data line.  In this case, the Atmega32 will be the master and the compass chip will be the slave.  Because it is a synchronous interface, much higher transfer speeds are possible than with an async serial interface like rs232.  The “slow” speed is 100 Kbps and you can go up to 1 Mbps with some devices.  Because of the fast speed, small number of connections required and ability to connect multiple devices on the same bus, i2c is a very popular communication mechanism for microcontrollers.  You can find devices that use i2c ranging from eeprom chips to sonars to motor controllers and compasses.

I soldered standard straight pins onto the compass breakout board thinking I’d use one of the 10 pin header ribbon cables I had to connect to similar pins on the final board.  This will allow me to connect and disconnect the compass easily if I want to use it on another project in the future.  On the Atmega32, pin PC0 is SCL and PC1 is SDA.  I used breadboard wires to connect the 10 pin header to the correct pins on the STK500 dev board.

I2c handles multiple devices on a single 2 wire interface by assigning each device a unique 7 bit address.  The compass by default uses address hex 42.  The master device doesn’t need an address.

AVRlib includes a module to handle i2c communication both as a master and slave.  We’ll use this module to communicate with the compass.  The library makes this pretty easy and we will only need to call one fuction to write to the device and 1 function to read from the device.  All the low-level stuff is taken care of by the library and the AVR hardware.  By default, the compass is in “query” mode.  It will take and return a reading on command.  This sounds fine for what we want to do.

To take a reading you must first send hex 41, then read 2 bytes (MSB, LSB) of heading.  The heading has an implied 1 decimal point so a reading of 2436 would be a heading of 243.6 degrees.  It takes 6 ms for the compass to compute the heading so we’ll put in a little delay of 10ms to let it do it’s magic.  First, in order to make changes easier, we’ll put in a define for the target address of our i2c commands.

//define for compass iic addresses
#define TARGET_ADDR 0×42

I put the code to read a compass heading in a function so it will be clean and easy to read the heading in our main processing loop.  In order to account for errors in compass placement (where the compass isn’t pointing directly forward on the car), I included an offset passed to the function that we can use later if we need to correct for some heading error.  Here is the code in my compass reading function.

float GetCompassHeading(float offset)
{
u08 c=0;
u08 in[2];
int reading = 0;
float returnHeading = 0.0;
c = 0×41;
i2cMasterSend(TARGET_ADDR, 1, &c);
timerPause(10);
i2cMasterReceive(TARGET_ADDR, 2, &in[0]);
reading = in[0];
reading = reading << 8;
reading += in[1];
returnHeading = (float) reading;
returnHeading = returnHeading / 10;
returnHeading += offset;
if(returnHeading < 0)
{
returnHeading += 360;
}
if(returnHeading >= 360)
{
returnHeading -= 360;
}

return returnHeading;
}

No indenting for some reason.  Still readable I guess.  Anyway, set up unsigned char variable c to hold the “command” to send and 2 byte array in for the returned data.  We’ll convert the reading to an in int called reading and use a float to hold the final reaturn heading.  We set the command to 0×41 (hex 41 or the character ‘A’) and then call i2cMasterSend, the AVRlib command to transmit.  The three parameters are i2c address, bytes to send and a pointer to the data.  Next we wait 10ms and then read the result.  i2cMasterReceive takes the same 3 parameters as send.  We read the first byte (MSB), then shift it 8 bits to the left and then add the next byte we received to construct the 16 bit int.  Next we convert it to a float and load it into our returnHeading, then divide by 10 to get our xxx.x heading.

Now we will deal with the offset passed into the function.  First we simplyadd it to the heading.  At this point, if the offset is negative  and the heading is close to 0, we could end up with negative heading.  For example, let’s say the heading is 006.0 and offset is -9.  The returned heading would be -003.0, which doesn’t make any sense.  To account for this, if the heading ends up less than 0, we add 360.  So in our example, -003.0 + 360 = 357.0.  In the same way, with a positive offset and a heading near 360, we could end up with a heading that greater than 360.  We take care of this situation by subtracting 360 if the heading is greater than or equal to 360.  Now we will always return a heading between 0 and 359.9 degrees.

I changed the main navigation loop to read the compass, use the compass heading for steering rather than the gps heading and display the compass heading on the LCD.  Turning on the system gave me…nothing.  I was getting 0 no matter which way I turned the compass.  I searched the web, I double-check the code, I tried non-interrupt driven i2c calls.  Then I checked the wiring.  It turns out I had the SDA line connected to the wrong spot on the header.  I moved the wire over one spot and bingo – rapidly changing heading readings.

The compass chip has a calibration mode where you spin the chip around for several seconds and it calibrates to it’s environment in case there is metal around, etc.  I added an option when the system starts up to allow the user to do the calibration and then point to the first waypoint stored in eeprom.  The car calculates an offset based on this and stores the offset to feed to the compass heading reading function.

Testing the new setup with the faster gps and digital compass proved successful.  The car no longer drove around in circles, but once it figured out bearing to the waypoint, it drove straight toward it.  The waypoint tended to move around because of gps error, but the car was able to track directly to where it thought the waypoint was.

Navigation is working.  The next step is to ditch the STK500 dev board and wire up the final, much smaller and lighter board and start attaching things more permanently to the car.

After looking at available options, I settled on the Locosys LS20031 5hz gps receiver.  It is a self-contained unit, complete with antenna.  It outputs nmea and I already have an nmea parser library.  The only connections required are power, ground and serial tx/rx.  After doing some research, the only problem to overcome was the 3.3v to 5v conversion required.

Sparkfun sells a 3.3v to 5v logic level converter board.  This little (and I mean little) board will convert tx/rx for 2 serial connections.  One side of the board takes 5v and the other takes 3.3v connections for power.  Tx/rx on the high side operates at 5v and the low side operates at 3.3v.  It worked like a charm.

The gps has no connectors, just pads to solder to so I soldered wires to the pads.  I decided to connect everything using a breadboard to make sure communication was working before doing the final wiring.  I used a 3.3v voltage regulator to supply the 3.3v, connected tx/rx on the low side to the gps and the high side to the tx/rx pins on the Atmega32.  Note that because the gps uses ttl serial I did not have to use an rs232 converter like I did with the Garmin unit.  That is why I am connecting directly to the pins on the microcontroller.

Here is a pic of the breadboard setup.  You can see the gps chip (large looking white/cream/green square – it is actually only about an inch square) to the right.  To the left you can barely see the logic level converter board buried in wires and just above and to the left of that the voltage regulator.

The gps has a little light that is supposed to flash once per second if it has a lock.  I turned on the power and waited to see if I’d get “flashy lights”.

All right, so the gps appears to be working.  To start I decided to connect it to the computer because it would be easier to debug communication problems when I can see the actual output and test various baud settings, etc.  After connecting things up I got…garbage.  The datasheet for the gps chip said by default it used 9600 bps.  Nope, after some experimentation I found by default it was spitting out data at 57,600 bps.  On the bright side, it seemed to be working perfectly.

There is a little program available for download on the sparkfun site here for another gps that I found works fine with the gps I bought as well.  It will show you how many sats it has locked, lat/long, settings, etc.  The nice thing is it will also let you set the parameters and save them to the chip.  I am only parsing 2 nmea sentences,  GGA and VTG, so I turned off all the others and set the unit for 5hz updates.

Sorry about the blurry pic, but you can at least see that it found satellites and is giving data to the program.  Everything looks great.

Next, I connected the gps directly to the microcontroller pins and started getting lat/long readings just like before, except a lot faster.  I increased the size of the serial buffer in my uart library to allow for the faster serial speed.  I also decided to cut back to 2 updates per second, at least for now.  This is a huge improvement over 1 every 2 seconds and if I get everything else working and I can spare the cpu cycles, I will increase it later.

Next on the agenda is connecting the digital compass.  It uses i2c to communicate with the microcontroller.  We’ll have to add some more library code to deal with that and connect to some more pins.  We’ll cover that in the next post.

Here is a little video of me showing the parts:

And here’s a vid of the first drive, which mostly features driving around in circles:

The main problem seems to be that the cpu only gets updates from the gps about once every 2 seconds and it is getting current heading data from the gps.

As the car turns, the gps computes heading based on where it was at the last fix and where it is at the current fix.  Unfortunately, while it’s turning, it is pointing way past this direction so it constantly overshoots where it is trying to turn.  This coupled with the fact that it only gets updates to its heading (as bad as that heading data may be) every 2 seconds means it can’t figure out where it is heading and spins around in a circle until it decides to spin around in a circle the other directions.

If the car is pointing where it should be going, it does drive straight.  So, it is computing where it wants to go correctly.  The problem is with the current sensor array, it can’t accurately point the car in the desired direction.

The good news is it looks like the basic programming and navigation software work.  So, it looks like we need to upgrade our sensor array.  I ordered a new gps that updates 5(!) times per second and a digital compass.  Hopefully more frequent updates from the gps will help.  More importantly, though, the digital compass should allow the cpu to get heading readings in real time so it can track to the desired heading accuratly.

The compass uses i2c, a synchronous serial interface and the gps is 3.3v, which will require a level converter.  In the next post, we’ll hook up the new hardware.