More SCMT implementation/ fixes.
Self-test completes correctly, and starting vigilance device intervenes as appropriate.

Fixed several issues of incorrect brake interventions.