Optimize and fix commutation timing calculations

[mathiasvr]

Improvements and restructuring of the routines that calculates and updates the commutation period.

• Fixes some bugs for startup phase
• Skip redundant steps when possible
• Use specialized code macros instead of loops
• General assembly optimizations