Fixing safety critical calculation (still commented out though) - shouldn't change any outputs