MSP430F5529 scheduler

Other Parts Discussed in Thread: MSP430F5529

Hello,

I'm  going to make a scheduler for some tasks for the MSP430F5529 microconttroller:

I've started from the following scenario:

uint32 OS_TASK1_RAM[OS_TASK_STACK_SIZE] = {0};
uint32 OS_TASK2_RAM[OS_TASK_STACK_SIZE] = {0};


volatile uint8 OS_TASK_ID;						    /* holds the current ID task */
volatile uint32 *OS_SP[OS_MAX_TASKS] = {0};			/* holds the address of the task */
volatile uint32 *currentTask;

void _OS_start()
{ 
        WDTCTL = WDTHOLD | WDTPW;

	OS_SP[0] = OS_STACK_INIT(ledToggle,&OS_TASK1_RAM[OS_TASK_STAK_TOP]);
	//OS_SP[1] = OS_STACK_INIT(TASK_gpsDataProcessing,&OS_TASK2_RAM[OS_TASK_STAK_TOP]);

	OS_TASK_ID = 0;

	currentTask = OS_SP[0];
	asm volatile (" MOV.A currentTask,R1 \n\t");

	OS_RESTORE_CONTEXT();

        _BIS_SR(GIE);
}

uint32 *OS_STACK_INIT(void (*pOS_TASK), uint32 *OS_STACK_LOCATION)
{
	uint8 index;

	*OS_STACK_LOCATION = (uint32)pOS_TASK;									/* save the address of the task */
	 OS_STACK_LOCATION--;

	 //*OS_STACK_LOCATION = ((uint32)0xF000 & (uint32)pOS_TASK >> 4) | OS_DEFAULT_SR;


	for(index = 0; index < OS_REG_BACKUP; index++)
	{
		OS_STACK_LOCATION--;												/* leave space for the other registers */
	}

	return (OS_STACK_LOCATION);

}

I don't know why is not working, what should I take in account besides of these ?