Should specify whether the bearing is supposed to be in radians or degrees (FYI, it's radians).
Also, in the 3rd paragraph the word "displacement" is in there twice. Minor typo :)
Zero-crossing detection saves the day since the Function block apparently doesn't have it.
In real Simulink, you'd just be able to use an "Abs" block here :)