Added Fly to set point functionality.

Motor signal values are currently buggy.