Make the fw att controller more robust against infinite input

Make the fw att controller more robust against infinite input by

* not running the controllers if one of the input arguments is not finite
* resetting the integrators if the output becomes infinite/NaN. It's clear that this should not happen in the first place. But as this is software for a flying system being able to recover from this kind of error is important.