When trying to see what is happening with your code, you could try to follow stack size and current running time. Here is a simple function for doing the work :

void dsk_printDebug() {
#ifdef DEBUG
    uint64_t time;
    uint32_t msec, t;
    uint16_t hour, min, sec;
    time = TD_SCHEDULER_GetTime();


    msec = time & 0x3FFF;
    time >>= 15;
    t = time;

    sec = t % 60;
    t = t / 60;
    min = t % 60;
    t = t / 60;
    hour = t % 24;
    t = t / 24;


    #ifndef __ICCARM__
        extern char __cs3_stack[];
        char *limit=__cs3_stack;
    #else
        extern char CSTACK$$Limit[];
        char *limit = CSTACK$$Limit;
    #endif
        uint32_t *sp = (uint32_t *) __get_MSP();

    tfp_printf("[debug] Time:%d.%02d:%02d:%02d.%3d || Stack size : %d / %d \r\n",
        (uint32_t) t, hour, min, sec, (msec * 1000) >> 15,
        ((uint32_t)limit - (uint32_t)sp), CONFIG_STACK_SIZE
    );
#endif
}