Update mw.c

Jitter wrecked the YAW function, so hacked in some parts of my other codebase (that is not uploaded atm) to correct that.