Exemplo n.º 1
0
void deepsleep (void)
{
  /* Set the SLEEPDEEP bit to enable deep sleep mode (STOP) */
  SCB_SCR |= SCB_SCR_SLEEPDEEP_MASK;

#if (defined(CMSIS))
  __wfi();
#elif (defined(KEIL))
	__wfi();
#else
  /* WFI instruction will start entry into STOP mode */
  asm("WFI");
#endif
}
Exemplo n.º 2
0
/* STOP mode entry routine. Puts the processor into normal stop mode.
 * In this mode core, bus and peripheral clocks are disabled.
 *
 * Mode transitions:
 * RUN -> STOP
 *
 * This function can be used to enter normal stop mode. 
 * If you are executing in normal run mode when calling this
 * function and AVLP = 0, then you will enter normal stop mode. 
 * If AVLP = 1 with previous write to PMPROT
 * then you will enter VLPS mode instead.
 *
 * STOP mode is exited using any enabled interrupt or RESET, so no
 * exit_stop routine is needed.
 *
 * Parameters:
 * Partial Stop Option:  
 *  0x00 = STOP - Normal Stop Mode
 *  0x40 = PSTOP1 - Partial Stop with both system and bus clocks disabled
 *  0x80 = PSTOP2 - Partial Stop with system clock disabled and bus clock enabled
 *  0xC0 = Reserved
 */
void enter_stop(unsigned char partial_stop_opt)
{
  volatile unsigned int dummyread;
  /* The PMPROT register may have already been written by init code
     If so then this next write is not done since  
     PMPROT is write once after RESET  
     this write-once bit allows the MCU to enter the
     normal STOP mode.
     If AVLP is already a 1, VLPS mode is entered instead of normal STOP*/
  SMC_PMPROT = 0;  

  /* Set the STOPM field to 0b000 for normal STOP mode */
  SMC_PMCTRL &= ~SMC_PMCTRL_STOPM_MASK;
  SMC_PMCTRL |=  SMC_PMCTRL_STOPM(0); 
  SMC_STOPCTRL &= ~SMC_STOPCTRL_PSTOPO_MASK;
  SMC_STOPCTRL |= partial_stop_opt;
  /*wait for write to complete to SMC before stopping core */  
  dummyread = SMC_PMCTRL;
  dummyread = dummyread + 1;
  
#ifdef CMSIS
  /* Set the SLEEPDEEP bit to enable deep sleep mode (STOP) */
  SCB_SCR |= SCB_SCR_SLEEPDEEP_MASK;
  __wfi();
#else
  stop();
#endif
}
Exemplo n.º 3
0
void enter_vlls0_nopor(void)
{
  volatile unsigned int dummyread;
  int i;
  /* Write to PMPROT to allow all possible power modes */
  SMC_PMPROT = SMC_PMPROT_AVLLS_MASK;   
  if ((SMC_PMSTAT & SMC_PMSTAT_PMSTAT_MASK)== 4){
     SMC_PMCTRL &= ~SMC_PMCTRL_RUNM_MASK;   // go back to RUN mode temporarily
     for (i=0;i<0xff;i++)
       {
       if ((SMC_PMSTAT & SMC_PMSTAT_PMSTAT_MASK)== 1)
    	 break;
       }
  }
  /* Set the STOPM field to 0b100 for VLLS0 mode */
  SMC_PMCTRL &= ~SMC_PMCTRL_STOPM_MASK; 
  SMC_PMCTRL |=  SMC_PMCTRL_STOPM(0x4); 
  /* set VLLSM = 00 * and PORPO = 1 */
  SMC_STOPCTRL &= ~SMC_STOPCTRL_VLLSM_MASK;
  SMC_STOPCTRL =  SMC_STOPCTRL_VLLSM(0) | SMC_STOPCTRL_PORPO_MASK;  
  /*wait for write to complete to SMC before stopping core */  
  dummyread = SMC_STOPCTRL;
  dummyread = dummyread + 1;
  /* Now execute the stop instruction to go into VLLS0 */
  #ifdef CMSIS
  /* Set the SLEEPDEEP bit to enable deep sleep mode (STOP) */
  SCB_SCR |= SCB_SCR_SLEEPDEEP_MASK;
  __wfi();
#else
  stop();
#endif
}
Exemplo n.º 4
0
/*
 * User's entry point.
 */
