Further refactoring of code to pull out common material into the base class. The Platform now stores all sensors.
Updated the waypoint controller to store the internal state.
Tweaked the test scenarios to get them to work properly.