Before we get to actual code...

Before we hop off into any actual implementation, I would suggest that we sit and think about some basics. The very first thing that should be done is to come up with a basic skeleton of the code that you would like to write. This may impact many decisions down the line. Basically, a UGV and a UAS/UAV are the same from the code's perspective. The UAV/UAS use different actual algorithms for certain processes, but the data flow and data collected are not so different. Some data is read, a decision is made, a servo solution is calculated, servos are moved into position and then the system loops again.

One thing that I did, was to develop an object that was a sensor, ISensor. This will be the part that actually interact with any device. ISensorReading will also act as a fundamental contract for data. I would make sure that this object follows in general an observable pattern. That way it is easy to allow other objects to read its data, or to be notified when new data is available. This will make it easy for filtering algorithms to execute and work on the newest data as soon as it arrives. It will also make it easier to make the data collection to be asynchronously collected and processed.

Then I made an instrumentation manager, IM. The IM will have factories for each kind of sensor and observe instances of each of the instruments. This way if something happens the IM can close an instrument and try to reconnect with a new object. In turn, the IM presents data to other parts of the code as needed. No other parts of the code need to see the sensors or interact with them directly.

On top of the instrumentation system, should be a director layer. This will actually be the layer that does all of the work. It makes a nice break line between the device and the presentation, or intelligence. Separating the systems is an important object-oriented technique that is necessary for keeping the code healthy.

More about the director later... I will see if I can get some pictures in to make it easier to see.