int main(void) {

    /* Initialisations  */
    printf("dma.c: main()\n");

    /* Installs IRQ handler */
#if defined(__TARGET_CPU_CORTEX_M3)
    install_handler((handler_t) irq_handler, (uint32_t *) 0x8);
#else   /* Assumes --cpu=6 or equivalent */
    setup_irq_stack();
    install_handler((handler_t) irq_handler, (uint32_t *) 0x18);
#endif

    /* Enables IRQ */
    __enable_irq();

    /* Initialize globals */
    end_transfer = 0;

    /* Single transfer */
    run();

#if defined(USE_WFI)
    /* Wait for interrupts */
    __wfi();
#endif
    exit(EXIT_SUCCESS);
    return EXIT_SUCCESS;
}
Exemplo n.º 5
0
void enter_vlls0(unsigned char PORPO_value )
{
  int i;
  /* Write to PMPROT to allow all possible power modes */
  SMC_PMPROT = SMC_PMPROT_AVLLS_MASK;   
  if ((SMC_PMSTAT & SMC_PMSTAT_PMSTAT_MASK)== 4){
     SMC_PMCTRL &= ~SMC_PMCTRL_RUNM_MASK;   // go back to RUN mode temporarily
     for (i=0;i<0xff;i++)
       {
       if ((SMC_PMSTAT & SMC_PMSTAT_PMSTAT_MASK)== 1)
    	 break;
       }
  }
  /* Set the STOPM field to 0b100 for VLLS0 mode */
  SMC_PMCTRL &= ~SMC_PMCTRL_STOPM_MASK; 
  SMC_PMCTRL |=  SMC_PMCTRL_STOPM(0x4); 
  /* set VLLSM = 0b00 */
  SMC_STOPCTRL &= ~SMC_STOPCTRL_VLLSM_MASK;
  SMC_STOPCTRL &= ~SMC_STOPCTRL_PORPO_MASK;
  SMC_STOPCTRL |=  (PORPO_value <<SMC_STOPCTRL_PORPO_SHIFT) 
                 | SMC_STOPCTRL_VLLSM(0);          
  /*wait for write to complete to SMC before stopping core */  
  PORPO_value = SMC_STOPCTRL;
  /* Now execute the stop instruction to go into VLLS0 */
  SCB_SCR |= SCB_SCR_SLEEPDEEP_MASK;
  __wfi();
//  #ifdef CMSIS
//  /* Set the SLEEPDEEP bit to enable deep sleep mode (STOP) */
//  SCB_SCR |= SCB_SCR_SLEEPDEEP_MASK;
//  __wfi();
//#else
//  stop();
//#endif
}
Exemplo n.º 6
0
/// \brief The idle demon is running when no other thread is ready to run
void os_idle_demon (void) {
 
  for (;;) {
    /* HERE: include optional user code to be executed when no thread runs.*/
		__wfi();
  }
}
Exemplo n.º 7
0
void __enter_sleep_mode()
{
	PWRCLK_ENABLE();
	dsb();
	__wfi();
	PWRCLK_DISABLE();
}
Exemplo n.º 8
0
void enter_vlls1(void)
{
  volatile unsigned int dummyread;
  SMC_PMPROT = SMC_PMPROT_AVLLS_MASK;
    
  /* Write to PMPROT to allow all possible power modes */
  /* Set the STOPM field to 0b100 for VLLS1 mode */
  SMC_PMCTRL &= ~SMC_PMCTRL_STOPM_MASK; 
  SMC_PMCTRL |=  SMC_PMCTRL_STOPM(0x4); 
  /* set VLLSM = 0b01 */
  SMC_STOPCTRL =  SMC_STOPCTRL_VLLSM(1);          
  /*wait for write to complete to SMC before stopping core */  
  dummyread = SMC_STOPCTRL;
  dummyread++;

  SCB_SCR |= SCB_SCR_SLEEPDEEP_MASK;
  __wfi();
  /* Now execute the stop instruction to go into VLLS1 */
//  #ifdef CMSIS
//  /* Set the SLEEPDEEP bit to enable deep sleep mode (STOP) */
//  SCB_SCR |= SCB_SCR_SLEEPDEEP_MASK;
//  __wfi();
//#else
//  stop();
//#endif
}
Exemplo n.º 9
0
/*----------------------------------------------------------------------------
  MAIN function
 *----------------------------------------------------------------------------*/
int main(void){
	
	//Initialise LEDs, buttons and interrupts
	init_RGB();
	init_switches();
	init_interrupts();
	done = 0;
	
	while(1){
		switch(done){
			
			//Toggle corresponding bits depending on which button was pressed
			
			case 1:
				toggle_r();
				break;
			case 2:
				toggle_g();
				break;
			case 3:
				toggle_b();
				break;
			case 4:
				toggle_all();
				break;
		}
		
		done = 0;
		delay(1);
		
		if (done == 0) //if done == 0, processor goes to sleep
			__wfi();
	}
}
Exemplo n.º 10
0
void enter_vlps(void)
{
  volatile unsigned int dummyread;
  /* The PMPROT register may have already been written by init code
     If so then this next write is not done since  
     PMPROT is write once after RESET 
     allows the MCU to enter the VLPR, VLPW, and VLPS modes.
     If AVLP is already writen to 0 
     Stop is entered instead of VLPS*/
  SMC_PMPROT = SMC_PMPROT_AVLP_MASK;           
  /* Set the STOPM field to 0b010 for VLPS mode */
  SMC_PMCTRL &= ~SMC_PMCTRL_STOPM_MASK; 
  SMC_PMCTRL |=  SMC_PMCTRL_STOPM(0x2); 
  /*wait for write to complete to SMC before stopping core */  
  dummyread = SMC_PMCTRL;
  dummyread = dummyread + 1;
  /* Now execute the stop instruction to go into VLPS */
  #ifdef CMSIS
  /* Set the SLEEPDEEP bit to enable deep sleep mode (STOP) */
  SCB_SCR |= SCB_SCR_SLEEPDEEP_MASK;
  __wfi();
#else
  stop();
#endif
}
Exemplo n.º 11
0
void sleep (void)
{
/* Clear the SLEEPDEEP bit to make sure we go into WAIT (sleep) 
 * mode instead of deep sleep.
 */
SCB_SCR &= ~SCB_SCR_SLEEPDEEP_MASK;

#if (defined(CMSIS))
  __wfi();
#elif (defined(KEIL))
	__wfi();
#else
  /* WFI instruction will start entry into WAIT mode */
  asm("WFI");
#endif 
}
Exemplo n.º 12
0
/* the voltage regulator disabled.
 * The 1.8 V domain is consequently powered off.
 * The PLL, the HSI oscillator and the HSE oscillator are also switched off.
 * SRAM and register contents are lost
 * except for registersin the Backup domain and Standby circuitry */
