QuadCop: Navigation, how do I get to a specific GPS coordinate?

Previous Posts Here:

http://www.element14.com/community/tags#/?tags=quadcop_project

Im currently working on 2 major updates this weekend.  I am adding in some sci fi elements and installing sensors to give the QuadCOP a more Science Fiction Look.  The modifications also add quite a bit of realestate to place things and remove the clutter.

Here is a preview, stay tuned for some auwsome pictures of what this looks like when painted!

The other thing I am working on is testing the Raspberry Pi Flight System (RPFS) and I need to give you more information on how this works.  I have to demonstrate the actual functionality of the QuadCOP and it is time to do that.

I currently have about 2000 lines of original code between the systems not counting the TinyGPS++ library I used.  While not a lot, for an embedded system its not small!

Heading Information

Heading information is a number between 0 and 360 with 0 pointing magnetic north and 90 degrees pointing magnetic east.  The numbers get bigger as you rotate right and smaller as you rotate left unless you cross north.

For a 3D flight system, there are two forms of heading.  There is a GPS heading which shows the direction that the GPS unit is heading.  Then there is the quadcopter heading, which shows the direction the front of the quadcopter is pointing.

The reason this is important is that the quadcopter can move in one direction while facing another.  So the quad copter may be moving sidways and the GPS heading may say 90 degrees.  But the quadcopter may be pointing north.  The reason this is important to know is that without the quadcopter heading we don't know which way to move to change heading.   If we need to go east, and dont know which way the quadcopter is facing, do we go forward, backwards or sideways?   GPS Heading can also help with wind conditions.  If we are facing north but moving northeast, we know something is pushing us sidways and can make a correction to go due north.  This can be done by adding in some sideways velocity or pointing the front of the quad at an angle to and apply more forward velocity,

Way Point Information

A waypoint is a structure that contains altitude, longitude, latitude and heading information.  This information is sent to the RPFS 2 times per second.  So when in automode there are two waypoints.  The current position, and the destination.  The current position is stored in a waypoint structure since it fits nicely.

The are then two critical functions needed that let the QuadCOP navigate.  Both of these functions provide relevant information given two waypoints.

HeadingTo:  Given two waypoints, this returns a heading between 0 and 360 that points directly to destination.  This function is ran 2 times per second and is passed the current waypoint (current position) and the destination waypoint.  The heading is then updated to make the front of the QuadCOP point towards the destination.

DistanceBetween:  This provides the distance, in inches, between two waypoints.

How do we know when we reach the destination?  Well when the DistanceBetween the current position and the destination is "Zero".  Zero, is not really zero but a threshold that is good enough.  For my purposes 1 foot (.3 meters) is considered Zero.  I also have experimented with allowing the QuadCOP to move faster if the distance between is large enough.

CurrentSpeed:  This is calculated by the GPS.  It is not used by the waypoint functions but rather the flight heuristic system.  If the current speed is too fast, the QuadCop will slow down.  If it is too slow to QuadCOP an apply more power to move forward.  This allows correction for wind conditions that may affect flight.

The simplest approach to move between points is to always move forward.  This means that we have to keep the QuadCOP pointing in the direction of the next waypoint and apply a forward velocity to move in that direction.

Setting and Correcting Heading and Velocity

Two functions are used to get the QuadCOP going in the right direction.

SetHeading:  Given the required heading, it is compared with the current heading of the QuadCOP.   Care is needed to ensure that this is done correctly.  As an example., lets way we want to move due east at 90 degrees.  But the heading information shows 93 degrees.  This means we need to rotate left 3 degrees.  How does the QuadCOP know which direction to rotate?  A serious issue could happen if the QuadCOP tires to get to 90 degrees from 93 degrees by rotating right.  It would do nearly a 360 degree turn!  So simple care is needed to ensure we rotate the least amount.  We also need to make sure we don't over rotate.  So some heuristics are applied based on the correction needed.  If we only need to adjust 3 degrees, only a small amount of power is needed.

The amount of rotation we apply given the degree differential, is called the Heading Gain.  As an example, we are facing 93 degrees and need to get to 90 degrees by rotating left.  If we apply too much power, by the time the heading information is updated we have over corrected and now are facing 88 degrees.  So now another correction is needed to rotate right, and if we once again apply too mulch power we may up facing 92 degrees,  This cycle repeats quickly and causes a condition called "wag".

The front of the quad copter is looking left and right at a rapid pace and may never be able to hit its target.  If it gets bad enough it can cause the flight to become unstable.  Choosing the correct gain is an empirical processes that requires guesswork.  Also it is possible different values for the gain are needed at different times.  All of this is handled by the heuristic built into the RPFS so it can detect how its actions affect the sensor input and make adjustments as needed.

This seems fine for setting the heading but what about getting the QuadCOP moving?

I mentioned in previous posts the ControlSwitch (ChipKit Pi) is what actually controls the QuadCOP, the RPFS simply sends commands to the ControlSwitch to tell it what to do.  This done via ControlBytes.  These are a set of bytes that represent directions and adjustment information.  This simple structure ignores power information and will result in a small predetermined amount of power, in MicroSeconds, being applied in each direction.  Another control byte can be sent that is on a per direction level, with power information indicated.  This allows the QuadCOP to make fine adjustments in each direction as needed.  This to deal with the wind as discussed above.

So that concludes the first update for the weekend.  More to Come!  We are now getting into the fun stuff that final week!