added diagnosis stage, stmpe implementation started
minor diagnostics fixes, fixed video generation on stmpe irq
cleanup, minor changes to diagnostics