Great.
Thanks. I thought so.
Since I have never done an auto pilot. I am up a creek without a paddle.
Wouldn't know where to begin?
How about some theory first? Not too much, you are not planning to do your PhD in it... but some key concepts and key words should be helpful.
Like this article for example:
[ame="http://en.wikipedia.org/wiki/Control_system"]Control system - Wikipedia, the free encyclopedia[/ame]
Basically you can vertically structure your autopilot code already around the letters of the TLA GNC: Guidance, Navigation and Control.
You have a navigation layer, that establishes where your spacecraft is currently pointing at (and maybe where it will be pointing at in the future, which is often also interesting). You have a guidance layer, that uses the navigation for telling where it should be pointing and how you could orient your spacecraft in that direction. And you have a control layer, that does the actual motion, for example by using the RCS.
If you look closer at the three layers, you can also maybe see, that its possible to interchange different kinds of navigation, guidance and control modules: If you have a new sensor available (like radar), you can include it into the navigation. if other thrusters are available, you can use a new control function.
Now the trick question is: Where do you start? Maybe you should start simple at the most basic problem you will get: Three-axis stabilization.
Important is for example: Which coordinate system do you use for navigation? Many spacecraft actually use multiple coordinate systems, depending on which guidance task they are actually doing. For example, for the MSL, you could use a Mars relative coordinate system, that has its origin in the landing site. Or you use the Entry Interface as reference. Or both.