Probably also nicer to set this in m/s instead of the fixedpoint representation.
Then when checking get the speed in float from the state interface...
Although that will have slightly more overhead, but since this is only called with approx 40Hz it should be fine.