How to modify uCOS-III source codes when MSP432's FPU is enabled?

Hi, I'm working on a project with MSP432P401R launchpad.  And I have already ported uCOS-III v3.05 to it successfully.

But when I add some functions which deal with float point data, the uCOS-III runs into App_Fault_ISR().

As far as I know,   FPU of MSP432 is enabled in default when compiled under CCS Studio v6.1.3.

While uCOS-III does not support FPU feature defaultly, so I have modified *OSTaskStkInit

CPU_STK  *OSTaskStkInit (OS_TASK_PTR    p_task,
                         void          *p_arg,
                         CPU_STK       *p_stk_base,
                         CPU_STK       *p_stk_limit,
                         CPU_STK_SIZE   stk_size,
                         OS_OPT         opt)
{
    CPU_STK    *p_stk;

    
    (void)opt;                                                  /* Prevent compiler warning                               */

    p_stk = &p_stk_base[stk_size];                              /* Load stack pointer                                     */
                                                                /* Align the stack to 8-bytes.                            */
    p_stk = (CPU_STK *)((CPU_STK)(p_stk) & 0xFFFFFFF8);
                                                                /* Registers stacked as if auto-saved on exception        */
#if(OS_CPU_ARM_FP_EN == DEF_ENABLED)
    *--p_stk = (CPU_STK)0x00000000u;                            /* No Name Register                                       */
    *--p_stk = (CPU_STK)0x00000000u;                            /* FPSCR                                                  */
    *--p_stk = (CPU_STK)0x15151515u;                            /* S15                                                    */
    *--p_stk = (CPU_STK)0x14141414u;                            /* S14                                                    */
    *--p_stk = (CPU_STK)0x13131313u;                            /* S13                                                    */
    *--p_stk = (CPU_STK)0x12121212u;                            /* S12                                                    */
    *--p_stk = (CPU_STK)0x11111111u;                            /* S11                                                    */
    *--p_stk = (CPU_STK)0x10101010u;                            /* S10                                                    */
    *--p_stk = (CPU_STK)0x09090909u;                            /* S9                                                     */
    *--p_stk = (CPU_STK)0x08080808u;                            /* S8                                                     */
    *--p_stk = (CPU_STK)0x07070707u;                            /* S7                                                     */
    *--p_stk = (CPU_STK)0x06060606u;                            /* S6                                                     */
    *--p_stk = (CPU_STK)0x05050505u;                            /* S5                                                     */
    *--p_stk = (CPU_STK)0x04040404u;                            /* S4                                                     */
    *--p_stk = (CPU_STK)0x03030303u;                            /* S3                                                     */
    *--p_stk = (CPU_STK)0x02020202u;                            /* S2                                                     */
    *--p_stk = (CPU_STK)0x01010101u;                            /* S1                                                     */
    *--p_stk = (CPU_STK)0x00000000u;                            /* S0                                                     */
#endif
    *--p_stk = (CPU_STK)0x01000000u;                            /* xPSR                                                   */
    *--p_stk = (CPU_STK)p_task;                                 /* Entry Point                                            */
    *--p_stk = (CPU_STK)OS_TaskReturn;                          /* R14 (LR)                                               */
    *--p_stk = (CPU_STK)0x12121212u;                            /* R12                                                    */
    *--p_stk = (CPU_STK)0x03030303u;                            /* R3                                                     */
    *--p_stk = (CPU_STK)0x02020202u;                            /* R2                                                     */
    *--p_stk = (CPU_STK)p_stk_limit;                            /* R1                                                     */
    *--p_stk = (CPU_STK)p_arg;                                  /* R0 : argument                                          */
                                                                /* Remaining registers saved on process stack             */
#if (OS_CPU_ARM_FP_EN == DEF_ENABLED)
    *--p_stk = (CPU_STK)0x31313131u;                            /* S31                                                    */
    *--p_stk = (CPU_STK)0x30303030u;                            /* S30                                                    */
    *--p_stk = (CPU_STK)0x29292929u;                            /* S29                                                    */
    *--p_stk = (CPU_STK)0x28282828u;                            /* S28                                                    */
    *--p_stk = (CPU_STK)0x27272727u;                            /* S27                                                    */
    *--p_stk = (CPU_STK)0x26262626u;                            /* S26                                                    */
    *--p_stk = (CPU_STK)0x25252525u;                            /* S25                                                     */
    *--p_stk = (CPU_STK)0x24242424u;                            /* S24                                                     */
    *--p_stk = (CPU_STK)0x23232323u;                            /* S23                                                     */
    *--p_stk = (CPU_STK)0x22222222u;                            /* S22                                                     */
    *--p_stk = (CPU_STK)0x21212121u;                            /* S21                                                     */
    *--p_stk = (CPU_STK)0x20202020u;                            /* S20                                                     */
    *--p_stk = (CPU_STK)0x19191919u;                            /* S19                                                     */
    *--p_stk = (CPU_STK)0x18181818u;                            /* S18                                                     */
    *--p_stk = (CPU_STK)0x17171717u;                            /* S17                                                     */
    *--p_stk = (CPU_STK)0x16161616u;                            /* S16                                                     */
#endif
    *--p_stk = (CPU_STK)0x11111111u;                            /* R11                                                    */
    *--p_stk = (CPU_STK)0x10101010u;                            /* R10                                                    */
    *--p_stk = (CPU_STK)0x09090909u;                            /* R9                                                     */
    *--p_stk = (CPU_STK)0x08080808u;                            /* R8                                                     */
    *--p_stk = (CPU_STK)0x07070707u;                            /* R7                                                     */
    *--p_stk = (CPU_STK)0x06060606u;                            /* R6                                                     */
    *--p_stk = (CPU_STK)0x05050505u;                            /* R5                                                     */
    *--p_stk = (CPU_STK)0x04040404u;                            /* R4                                                     */

    return (p_stk);
}