void __enter_standby_mode()
{
	SCB_SCR |= 4; /* Set SLEEPDEEP bit */
	PWR_CR |= 2; /* Set PDDS bit in Power Control register (PWR_CR) */
	PWR_CR |= 4; /* Clear WUF bit in Power Control register (PWR_CSR) */

	__wfi();

	/* wakeup latency: reset phase */
}
Exemplo n.º 13
0
/*---------------------------------------------------------------------------------------------------------*/
void DrvPMU_DeepSleep(void)
{
	UNLOCKREG();
	SYSCLK->PWRCON.STOP = 0;
	SYSCLK->PWRCON.STANDBY_PD = 0;
	SYSCLK->PWRCON.DEEP_PD = 0;
	SCB->SCR = SCB_SCR_SLEEPDEEP;
	LOCKREG();
	__wfi();
}
Exemplo n.º 14
0
void stop(void)
{
    /* Set the SLEEPDEEP bit to enable deep sleep mode (STOP) */
    SCB_SCR |= SCB_SCR_SLEEPDEEP_MASK;
    /* WFI instruction will start entry into STOP mode */
#ifndef KEIL
    asm("WFI");
#else
    __wfi();
#endif
}
Exemplo n.º 15
0
/*---------------------------------------------------------------------------------------------------------*/
void DrvPMU_StandbyPowerDown(void)
{
	UNLOCKREG();

	SCB->SCR = SCB_SCR_SLEEPDEEP;
	SYSCLK->PWRCON.STANDBY_PD = 1;
	SYSCLK->PWRCON.STOP = 0;
	SYSCLK->PWRCON.DEEP_PD = 0;
	LOCKREG();
	__wfi();
}
/*********************************************************************
*
*       Idle loop  (OS_Idle)
*
*       Please note:
*       This is basically the "core" of the idle loop.
*       This core loop can be changed, but:
*       The idle loop does not have a stack of its own, therefore no
*       functionality should be implemented that relies on the stack
*       to be preserved. However, a simple program loop can be programmed
*       (like toggeling an output or incrementing a counter)
*/
void OS_Idle(void) {     // Idle loop: No task is ready to execute
  while (1) {
    #if ((OS_USE_JLINKMEM == 0) && (DEBUG == 0))     // Enter CPU halt mode when not in DEBUG build and J-Link communication not used
      #ifdef __ICCARM__  // IAR
        __asm(" wfi");
      #endif
      #ifdef __CC_ARM    // KEIL
        __wfi();
      #endif
    #endif
  }
}
void stop (void)
{
	/* Set the SLEEPDEEP bit to enable deep sleep mode (STOP) */
	SCB_SCR |= SCB_SCR_SLEEPDEEP_MASK;	

	/* WFI instruction will start entry into STOP mode */
  #if defined(__ARMCC_VERSION)
    __wfi();
  #else
    asm("WFI");
  #endif	
}
Exemplo n.º 18
0
void deepsleep (void)
{
  /* Set the SLEEPDEEP bit to enable deep sleep mode (STOP) */
  SCB_SCR |= SCB_SCR_SLEEPDEEP_MASK;
__wfi();
//#ifdef CMSIS
//  __wfi();
//#else
//  /* WFI instruction will start entry into STOP mode */
//  asm("WFI");
//#endif
}
Exemplo n.º 19
0
/* WAIT mode entry routine. Puts the processor into wait mode.
 * In this mode the core clock is disabled (no code executing), but 
 * bus clocks are enabled (peripheral modules are operational). 
 *
 * Mode transitions:
 * RUN -> WAIT
 * VLPR -> VLPW
 *
 * This function can be used to enter normal wait mode or VLPW
 * mode. If you are executing in normal run mode when calling this
 * function, then you will enter normal wait mode. If you are in VLPR
 * mode when calling this function, then you will enter VLPW mode instead.
 *
 * NOTE: Some modules include a programmable option to disable them in 
 * wait mode. If those modules are programmed to disable in wait mode, 
 * they will not be able to generate interrupts to wake up the core.
 *
 * WAIT mode is exited using any enabled interrupt or RESET, so no
 * exit_wait routine is needed. 
 * 
 * 
 * Parameters:
 * none
 */
void enter_wait(void)
{
#ifdef CMSIS
    /* Clear the SLEEPDEEP bit to make sure we go into WAIT (sleep) mode instead
     * of deep sleep.
     */
	SCB_SCR &= ~SCB_SCR_SLEEPDEEP_MASK;
        
    __wfi();
#else
    wait();
#endif
}
Exemplo n.º 20
0
void wait(void)
{
    /* Clear the SLEEPDEEP bit to make sure we go into WAIT (sleep) mode instead
     * of deep sleep.
     */
    SCB_SCR &= ~SCB_SCR_SLEEPDEEP_MASK;
    /* WFI instruction will start entry into WAIT mode */
#ifndef KEIL
    asm("WFI");
#else
    __wfi();
#endif
}
Exemplo n.º 21
0
void sleep (void)
{
/* Clear the SLEEPDEEP bit to make sure we go into WAIT (sleep) 
 * mode instead of deep sleep.
 */
SCB_SCR &= ~SCB_SCR_SLEEPDEEP_MASK;
 __wfi();
//#ifdef CMSIS
//  __wfi();
//#else
//  /* WFI instruction will start entry into WAIT mode */
//  asm("WFI");
//#endif 
}
void Entry_Deep_PD(void)
{
	// step1
	LPC_PMU->PCON |= (1<<1);	// DPDEN=1; 选择deep_power_down模式
	// step2 (这一步可以保存需要保存的数据,一共有4个32位的通用寄存器可用)
// 	LPC_PMU->GPREG0 = ;
// 	LPC_PMU->GPREG1 = ;
// 	LPC_PMU->GPREG2 = ;
// 	LPC_PMU->GPREG3 = ;
	// step3
	SCB->SCR |= (1<<2);      // SLEEPDEEP=1;
	// step4
	LPC_SYSCON->PDRUNCFG &= ~(0X3); // 确保IRCOUT_PD和IRC_PD 为0
	// setp5
  __wfi();				          // 写wfi指令进入低功耗模式
}
Exemplo n.º 23
0
/* WAIT mode entry routine. Puts the processor into wait mode.
 * In this mode the core clock is disabled (no code executing), but 
 * bus clocks are enabled (peripheral modules are operational). 
 *
 * Mode transitions:
 * RUN -> WAIT
 * VLPR -> VLPW
 *
 * This function can be used to enter normal wait mode or VLPW
 * mode. If you are executing in normal run mode when calling this
 * function, then you will enter normal wait mode. If you are in VLPR
 * mode when calling this function, then you will enter VLPW mode instead.
 *
 * NOTE: Some modules include a programmable option to disable them in 
 * wait mode. If those modules are programmed to disable in wait mode, 
 * they will not be able to generate interrupts to wake up the core.
 *
 * WAIT mode is exited using any enabled interrupt or RESET, so no
 * exit_wait routine is needed. 
 * 
 * 
 * Parameters:
 * none
 */
