Sensor test minimal

Sensor test minimal for mavlink waypoint testing