and the OS_CPU_PendSVHandler() function

    .asmfunc
OS_CPU_PendSVHandler:
    CPSID   I                                                   ; Prevent interruption during context switch
    MRS     R0, PSP                                             ; PSP is process stack pointer
;.if __TI_TMS470_V7M4__ & __TI_VFP_SUPPORT__
    VSTMDB    R0!, {S16-S31}                                 ; Save remaining regs s31-s16 on stack
;.endif
    STMFD   R0!, {R4-R11}                                       ; Save remaining regs r4-11 on process stack

    LDR     R5, OSTCBCurPtrAddr                                 ; OSTCBCurPtr->OSTCBStkPtr = SP;
    LDR     R6, [R5]
    STR     R0, [R6]                                            ; R0 is SP of process being switched out

                                                                ; At this point, entire context of process has been saved
    MOV     R4, LR                                              ; Save LR exc_return value
    BL      OSTaskSwHook                                        ; OSTaskSwHook();

    LDR     R0, OSPrioCurAddr                                   ; OSPrioCur   = OSPrioHighRdy;
    LDR     R1, OSPrioHighRdyAddr
    LDRB    R2, [R1]
    STRB    R2, [R0]

    LDR     R1, OSTCBHighRdyPtrAddr                             ; OSTCBCurPtr = OSTCBHighRdyPtr;
    LDR     R2, [R1]
    STR     R2, [R5]


    ORR     LR, R4, #0xF4                                       ; Ensure exception return uses process stack
    LDR     R0, [R2]                                            ; R0 is new process SP; SP = OSTCBHighRdyPtr->StkPtr;
    LDMFD   R0!, {R4-R11}                                       ; Restore r4-11 from new process stack
    .if __TI_TMS470_V7M4__ & __TI_VFP_SUPPORT__
    VLDMIA   R0!, {S16-S31}
    .endif
    MSR     PSP, R0                                             ; Load PSP with new process SP
    CPSIE   I
    BX      LR                                                  ; Exception return will restore remaining context
    .endasmfunc

.end

In addition, I add "__VFP_FP__" predefine in project config.

Also the "OS_OPT_TASK_SAVE_FP" option is enabled when task in uCOS-III is created.

Despire all the changes, the uCOS-III still runs into App_Fault_ISR() routine after the first task is switch out.

Any one has any idea about the issue? Thank you vech much!