void enter_wait(void)
{
		SCB_SCR &= ~SCB_SCR_SLEEPDEEP_MASK;
        
    __wfi();
//#ifdef CMSIS
//    /* Clear the SLEEPDEEP bit to make sure we go into WAIT (sleep) mode instead
//     * of deep sleep.
//     */
//	SCB_SCR &= ~SCB_SCR_SLEEPDEEP_MASK;
//        
//    __wfi();
//#else
//    wait();
//#endif
}
Exemplo n.º 24
0
//sample the mic with adc14 and set if bad guy is here
void sample_mic()
{

    speed_analysis[1]++;
    if(!alarm_status){

        return;
    }

	  ADC14CTL0 |= ADC14ENC |ADC14SC ;    // Start sampling/conversion
	  __wfi();                            // delay for adc

	  if(ADC14MEM0 > 9000){
        badguy_here = 1;
	  }

}
Exemplo n.º 25
0
void enter_lls(void)
{
  volatile unsigned int dummyread;
  /* Write to PMPROT to allow LLS power modes this write-once 
     bit allows the MCU to enter the LLS low power mode*/
  SMC_PMPROT = SMC_PMPROT_ALLS_MASK;   
  /* Set the STOPM field to 0b011 for LLS mode  */
  SMC_PMCTRL &= ~SMC_PMCTRL_STOPM_MASK; 
  SMC_PMCTRL |=  SMC_PMCTRL_STOPM(0x3); 
  /*wait for write to complete to SMC before stopping core */  
  dummyread = SMC_PMCTRL;
  dummyread = dummyread + 1;
  /* Now execute the stop instruction to go into LLS */
  #ifdef CMSIS
  /* Set the SLEEPDEEP bit to enable deep sleep mode (STOP) */
  SCB_SCR |= SCB_SCR_SLEEPDEEP_MASK;
  __wfi();
#else
  stop();
#endif
}
Exemplo n.º 26
0
/*---------------------------------------------------------------------------------------------------------*/
void DrvPMU_DeepPowerDown(uint32_t u32DPDWakeupMode, uint32_t u32TimerSel)
{
	UNLOCKREG();

	SCB->SCR = SCB_SCR_SLEEPDEEP;
	SYSCLK->PWRCON.DEEP_PD = 1;
	SYSCLK->PWRCON.STOP = 0;
	SYSCLK->PWRCON.STANDBY_PD = 0;

	SYSCLK->PWRCON.PIN_ENB= 0;
	SYSCLK->PWRCON.OSC10K_ENB= 0;
	SYSCLK->PWRCON.TIMER_SEL = u32TimerSel;

	switch(u32DPDWakeupMode)
	{
		case DPDWAKEUPMODE_PINOSC10KWAKEUP:

		     break;

		case DPDWAKEUPMODE_PINWAKEUP:
		     SYSCLK->PWRCON.OSC10K_ENB = 1; //Disable OSC10K Wakeup
		     break;

		case DPDWAKEUPMODE_OSC10KWAKEUP:
			 SYSCLK->PWRCON.PIN_ENB = 1;    //Disable PIN Wakeup

		     break;

		case DPDWAKEUPMODE_ONLYPORWAKEUP:
		     SYSCLK->PWRCON.PIN_ENB = 1;    //Disable PIN Wakeup
		     SYSCLK->PWRCON.OSC10K_ENB = 1; //Disable OSC10K Wakeup
		     break;
    }

	LOCKREG();

	__wfi();
}
Exemplo n.º 27
0
/* all clocks in the 1.8 V domain are stopped,
 * the PLL, the HSI and the HSE RC oscillators are disabled.
 * SRAM and register contents are preserved.
 * all I/O pins keep the same state as in the Run mode.
 * ADC, DAC, WDG, RTC, LSI_RC, and LSE_OSC can consume power. */
