Fixes Waypoint Expression

This got rolled back in a previous commit. Derp.
Merge branch 'testbench'