Back to FreeRTOS definition of pushg and save context macros

This commit has to changes:
Back to FreeRTOS definition of push and save context macros
Initialize the SP with the upper part to 0, cause gcc problem to get 3byte PC