void __enter_stop_mode()
{
	unsigned int irqflag;

	irq_save(irqflag);
	local_irq_disable();

	PWRCLK_ENABLE();
	SCB_SCR |= 4; /* Set SLEEPDEEP bit */
	/* Clear PDDS bit in Power Control register (PWR_CR) */
	PWR_CR |= 1; /* configure LPDS bit in PWR_CR */

	stop_scheduler();
	dsb();
	__wfi();
	clock_init();
	run_scheduler();

	SCB_SCR &= ~4;
	PWR_CR &= ~1;
	PWRCLK_DISABLE();

	irq_restore(irqflag);

	/* wakeup latency:
	 * HSI RC wakeup time + regulator wakeup time from Low-power mode */

	/* Note from reference manual:
	 * If the application needs to disable the external clock before
	 * entering Stop mode, the HSEON bit must first be disabled and the
	 * system clock switched to HSI. Otherwise, if the HSEON bit remains
	 * enabled and the external clock (external oscillator) is removed when
	 * entering Stop mode, the clock security system (CSS) feature must be
	 * enabled to detect any external oscillator failure and avoid a
	 * malfunction behavior when entering stop mode. */
	//BITBAND(&RCC_CR, CSSON, ON);
}
Exemplo n.º 28
0
void run_periodic_tasks(void)
{
	struct task *best;
	unsigned long state;
	
	puts("\n--------------Starting tasks--------------\n\n");
	
	for (;;) {
		state = globalreleases;
		best = select_best_task(); /* Get the highest priority job to execute */
		
		/* If globalreleases differs, a new task was released in the meantime.
		 * We cannot execute the selected job because it is possible that the
		 * new job has an higher priority */
		if (best != NULL && state == globalreleases) {
			best->job(best->arg); /* Execute the job */
			best->released--;
		}
		else if (best == NULL)
			/* If nothing to do put the CPU into
			 * low power state until next timer interrupt */
			__wfi();
	}
}
Exemplo n.º 29
0
	__weak void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime )
	{
	uint32_t ulReloadValue, ulCompleteTickPeriods, ulCompletedSysTickDecrements, ulSysTickCTRL;
	TickType_t xModifiableIdleTime;

		/* Make sure the SysTick reload value does not overflow the counter. */
		if( xExpectedIdleTime > xMaximumPossibleSuppressedTicks )
		{
			xExpectedIdleTime = xMaximumPossibleSuppressedTicks;
		}

		/* Stop the SysTick momentarily.  The time the SysTick is stopped for
		is accounted for as best it can be, but using the tickless mode will
		inevitably result in some tiny drift of the time maintained by the
		kernel with respect to calendar time. */
		portNVIC_SYSTICK_CTRL_REG &= ~portNVIC_SYSTICK_ENABLE_BIT;

		/* Calculate the reload value required to wait xExpectedIdleTime
		tick periods.  -1 is used because this code will execute part way
		through one of the tick periods. */
		ulReloadValue = portNVIC_SYSTICK_CURRENT_VALUE_REG + ( ulTimerCountsForOneTick * ( xExpectedIdleTime - 1UL ) );
		if( ulReloadValue > ulStoppedTimerCompensation )
		{
			ulReloadValue -= ulStoppedTimerCompensation;
		}

		/* Enter a critical section but don't use the taskENTER_CRITICAL()
		method as that will mask interrupts that should exit sleep mode. */
		__disable_irq();

		/* If a context switch is pending or a task is waiting for the scheduler
		to be unsuspended then abandon the low power entry. */
		if( eTaskConfirmSleepModeStatus() == eAbortSleep )
		{
			/* Restart from whatever is left in the count register to complete
			this tick period. */
			portNVIC_SYSTICK_LOAD_REG = portNVIC_SYSTICK_CURRENT_VALUE_REG;

			/* Restart SysTick. */
			portNVIC_SYSTICK_CTRL_REG |= portNVIC_SYSTICK_ENABLE_BIT;

			/* Reset the reload register to the value required for normal tick
			periods. */
			portNVIC_SYSTICK_LOAD_REG = ulTimerCountsForOneTick - 1UL;

			/* Re-enable interrupts - see comments above __disable_irq() call
			above. */
			__enable_irq();
		}
		else
		{
			/* Set the new reload value. */
			portNVIC_SYSTICK_LOAD_REG = ulReloadValue;

			/* Clear the SysTick count flag and set the count value back to
			zero. */
			portNVIC_SYSTICK_CURRENT_VALUE_REG = 0UL;

			/* Restart SysTick. */
			portNVIC_SYSTICK_CTRL_REG |= portNVIC_SYSTICK_ENABLE_BIT;

			/* Sleep until something happens.  configPRE_SLEEP_PROCESSING() can
			set its parameter to 0 to indicate that its implementation contains
			its own wait for interrupt or wait for event instruction, and so wfi
			should not be executed again.  However, the original expected idle
			time variable must remain unmodified, so a copy is taken. */
			xModifiableIdleTime = xExpectedIdleTime;
			configPRE_SLEEP_PROCESSING( xModifiableIdleTime );
			if( xModifiableIdleTime > 0 )
			{
				__dsb( portSY_FULL_READ_WRITE );
				__wfi();
				__isb( portSY_FULL_READ_WRITE );
			}
			configPOST_SLEEP_PROCESSING( xExpectedIdleTime );

			/* Stop SysTick.  Again, the time the SysTick is stopped for is
			accounted for as best it can be, but using the tickless mode will
			inevitably result in some tiny drift of the time maintained by the
			kernel with respect to calendar time. */
			ulSysTickCTRL = portNVIC_SYSTICK_CTRL_REG;
			portNVIC_SYSTICK_CTRL_REG = ( ulSysTickCTRL & ~portNVIC_SYSTICK_ENABLE_BIT );

			/* Re-enable interrupts - see comments above __disable_irq() call
			above. */
			__enable_irq();

			if( ( ulSysTickCTRL & portNVIC_SYSTICK_COUNT_FLAG_BIT ) != 0 )
			{
				uint32_t ulCalculatedLoadValue;

				/* The tick interrupt has already executed, and the SysTick
				count reloaded with ulReloadValue.  Reset the
				portNVIC_SYSTICK_LOAD_REG with whatever remains of this tick
				period. */
				ulCalculatedLoadValue = ( ulTimerCountsForOneTick - 1UL ) - ( ulReloadValue - portNVIC_SYSTICK_CURRENT_VALUE_REG );

				/* Don't allow a tiny value, or values that have somehow
				underflowed because the post sleep hook did something
				that took too long. */
				if( ( ulCalculatedLoadValue < ulStoppedTimerCompensation ) || ( ulCalculatedLoadValue > ulTimerCountsForOneTick ) )
				{
					ulCalculatedLoadValue = ( ulTimerCountsForOneTick - 1UL );
				}

				portNVIC_SYSTICK_LOAD_REG = ulCalculatedLoadValue;

				/* The tick interrupt handler will already have pended the tick
				processing in the kernel.  As the pending tick will be
				processed as soon as this function exits, the tick value
				maintained by the tick is stepped forward by one less than the
				time spent waiting. */
				ulCompleteTickPeriods = xExpectedIdleTime - 1UL;
			}
			else
			{
				/* Something other than the tick interrupt ended the sleep.
				Work out how long the sleep lasted rounded to complete tick
				periods (not the ulReload value which accounted for part
				ticks). */
				ulCompletedSysTickDecrements = ( xExpectedIdleTime * ulTimerCountsForOneTick ) - portNVIC_SYSTICK_CURRENT_VALUE_REG;

				/* How many complete tick periods passed while the processor
				was waiting? */
				ulCompleteTickPeriods = ulCompletedSysTickDecrements / ulTimerCountsForOneTick;

				/* The reload value is set to whatever fraction of a single tick
				period remains. */
				portNVIC_SYSTICK_LOAD_REG = ( ( ulCompleteTickPeriods + 1 ) * ulTimerCountsForOneTick ) - ulCompletedSysTickDecrements;
			}

			/* Restart SysTick so it runs from portNVIC_SYSTICK_LOAD_REG
			again, then set portNVIC_SYSTICK_LOAD_REG back to its standard
			value.  The critical section is used to ensure the tick interrupt
			can only execute once in the case that the reload register is near
			zero. */
			portNVIC_SYSTICK_CURRENT_VALUE_REG = 0UL;
			portENTER_CRITICAL();
			{
				portNVIC_SYSTICK_CTRL_REG |= portNVIC_SYSTICK_ENABLE_BIT;
				vTaskStepTick( ulCompleteTickPeriods );
				portNVIC_SYSTICK_LOAD_REG = ulTimerCountsForOneTick - 1UL;
			}
			portEXIT_CRITICAL();
		}
	}
