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
}
/* 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
}
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
}
/*
* 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;
}
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
}
/// \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();
}
}
void __enter_sleep_mode()
{
PWRCLK_ENABLE();
dsb();
__wfi();
PWRCLK_DISABLE();
}
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
}
/*----------------------------------------------------------------------------
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();
}
}
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
}
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
}
/* 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 */
}
/*---------------------------------------------------------------------------------------------------------*/
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();
}
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
}
/*---------------------------------------------------------------------------------------------------------*/
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
}
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
}
/* 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
}
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
}
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指令进入低功耗模式
}
/* 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
}
//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;
}
}
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
}
/*---------------------------------------------------------------------------------------------------------*/
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();
}
/* 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);
}
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();
}
}
__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();
}
}
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*)®_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();
}
}