sensors: minor optimization, cleanup and refactoring
Merge pull request #449 from PX4/sensors_fix

sensors: minor optimization, cleanup and refactoring