Algorithm debugged, ready for testing
Prevent oscillating about zero angular velocity

Occurs when holding position in simulator, angular velocity oscillates between +- (accel_lim_w * period) / 2

This patch should prevent it