So you're trying to write the firmware from scratch?
Are you familiar with Multiwii in detail? It's a *large* arduino sketch that implements a fairly solid set of control loops. If you want to see "how others have done it", crack open the source -- that's the best place to look.
If you've got a specific question about how is "X" done in multiwii, there are a few people here (myself included) who can help you understand the code that exists.
As for how to process the data from a sensor, that's never a simple question, and the simple answer is vague:
- You receive the data at your designed rate across a data channel you've designed
- You do mumbo-jumbo magic processing (and as little as you have to) depending on the nature of the data and the information you need to extract
- You use the extracted information to do something.
You'll either need to get more specific, or open that source and take a look at what they do to get into the nuts-and-bolts of the work.