Exemplo n.º 30
0
int main(void)
{
	int i;
	
	uint16_t sensor_sample_count;
	struct sensor_s sensor;
	struct angle_s angle;
	
	uint16_t radio_frame_count;
	struct radio_raw_s radio_raw;
	struct radio_s radio;
	float radio_pitch_smooth;
	float radio_roll_smooth;
	
	float error_pitch;
	float error_roll;
	float error_yaw;
	float error_pitch_z;
	float error_roll_z;
	float error_yaw_z;
	
	float p_pitch;
	float i_pitch;
	float d_pitch;
	float p_roll;
	float i_roll;
	float d_roll;
	
	float pitch_p_term;
	float pitch_i_term;
	float pitch_d_term;
	float roll_p_term;
	float roll_i_term;
	float roll_d_term;
	float yaw_p_term;
	float yaw_i_term;
	float yaw_d_term;
	
	float pitch;
	float roll;
	float yaw;
	
	float motor[4];
	int32_t motor_clip[4];
	uint32_t motor_raw[4];
	
	uint16_t vbat_sample_count;
	
	int32_t t1;
	int32_t t2;
	_Bool flag_acro_z;
	_Bool error;
	uint16_t sensor_sample_count1;
	
	host_buffer_tx_t host_buffer_tx;
	
	/* Variable initialisation -----------------------------------------------------*/
	
	flag_sensor = 0;
	flag_radio = 0;
	flag_vbat = 0;
	flag_rf = 0;
	flag_host = 0;
	flag_sensor_host_read = 0;
	flag_rf_host_read = 0;
	flag_timeout_sensor = 0;
	flag_timeout_radio = 0;
	flag_armed = 0;
	flag_acro = 1;
	flag_acro_z = 0;

	flag_beep_user = 0;
	flag_beep_radio = 0;
	flag_beep_sensor = 0;
	flag_beep_host = 0;
	flag_beep_vbat = 0;
	
	sensor_sample_count = 0;
	sensor_error_count = 0;
	angle.pitch = 0;
	angle.roll = 0;
	
	radio_frame_count = 0;
	radio_error_count = 0;
	radio_pitch_smooth = 0;
	radio_roll_smooth = 0;
	
	pitch_i_term = 0;
	roll_i_term = 0;
	yaw_i_term = 0;
	
	vbat_sample_count = 0;
	
	rf_error_count = 0;
	
	sensor_sample_count1 = 0;
	
	/* Setup -----------------------------------------------------*/
	
	board_init(); // BOARD_DEPENDENT
	SysTick->CTRL &= ~SysTick_CTRL_TICKINT_Msk; // Disable Systick interrupt, not needed anymore (but can still use COUNTFLAG)
	reg_init();
	
	/* Loop ----------------------------------------------------------------------------
	-----------------------------------------------------------------------------------*/
	
	while (1)
	{
		// Processing time
		t1 = (int32_t)get_timer_process();
		
		/* Process radio commands -----------------------------------------------------*/
		
		if (flag_radio)
		{
			flag_radio = 0;
			
			// Decode radio commands
			error = radio_decode(&radio_frame, &radio_raw, &radio);
			if (error) {
				radio_error_count++;
				radio_synch();
			}
			else
			{
				reset_timeout_radio();
				flag_beep_radio = 0; // Stop beeping
				radio_frame_count++;
			
				// Arm procedure
				if (radio.aux[0] < 0.33f)
					flag_armed = 0;
				else if (!flag_armed && (radio.aux[0] > 0.33f) && ((radio.throttle < 0.01f)))
					flag_armed = 1;
				if (radio.aux[0] < 0.66f)
					flag_acro = 1;
				else
					flag_acro = 0;
				
				// Expo and smooth
				radio_expo(&radio, flag_acro);
				
				// Beep if requested
				if (radio.aux[1] > 0.33f)
					flag_beep_user = 1;
				else
					flag_beep_user = 0;
				
				// Send data to host
				if ((REG_DEBUG__CASE > 0) && ((radio_frame_count & REG_DEBUG__MASK) == 0)) {
					if (REG_DEBUG__CASE == 4)
						host_send((uint8_t*)&radio_raw, sizeof(radio_raw));
					else if (REG_DEBUG__CASE == 5)
						host_send((uint8_t*)&radio, sizeof(radio));
				}
				
				// Toggle LED at rate of Radio flag
				if ((radio_frame_count & 0x3F) == 0)
					toggle_led_radio();
			}
		}
		
		/* Process sensors -----------------------------------------------------------------------*/
		
		if (flag_sensor)
		{
			flag_sensor = 0;
			
			sensor_sample_count++;
			flag_beep_sensor = 0; // Disable beeping
			
			// Record sensor transaction time
			t2 = (int32_t)timer_sensor[1] - (int32_t)timer_sensor[0];
			if (t2 < 0)
				t2 += 0xFFFF;
			if ((REG_CTRL__TIME_MAXHOLD == 0) || (((uint16_t)t2 > time_sensor) && REG_CTRL__TIME_MAXHOLD))
				time_sensor = (uint16_t)t2;
			
			// Recovery time before activating yaw agnle transfer
			if (flag_acro != flag_acro_z)
				sensor_sample_count1 = 0;
			else if (sensor_sample_count1 < RECOVERY_TIME)
				sensor_sample_count1++;
			
			// Procees sensor data
			mpu_process_samples(&sensor_raw, &sensor);
			
			// Estimate angle
			angle_estimate(&sensor, &angle, (sensor_sample_count1 == RECOVERY_TIME));
			
			// Smooth pitch and roll commands in angle mode
			if (!flag_acro) {
				radio_pitch_smooth += filter_alpha_radio * radio.pitch - filter_alpha_radio * radio_pitch_smooth;
				radio_roll_smooth  += filter_alpha_radio * radio.roll  - filter_alpha_radio * radio_roll_smooth;
			}
			
			// Previous error
			error_pitch_z = error_pitch;
			error_roll_z = error_roll;
			error_yaw_z = error_yaw;
			
			// Current error
			if (flag_acro) {
				error_pitch = sensor.gyro_x - radio.pitch * (float)REG_RATE__PITCH_ROLL;
				error_roll = sensor.gyro_y - radio.roll * (float)REG_RATE__PITCH_ROLL;
			}
			else {
				error_pitch = angle.pitch - radio_pitch_smooth * (float)REG_RATE__ANGLE;
				error_roll = angle.roll - radio_roll_smooth * (float)REG_RATE__ANGLE;
			}
			error_yaw = sensor.gyro_z - radio.yaw * (float)REG_RATE__YAW;
			
			// Switch PID coefficients for acro
			if (flag_acro != flag_acro_z) {
				if (flag_acro) {
					p_pitch = REG_P_PITCH;
					i_pitch = REG_I_PITCH;
					d_pitch = REG_D_PITCH;
					p_roll  = REG_P_ROLL;
					i_roll  = REG_I_ROLL;
					d_roll  = REG_D_ROLL;
				}
				else {
					p_pitch = REG_P_PITCH_ANGLE;
					i_pitch = REG_I_PITCH_ANGLE;
					d_pitch = REG_D_PITCH_ANGLE;
					p_roll  = REG_P_ROLL_ANGLE;
					i_roll  = REG_I_ROLL_ANGLE;
					d_roll  = REG_D_ROLL_ANGLE;
				}
			}
			
			// P,I and D
			pitch_p_term = error_pitch * p_pitch;
			roll_p_term = error_roll * p_roll;
			yaw_p_term = error_yaw * REG_P_ROLL;
			
			if ((!flag_armed && (REG_CTRL__ARM_TEST == 0)) || (flag_acro != flag_acro_z)) {
				pitch_i_term = 0;
				roll_i_term = 0;
			}
			else {
				pitch_i_term += error_pitch * i_pitch;
				roll_i_term += error_roll * i_roll;
			}
			if (!flag_armed && (REG_CTRL__ARM_TEST == 0))
				yaw_i_term = 0;
			else
				yaw_i_term += error_yaw * REG_I_YAW;
			
			pitch_d_term = (error_pitch - error_pitch_z) * d_pitch;
			roll_d_term = (error_roll - error_roll_z) * d_roll;
			yaw_d_term = (error_yaw - error_yaw_z) * REG_D_YAW;
			
			flag_acro_z = flag_acro;
			
			// Clip I
			if      (pitch_i_term < -I_MAX) pitch_i_term = -I_MAX;
			else if (pitch_i_term >  I_MAX) pitch_i_term =  I_MAX;
			if      (roll_i_term  < -I_MAX) roll_i_term  = -I_MAX;
			else if (roll_i_term  >  I_MAX) roll_i_term  =  I_MAX;
			if      (yaw_i_term   < -I_MAX) yaw_i_term   = -I_MAX;
			else if (yaw_i_term   >  I_MAX) yaw_i_term   =  I_MAX;
			
			// P+I+D
			pitch = pitch_p_term + pitch_i_term + pitch_d_term;
			roll = roll_p_term + roll_i_term + roll_d_term;
			yaw = yaw_p_term + yaw_i_term + yaw_d_term;
			
			// Clip P+I+D
			if      (pitch < -PID_MAX) pitch = -PID_MAX;
			else if (pitch >  PID_MAX) pitch =  PID_MAX;
			if      (roll  < -PID_MAX) roll  = -PID_MAX;
			else if (roll  >  PID_MAX) roll  =  PID_MAX;
			if      (yaw   < -PID_MAX) yaw   = -PID_MAX;
			else if (yaw   >  PID_MAX) yaw   =  PID_MAX;
			
			// Desactivate throttle when arm test
			if (REG_CTRL__ARM_TEST > 0)
				radio.throttle = 0;
			
			// Motor matrix
			motor[0] = radio.throttle * (float)REG_MOTOR__RANGE + roll + pitch - yaw;
			motor[1] = radio.throttle * (float)REG_MOTOR__RANGE + roll - pitch + yaw;
			motor[2] = radio.throttle * (float)REG_MOTOR__RANGE - roll - pitch - yaw;
			motor[3] = radio.throttle * (float)REG_MOTOR__RANGE - roll + pitch + yaw;
			
			// Offset and clip motor value
			for (i=0; i<4; i++) {
				motor_clip[i] = (int32_t)motor[i] + (int32_t)REG_MOTOR__ARMED;
				
				if (motor_clip[i] < (int32_t)REG_MOTOR__START)
					motor_clip[i] = (int32_t)REG_MOTOR__START;
				else if (motor_clip[i] > (int32_t)MOTOR_MAX)
					motor_clip[i] = (int32_t)MOTOR_MAX;
			}
			
			// Motor command
			for (i=0; i<4; i++) {
				if (REG_MOTOR_TEST__SELECT & (1 << i))
					motor_raw[i] = (uint32_t)REG_MOTOR_TEST__VALUE;
				else if (flag_armed || (REG_CTRL__ARM_TEST > 0))
					motor_raw[i] = (uint32_t)motor_clip[i];
				else
					motor_raw[i] = 0;
			}
			set_motors(motor_raw);
			
			// Send data to host
			if ((REG_DEBUG__CASE > 0) && ((sensor_sample_count & REG_DEBUG__MASK) == 0)) {
				if (REG_DEBUG__CASE == 1) 
					host_send((uint8_t*)&sensor_raw.bytes[1], sizeof(sensor_raw)-1);
				else if (REG_DEBUG__CASE == 2)
					host_send((uint8_t*)&sensor, sizeof(sensor));
				else if (REG_DEBUG__CASE == 3)
					host_send((uint8_t*)&angle, sizeof(angle));
				else if (REG_DEBUG__CASE == 6) {
					host_buffer_tx.f[0] = pitch;
					host_buffer_tx.f[1] = roll;
					host_buffer_tx.f[2] = yaw;
					host_send(host_buffer_tx.u8, 3*4);
				}
				else if (REG_DEBUG__CASE == 7)
					host_send((uint8_t*)&motor_raw, sizeof(motor_raw));
			}
			
			// Toggle LED at rate of sensor flag
			if ((sensor_sample_count & 0x01FF) == 0)
				toggle_led_sensor();
		}
		
		/* VBAT ---------------------------------------------------------------------*/
		
		if (flag_vbat)
		{
			flag_vbat = 0;
			
			vbat_sample_count++;
			
			REG_VBAT += filter_alpha_vbat * get_vbat() - filter_alpha_vbat * REG_VBAT;
			
			// Send VBAT to host
			if ((REG_DEBUG__CASE == 8) && ((vbat_sample_count & REG_DEBUG__MASK) == 0))
				host_send((uint8_t*)&REG_VBAT,4);
			
			// Beep if VBAT too low
			if ((REG_VBAT < REG_VBAT_MIN) && (REG_VBAT > 8.0f))
				flag_beep_vbat = 1;
			else
				flag_beep_vbat = 0;
		}
		
		/* Host requests ------------------------------------------------------------------*/
		
		if (flag_host)
		{
			flag_host = 0;
			reg_access(&host_buffer_rx);
		}
		
		/* Handle timeout -----------------------------------------------------------------------*/
		
		if (flag_timeout_sensor) {
			flag_timeout_sensor = 0;
			for (i=0; i<4; i++)
				motor_raw[i] = 0;
			set_motors(motor_raw);
			flag_beep_sensor = 1;
		}
		if (flag_timeout_radio) {
			flag_timeout_radio = 0;
			flag_armed = 0;
			flag_beep_radio = 1;
		}
		
		/*------------------------------------------------------------------*/
		
		// Record processing time
		t1 = (int32_t)get_timer_process() - t1;
		if (t1 < 0)
			t1 += 0xFFFF;
		if ((REG_CTRL__TIME_MAXHOLD == 0) || (((uint16_t)t1 > time_process) && REG_CTRL__TIME_MAXHOLD))
			time_process = (uint16_t)t1;
		
		// Wait for interrupts if all flags are processed
		if (!flag_radio && !flag_sensor && !flag_vbat && !flag_host && !flag_timeout_sensor && !flag_timeout_radio)
			__wfi();
	}
}