Reverted back to working RTOS version (no ADC interrupt)