FW pos control: fix warnings and remove code

Fixed several compiler warnings and removed some old code which had no effect. Much more could be done but this will be part of #1101 (not so soon).

Please review and test in HIL.