Fw cleanup

This cleans the FW position controller and addresses a number of relevant compile warnings.

@sjwilks @jean-m-cyr Code review would be cool.