/* * API to initialize the INTERRUPT APP */ INTERRUPT_STATUS_t INTERRUPT_Init(const INTERRUPT_t *const handler) { XMC_ASSERT("INTERRUPT_Init:HandlePtr NULL", (handler != NULL)); #if(UC_FAMILY == XMC4) NVIC_SetPriority(handler->node, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), handler->priority, handler->subpriority)); if (handler->enable_at_init == true) { INTERRUPT_Enable(handler); } #endif #if(UC_FAMILY == XMC1) NVIC_SetPriority(handler->node, handler->priority); #if (UC_SERIES == XMC14) XMC_SCU_SetInterruptControl((uint8_t)handler->node, (XMC_SCU_IRQCTRL_t)((handler->node << 8) | handler->irqctrl)); #endif /* Enable the interrupt if enable_at_init is enabled */ if (handler->enable_at_init == true) { INTERRUPT_Enable(handler); } #endif return (INTERRUPT_STATUS_SUCCESS); }
/* * API to initialize the PIN_INTERRUPT APP ERU Event Trigger Logic, Output Gating Unit Hardware initialization * and NVIC node configuration. */ PIN_INTERRUPT_STATUS_t PIN_INTERRUPT_Init(const PIN_INTERRUPT_t *const handle) { XMC_ASSERT("PIN_INTERRUPT_Init: PIN_INTERRUPT APP handle function pointer uninitialized", (handle != NULL)); /* Initializes input pin characteristics */ XMC_GPIO_Init(handle->port, handle->pin, &handle->gpio_config); /* ERU Event Trigger Logic Hardware initialization based on UI */ XMC_ERU_ETL_Init(handle->eru, handle->etl, &handle->etl_config); /* OGU is configured to generate event on configured trigger edge */ XMC_ERU_OGU_SetServiceRequestMode(handle->eru, handle->ogu, XMC_ERU_OGU_SERVICE_REQUEST_ON_TRIGGER); #if (UC_FAMILY == XMC1) /* Configure NVIC node and priority */ NVIC_SetPriority((IRQn_Type)handle->IRQn, handle->irq_priority); #else /* Configure NVIC node, priority and subpriority */ NVIC_SetPriority((IRQn_Type)handle->IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), handle->irq_priority, handle->irq_subpriority)); #endif #if (UC_SERIES == XMC14) XMC_SCU_SetInterruptControl((IRQn_Type)handle->IRQn, (XMC_SCU_IRQCTRL_t)handle->irqctrl); #endif if (true == handle->enable_at_init) { /* Clear pending interrupt before enabling it */ NVIC_ClearPendingIRQ((IRQn_Type)handle->IRQn); /* Enable NVIC node */ NVIC_EnableIRQ((IRQn_Type)handle->IRQn); } return (PIN_INTERRUPT_STATUS_SUCCESS); }
/* * Initialization function which initializes the App internal data * structures to default values. */ void SYSTM002_Init( void) { volatile uint32_t Timer_Status = SYSTM002_TIMER_CONFIGURATION_SUCCESS; /* <<<DD_SYSTM002 _API_1>>> */ /** Initialize the header of the list */ TimerList = NULL; /** Initialize timer tracker */ Timer_Status = SysTick_Config((uint32_t)(SYSTM002_SysTickMicroSec( \ SYSTM002_SYSTICK_INTERVAL))); if(SYSTM002_TIMER_CONFIGURATION_FAILURE == Timer_Status) { DBG002_INFO(APP_GID, DBG002_MESSAGEID_LITERAL, \ sizeof("SYSTM002_Init: Timer reload value out of range"), \ "SYSTM002_Init: Timer reload value out of range"); } else { /** setting of Priority and subpriority value for XMC4000 devices */ #if ((__TARGET_DEVICE__ == XMC44) || (__TARGET_DEVICE__ == XMC42) || \ (__TARGET_DEVICE__ == XMC41) || (__TARGET_DEVICE__ == XMC45)) NVIC_SetPriority(SysTick_IRQn, NVIC_EncodePriority( \ NVIC_GetPriorityGrouping(),SYSTM002_PRIORITY,SYSTM002_SUBPRIORITY)); /** setting of Priority value for XMC1000 devices */ #elif ((__TARGET_DEVICE__ == XMC11) || (__TARGET_DEVICE__ == XMC12) \ || (__TARGET_DEVICE__ == XMC13)) NVIC_SetPriority(SysTick_IRQn, SYSTM002_PRIORITY); #endif TimerTracker = 0UL; } }
/* Function to configure SCU Interrupts based on user configuration. * */ void NVIC_SCU001_Init() { FUNCTION_ENTRY(GID_NVIC_SCU001,NVIC_SCU001_FUNC_ENTRY); NVIC_SetPriority(64, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),63,0)); /* Enable Interrupt */ NVIC_EnableIRQ(64); FUNCTION_EXIT(GID_NVIC_SCU001,NVIC_SCU001_FUNC_EXIT); }
/** Function to initialize the NVIC node parameters based on * UI configuration. */ void NVIC002_Init(void) { /*<<<DD_NVIC002_API_1>>>*/ FUNCTION_ENTRY(GID_NVIC002,NVIC002_FUNC_ENTRY); // Set Interrupt Priority for NVIC 5 Node App Instance 0 NVIC_SetPriority(5, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),63,0)); /* Enable Interrupt */ NVIC_EnableIRQ(5); FUNCTION_EXIT(GID_NVIC002,NVIC002_FUNC_EXIT); }
/** * @brief Main program * @param None * @retval None */ int main(void) { /*!< At this stage the microcontroller clock setting is already configured, this is done through SystemInit() function which is called from startup files (startup_stm32f40_41xxx.s/startup_stm32f427_437xx.s/startup_stm32f429_439xx.s) before to branch to application main. To reconfigure the default setting of SystemInit() function, refer to system_stm32f4xx.c file */ /* NVIC configuration ------------------------------------------------------*/ NVIC_Config(); /* Initialize LEDs mounted on EVAL board */ STM_EVAL_LEDInit(LED1); STM_EVAL_LEDInit(LED2); STM_EVAL_LEDInit(LED3); STM_EVAL_LEDInit(LED4); /* Initialize the KEY/Tamper and Wakeup buttons mounted on EVAL board */ STM_EVAL_PBInit(BUTTON_KEY_TAMPER, BUTTON_MODE_EXTI); STM_EVAL_PBInit(BUTTON_WAKEUP, BUTTON_MODE_EXTI); /* Configure the SysTick Handler Priority: Preemption priority and subpriority */ NVIC_SetPriority(SysTick_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), !ubPreemptionPriorityValue, 0)); while (1) { if(ubPreemptionOccurred != 0) { /* Toggle LED1 */ STM_EVAL_LEDToggle(LED1); /* Insert delay Time */ Delay(0x5FFFF); /* Toggle LED2 */ STM_EVAL_LEDToggle(LED2); Delay(0x5FFFF); /* Toggle LED3 */ STM_EVAL_LEDToggle(LED3); Delay(0x5FFFF); /* Toggle LED4 */ STM_EVAL_LEDToggle(LED4); Delay(0x5FFFF); } } }
/** * @brief Main program. * @param None * @retval None */ int main(void) { /*!< At this stage the microcontroller clock setting is already configured, this is done through SystemInit() function which is called from startup file (startup_stm32f10x_xx.s) before to branch to application main. To reconfigure the default setting of SystemInit() function, refer to system_stm32f10x.c file */ /* Initialize LED1..LED4, Key and Wakeup Buttons mounted on STM3210X-EVAL board */ STM_EVAL_LEDInit(LED1); STM_EVAL_LEDInit(LED2); STM_EVAL_LEDInit(LED3); STM_EVAL_LEDInit(LED4); STM_EVAL_PBInit(BUTTON_KEY, BUTTON_MODE_EXTI); STM_EVAL_PBInit(BUTTON_WAKEUP, BUTTON_MODE_EXTI); /* Configure one bit for preemption priority */ NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1); /* Enable the WAKEUP_BUTTON_EXTI_IRQn Interrupt */ NVIC_InitStructure.NVIC_IRQChannel = WAKEUP_BUTTON_EXTI_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PreemptionPriorityValue; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); /* Enable the KEY_BUTTON_EXTI_IRQn Interrupt */ NVIC_InitStructure.NVIC_IRQChannel = KEY_BUTTON_EXTI_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); /* Configure the SysTick Handler Priority: Preemption priority and subpriority */ NVIC_SetPriority(SysTick_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), !PreemptionPriorityValue, 0)); while (1) { if(PreemptionOccured != FALSE) { STM_EVAL_LEDToggle(LED1); Delay(0x5FFFF); STM_EVAL_LEDToggle(LED2); Delay(0x5FFFF); STM_EVAL_LEDToggle(LED3); Delay(0x5FFFF); STM_EVAL_LEDToggle(LED4); Delay(0x5FFFF); } } }
/** * @brief Sets the priority of an interrupt. * @param IRQn: External interrupt number. * This parameter can be an enumerator of IRQn_Type enumeration * (For the complete STM32 Devices IRQ Channels list, please refer to the appropriate CMSIS device file (stm32f4xxxx.h)) * @param PreemptPriority: The preemption priority for the IRQn channel. * This parameter can be a value between 0 and 15 * A lower priority value indicates a higher priority * @param SubPriority: the subpriority level for the IRQ channel. * This parameter can be a value between 0 and 15 * A lower priority value indicates a higher priority. * @retval None */ void HAL_NVIC_SetPriority(IRQn_Type IRQn, uint32_t PreemptPriority, uint32_t SubPriority) { uint32_t prioritygroup = 0x00; /* Check the parameters */ assert_param(IS_NVIC_SUB_PRIORITY(SubPriority)); assert_param(IS_NVIC_PREEMPTION_PRIORITY(PreemptPriority)); prioritygroup = NVIC_GetPriorityGrouping(); NVIC_SetPriority(IRQn, NVIC_EncodePriority(prioritygroup, PreemptPriority, SubPriority)); }
/* * Initialization function which initializes the SYSTIMER APP, configures SysTick timer and SysTick exception. */ SYSTIMER_STATUS_t SYSTIMER_Init(SYSTIMER_t *handle) { SYSTIMER_STATUS_t status = SYSTIMER_STATUS_SUCCESS; XMC_ASSERT("SYSTIMER_Init: SYSTIMER APP handle pointer uninitialized", (handle != NULL)); /* Check APP initialization status to ensure whether SYSTIMER_Init called or not, initialize SYSTIMER if * SYSTIMER_Init called first time. */ if (false == handle->init_status) { #if (UC_FAMILY == XMC4) /* Initialization of CPU_CTRL_XMC4 APP */ status = (SYSTIMER_STATUS_t)CPU_CTRL_XMC4_Init(CPU_CTRL_HANDLE); #else /* Initialization of CPU_CTRL_XMC1 APP */ status = (SYSTIMER_STATUS_t)CPU_CTRL_XMC1_Init(CPU_CTRL_HANDLE); #endif if (SYSTIMER_STATUS_FAILURE != status) { /* Initialize the header of the list */ g_timer_list = NULL; /* Initialize SysTick timer */ status = (SYSTIMER_STATUS_t)SysTick_Config((uint32_t)(SYSTIMER_SYSTICK_CLOCK * SYSTIMER_TICK_PERIOD)); if (SYSTIMER_STATUS_FAILURE == status) { XMC_DEBUG("SYSTIMER_Init: Timer reload value out of range"); } else { #if (UC_FAMILY == XMC4) /* setting of First SW Timer period is always and subpriority value for XMC4000 devices */ NVIC_SetPriority(SysTick_IRQn, NVIC_EncodePriority( NVIC_GetPriorityGrouping(), SYSTIMER_PRIORITY, SYSTIMER_SUBPRIORITY)); #elif (UC_FAMILY == XMC1) /* setting of priority value for XMC1000 devices */ NVIC_SetPriority(SysTick_IRQn, SYSTIMER_PRIORITY); #endif g_timer_tracker = 0U; /* Update the Initialization status of the SYSTIMER APP instance */ handle->init_status = true; status = SYSTIMER_STATUS_SUCCESS; } } } return (status); }
/*Channel initialization function*/ UART_STATUS_t TempUart_init() { UART_STATUS_t status = UART_STATUS_SUCCESS; /*Configure Receive pin*/ XMC_GPIO_Init((XMC_GPIO_PORT_t *)PORT5_BASE, 0U, &TempUart_rx_pin_config); /* Initialize USIC channel in UART mode*/ XMC_UART_CH_Init(XMC_UART0_CH0, &TempUart_channel_config); /*Set input source path*/ XMC_USIC_CH_SetInputSource(XMC_UART0_CH0, XMC_USIC_CH_INPUT_DX0, 3U); /* Start UART */ XMC_UART_CH_Start(XMC_UART0_CH0); /* Initialize UART TX pin */ XMC_GPIO_Init((XMC_GPIO_PORT_t *)PORT5_BASE, 1U, &TempUart_tx_pin_config); /*Set service request for transmit interrupt*/ XMC_USIC_CH_SetInterruptNodePointer(XMC_UART0_CH0, XMC_USIC_CH_INTERRUPT_NODE_POINTER_TRANSMIT_BUFFER, 3U); /*Set service request for receive interrupt*/ XMC_USIC_CH_SetInterruptNodePointer(XMC_UART0_CH0, XMC_USIC_CH_INTERRUPT_NODE_POINTER_RECEIVE, 5U); XMC_USIC_CH_SetInterruptNodePointer(XMC_UART0_CH0, XMC_USIC_CH_INTERRUPT_NODE_POINTER_ALTERNATE_RECEIVE, 5U); /*Set service request for UART protocol events*/ XMC_USIC_CH_SetInterruptNodePointer(XMC_UART0_CH0, XMC_USIC_CH_INTERRUPT_NODE_POINTER_PROTOCOL, 0U); /*Set priority and enable NVIC node for transmit interrupt*/ NVIC_SetPriority((IRQn_Type)87, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 18U, 0U)); NVIC_EnableIRQ((IRQn_Type)87); /*Set priority and enable NVIC node for receive interrupt*/ NVIC_SetPriority((IRQn_Type)89, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 18U, 0U)); NVIC_EnableIRQ((IRQn_Type)89); return status; }
void setISRPriorities() { __disable_irq(); // set two bits for preemptive data, two bits for priority as the // structure for the IPRs. Grouping is global withing the IPRs so // this value should only be changed here. NVIC_SetPriorityGrouping(5); uint32_t priorityGrouping = NVIC_GetPriorityGrouping(); // set preemptive priority default to 2 (0..3) // set priority default to 1 (0..3) uint32_t defaultPriority = NVIC_EncodePriority(priorityGrouping, 2, 1); // When the kernel initialzes the PNVIC, all ISRs are set to the // highest priority, making it impossible to elevate a few over // the rest, so the default priority is lowered globally for the // table first. // // Consult LPC17xx.h under IRQn_Type for PNVIC ranges, this is LPC1768 // specific for (uint32_t IRQn = TIMER0_IRQn; IRQn <= CANActivity_IRQn; IRQn++) NVIC_SetPriority((IRQn_Type)IRQn, defaultPriority); // reestablish watchdog NVIC_SetPriority(WDT_IRQn, NVIC_EncodePriority(priorityGrouping, 0, 0)); // make TIMER #2 2nd in line t the watchdog timer NVIC_SetPriority(TIMER2_IRQn, NVIC_EncodePriority(priorityGrouping, 0, 1)); // this is the timer used in the mbed Ticker library NVIC_SetPriority(TIMER3_IRQn, NVIC_EncodePriority(priorityGrouping, 0, 2)); // Brown-Out Detect NVIC_SetPriority(BOD_IRQn, NVIC_EncodePriority(priorityGrouping, 0, 4)); // The I2C interface that's in use NVIC_SetPriority(I2C2_IRQn, NVIC_EncodePriority(priorityGrouping, 1, 1)); // The SPI interface that's in use. NVIC_SetPriority(SPI_IRQn, NVIC_EncodePriority(priorityGrouping, 1, 2)); // set UART (console) interrupts to minimal priority // when debugging radio and other time sensitive operations, this // interrupt will need to be deferred. NVIC_SetPriority(UART0_IRQn, NVIC_EncodePriority(priorityGrouping, 1, 4)); __enable_irq(); }
/** * @brief Main program. * @param None * @retval None */ int main(void) { /*!< At this stage the microcontroller clock setting is already configured, this is done through SystemInit() function which is called from startup file (startup_stm32f10x_xx.s) before to branch to application main. To reconfigure the default setting of SystemInit() function, refer to system_stm32f10x.c file */ /* NVIC configuration ------------------------------------------------------*/ NVIC_Config(); /* Initialize LED1..LED4, Key and Sel Joystick Buttons mounted on STM3210X-EVAL board */ STM_EVAL_LEDInit(LED1); STM_EVAL_LEDInit(LED2); STM_EVAL_LEDInit(LED3); STM_EVAL_LEDInit(LED4); STM_EVAL_PBInit(BUTTON_KEY, BUTTON_MODE_EXTI); STM_EVAL_PBInit(BUTTON_WAKEUP, BUTTON_MODE_EXTI); /* Configure the SysTick Handler Priority: Preemption priority and subpriority */ NVIC_SetPriority(SysTick_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), !PreemptionPriorityValue, 0)); while (1) { if(PreemptionOccured != 0) { /* Toggel The lED1 */ STM_EVAL_LEDToggle(LED1); /* Insert delay Time */ Delay(0x5FFFF); STM_EVAL_LEDToggle(LED2); Delay(0x5FFFF); STM_EVAL_LEDToggle(LED3); Delay(0x5FFFF); STM_EVAL_LEDToggle(LED4); Delay(0x5FFFF); } } }
/** * @brief Configures NVIC. * @param None * @retval None */ void NVIC_Configuration(void) { NVIC_InitTypeDef NVIC_InitStructure; /* 1 bits for pre-emption priority and 3 bits for subpriority */ NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1); /* Set Button EXTI Interrupt priority to 0 (highest) */ NVIC_SetPriority(KEY_BUTTON_EXTI_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),0,0)); /* Set WWDG interrupt vector Preemption Priority to 1 */ NVIC_InitStructure.NVIC_IRQChannel = WWDG_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); }
static void stm32f4_adc_dma_init(ADC_HandleTypeDef* hadc) { DMA_HandleTypeDef *hdma; assert(hadc); hdma = hadc->DMA_Handle; stm32f4_adc_clk_enable(hadc); __HAL_RCC_DMA2_CLK_ENABLE(); HAL_DMA_Init(hdma); dma_handle[stm32f4_resolve_dma_handle_idx(hdma)] = hdma; NVIC_SetPriority(stm32f4_resolve_adc_dma_irq(hdma), NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 0, 0)); NVIC_SetVector(stm32f4_resolve_adc_dma_irq(hdma), stm32f4_resolve_adc_dma_irq_handler(hdma)); NVIC_EnableIRQ(stm32f4_resolve_adc_dma_irq(hdma)); }
/** * @brief This function handles External lines 15 to 10 interrupt request. * @param None * @retval None */ void EXTI15_10_IRQHandler(void) { NVIC_InitTypeDef NVIC_InitStructure; if(EXTI_GetITStatus(KEY_BUTTON_EXTI_LINE) != RESET) { PreemptionPriorityValue = !PreemptionPriorityValue; PreemptionOccured = 0; /* Modify the WAKEUP_BUTTON_EXTI_IRQn Interrupt Preemption Priority */ NVIC_InitStructure.NVIC_IRQChannel = WAKEUP_BUTTON_EXTI_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PreemptionPriorityValue; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); /* Configure the SysTick Handler Priority: Preemption priority and subpriority */ NVIC_SetPriority(SysTick_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), !PreemptionPriorityValue, 0)); /* Clear KEY_BUTTON_EXTI_LINE pending bit */ EXTI_ClearITPendingBit(KEY_BUTTON_EXTI_LINE); } }
/** * @brief Gets the priority grouping field from the NVIC Interrupt Controller. * @retval Priority grouping field (SCB->AIRCR [10:8] PRIGROUP field) */ uint32_t HAL_NVIC_GetPriorityGrouping(void) { /* Get the PRIGROUP[10:8] field value */ return NVIC_GetPriorityGrouping(); }
/* Initialization routine to call ADC LLD API's */ ADC_MEASUREMENT_STATUS_t ADC_MEASUREMENT_Init(ADC_MEASUREMENT_t *const handle_ptr) { const ADC_MEASUREMENT_CHANNEL_t *indexed; uint8_t j; ADC_MEASUREMENT_STATUS_t status; XMC_ASSERT("ADC_MEASUREMENT_Init:Invalid handle_ptr", (handle_ptr != NULL)) if (ADC_MEASUREMENT_STATUS_UNINITIALIZED == handle_ptr->init_state) { /* Call the function to initialise Clock and ADC global functional units*/ status = (ADC_MEASUREMENT_STATUS_t) GLOBAL_ADC_Init(handle_ptr->global_handle); /*Initialize the Global Conversion class 0*/ XMC_VADC_GLOBAL_InputClassInit(handle_ptr->global_handle->module_ptr,*handle_ptr->iclass_config_handle, XMC_VADC_GROUP_CONV_STD,ADC_MEASUREMENT_ICLASS_NUM); #if (UC_SERIES == XMC11) /*Initialize the Global Conversion class 1*/ XMC_VADC_GLOBAL_InputClassInit(handle_ptr->global_handle->module_ptr,*handle_ptr->iclass_config_handle, XMC_VADC_GROUP_CONV_STD,ADC_MEASUREMENT_ICLASS_NUM_XMC11); #endif /* Initialize the Background Scan hardware */ XMC_VADC_GLOBAL_BackgroundInit(handle_ptr->global_handle->module_ptr, handle_ptr->backgnd_config_handle); #if (XMC_VADC_GROUP_AVAILABLE == 0U) /* Initialize the global result register */ XMC_VADC_GLOBAL_ResultInit(handle_ptr->global_handle->module_ptr,handle_ptr->array->res_handle); #endif for (j = (uint8_t)0; j < (uint8_t)ADC_MEASUREMENT_MAXCHANNELS; j++) { indexed = handle_ptr->array->channel_array[j]; #if (XMC_VADC_GROUP_AVAILABLE == 1U) /* Initialize for configured channels*/ XMC_VADC_GROUP_ChannelInit(indexed->group_handle,(uint32_t)indexed->ch_num, indexed->ch_handle); /* Initialize for configured result registers */ XMC_VADC_GROUP_ResultInit(indexed->group_handle, (uint32_t)indexed->ch_handle->result_reg_number, indexed->res_handle); #endif /* Add all channels into the Background Request Source Channel Select Register */ XMC_VADC_GLOBAL_BackgroundAddChannelToSequence(handle_ptr->global_handle->module_ptr, (uint32_t)indexed->group_index, (uint32_t)indexed->ch_num); #ifdef ADC_MEASUREMENT_ANALOG_IO_USED /* ANALOG_IO initialization for the channel*/ if(indexed->analog_io_config != NULL) { status |= (ADC_MEASUREMENT_STATUS_t) ANALOG_IO_Init(indexed->analog_io_config); } #endif } #if(UC_SERIES != XMC11) if ((handle_ptr->backgnd_config_handle->req_src_interrupt) && (handle_ptr->req_src_intr_handle != NULL )) { #if (UC_FAMILY == XMC1) NVIC_SetPriority((IRQn_Type)handle_ptr->req_src_intr_handle->node_id, handle_ptr->req_src_intr_handle->priority); #else NVIC_SetPriority((IRQn_Type)handle_ptr->req_src_intr_handle->node_id, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), handle_ptr->req_src_intr_handle->priority, handle_ptr->req_src_intr_handle->sub_priority)); #endif /* Connect background Request Source Event to NVIC node */ XMC_VADC_GLOBAL_BackgroundSetReqSrcEventInterruptNode(handle_ptr->global_handle->module_ptr, (XMC_VADC_SR_t) handle_ptr->srv_req_node); /* Enable Background Scan Request source IRQ */ NVIC_EnableIRQ((IRQn_Type)handle_ptr->req_src_intr_handle->node_id); #ifdef ADC_MEASUREMENT_NON_DEFAULT_IRQ_SOURCE_SELECTED XMC_SCU_SetInterruptControl(handle_ptr->req_src_intr_handle->node_id, ((handle_ptr->req_src_intr_handle->node_id << 8) | handle_ptr->req_src_intr_handle->irqctrl)); #endif } #else /* Selected device is XMC11*/ XMC_VADC_GLOBAL_SetResultEventInterruptNode(handle_ptr->global_handle->module_ptr, handle_ptr->srv_req_node ); #ifdef ADC_MEASUREMENT_CPU_1X /* End of single measurement is enabled*/ NVIC_SetPriority((IRQn_Type)handle_ptr->result_intr_handle->node_id, handle_ptr->result_intr_handle->priority); /* Enable Background Scan Request source IRQ */ NVIC_EnableIRQ((IRQn_Type)handle_ptr->result_intr_handle->node_id); #endif #endif /* Mux Configuration is done*/ if (handle_ptr->mux_config != NULL) { (handle_ptr->mux_config)(); } if (handle_ptr->start_conversion != (bool)false) { /* Start conversion manually using load event trigger*/ XMC_VADC_GLOBAL_BackgroundTriggerConversion(handle_ptr->global_handle->module_ptr); } handle_ptr->init_state = status; } return (handle_ptr->init_state); }
/** * @brief Main program. * @param None * @retval None */ int main(void) { /*!< At this stage the microcontroller clock setting is already configured, this is done through SystemInit() function which is called from startup file (startup_stm32f10x_xx.s) before to branch to application main. To reconfigure the default setting of SystemInit() function, refer to system_stm32f10x.c file */ /* Initialize LED3 and USER Button mounted on STM32 Discovery board */ STM32vldiscovery_LEDInit(LED3); STM32vldiscovery_LEDInit(LED4); STM32vldiscovery_PBInit(BUTTON_USER, BUTTON_MODE_EXTI); /* Configure SysTick to generate an interrupt each 250ms */ SysTick_Configuration(); /* 1 bits for pre-emption priority and 3 bits for subpriority */ NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1); /* Set Button EXTI Interrupt priority to 0 (highest) */ NVIC_SetPriority(USER_BUTTON_EXTI_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),0,0)); /* Set SysTick interrupt vector Preemption Priority to 1 */ NVIC_SetPriority(SysTick_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),1,0)); /* Check if the system has resumed from IWDG reset */ if (RCC_GetFlagStatus(RCC_FLAG_IWDGRST) != RESET) { /* IWDGRST flag set */ /* Turn on LD3 */ STM32vldiscovery_LEDOn(LED3); /* Clear reset flags */ RCC_ClearFlag(); } else { /* IWDGRST flag is not set */ /* Turn off LD3 */ STM32vldiscovery_LEDOff(LED3); } /* IWDG timeout equal to 280 ms (the timeout may vary due to LSI frequency dispersion) */ /* Enable write access to IWDG_PR and IWDG_RLR registers */ IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable); /* IWDG counter clock: 40KHz(LSI) / 32 = 1.25 KHz */ IWDG_SetPrescaler(IWDG_Prescaler_32); /* Set counter reload value to 349 */ IWDG_SetReload(349); /* Reload IWDG counter */ IWDG_ReloadCounter(); /* Enable IWDG (the LSI oscillator will be enabled by hardware) */ IWDG_Enable(); while (1) {} }
void HAL_Interrupts_Attach(uint16_t pin, HAL_InterruptHandler handler, void* data, InterruptMode mode, HAL_InterruptExtraConfiguration* config) { uint8_t GPIO_PortSource = 0; //variable to hold the port number //EXTI structure to init EXT EXTI_InitTypeDef EXTI_InitStructure = {0}; //NVIC structure to set up NVIC controller NVIC_InitTypeDef NVIC_InitStructure = {0}; //Map the Spark pin to the appropriate port and pin on the STM32 STM32_Pin_Info* PIN_MAP = HAL_Pin_Map(); GPIO_TypeDef *gpio_port = PIN_MAP[pin].gpio_peripheral; uint16_t gpio_pin = PIN_MAP[pin].gpio_pin; uint8_t GPIO_PinSource = PIN_MAP[pin].gpio_pin_source; //Clear pending EXTI interrupt flag for the selected pin EXTI_ClearITPendingBit(gpio_pin); //Select the port source if (gpio_port == GPIOA) { GPIO_PortSource = 0; } else if (gpio_port == GPIOB) { GPIO_PortSource = 1; } else if (gpio_port == GPIOC) { GPIO_PortSource = 2; } else if (gpio_port == GPIOD) { GPIO_PortSource = 3; } // Register the handler for the user function name if (config && config->version >= HAL_INTERRUPT_EXTRA_CONFIGURATION_VERSION_2 && config->keepHandler) { // keep the old handler } else { exti_channels[GPIO_PinSource].fn = handler; exti_channels[GPIO_PinSource].data = data; } //Connect EXTI Line to appropriate Pin SYSCFG_EXTILineConfig(GPIO_PortSource, GPIO_PinSource); //Configure GPIO EXTI line EXTI_InitStructure.EXTI_Line = gpio_pin;//EXTI_Line; //select the interrupt mode EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt; switch (mode) { //case LOW: //There is no LOW mode in STM32, so using falling edge as default //EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling; //break; case CHANGE: //generate interrupt on rising or falling edge EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising_Falling; break; case RISING: //generate interrupt on rising edge EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising; break; case FALLING: //generate interrupt on falling edge EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling; break; } //enable EXTI line EXTI_InitStructure.EXTI_LineCmd = ENABLE; //send values to registers EXTI_Init(&EXTI_InitStructure); //configure NVIC //select NVIC channel to configure NVIC_InitStructure.NVIC_IRQChannel = GPIO_IRQn[GPIO_PinSource]; if (config == NULL) { NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 14; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; } else { NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = config->IRQChannelPreemptionPriority; NVIC_InitStructure.NVIC_IRQChannelSubPriority = config->IRQChannelSubPriority; // Keep the same priority if (config->version >= HAL_INTERRUPT_EXTRA_CONFIGURATION_VERSION_2) { if (config->keepPriority) { uint32_t priorityGroup = NVIC_GetPriorityGrouping(); uint32_t priority = NVIC_GetPriority(NVIC_InitStructure.NVIC_IRQChannel); uint32_t p, sp; NVIC_DecodePriority(priority, priorityGroup, &p, &sp); NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = p; NVIC_InitStructure.NVIC_IRQChannelSubPriority = sp; } } } //enable IRQ channel NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //update NVIC registers NVIC_Init(&NVIC_InitStructure); }
/** * @brief Initializes SDRAM MSP. * @param hsdram: SDRAM handle * @param Params * @retval None */ static void BSP_SDRAM_MspInit(SDRAM_HandleTypeDef *hsdram) { static DMA_HandleTypeDef dma_handle; #if !GFX_USE_OS_CHIBIOS GPIO_InitTypeDef gpio_init_structure; #endif /* Enable FMC clock */ __HAL_RCC_FMC_CLK_ENABLE(); /* Enable chosen DMAx clock */ __DMAx_CLK_ENABLE(); /* Enable GPIOs clock */ __HAL_RCC_GPIOC_CLK_ENABLE(); __HAL_RCC_GPIOD_CLK_ENABLE(); __HAL_RCC_GPIOE_CLK_ENABLE(); __HAL_RCC_GPIOF_CLK_ENABLE(); __HAL_RCC_GPIOG_CLK_ENABLE(); __HAL_RCC_GPIOH_CLK_ENABLE(); /* Common GPIO configuration - some are already setup by ChibiOS Init */ #if !GFX_USE_OS_CHIBIOS gpio_init_structure.Mode = GPIO_MODE_AF_PP; gpio_init_structure.Pull = GPIO_PULLUP; gpio_init_structure.Speed = GPIO_SPEED_FAST; gpio_init_structure.Alternate = GPIO_AF12_FMC; /* GPIOC configuration */ gpio_init_structure.Pin = GPIO_PIN_3; HAL_GPIO_Init(GPIOC, &gpio_init_structure); /* GPIOD configuration */ gpio_init_structure.Pin = GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_3 | GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_14 | GPIO_PIN_15; HAL_GPIO_Init(GPIOD, &gpio_init_structure); /* GPIOE configuration */ gpio_init_structure.Pin = GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_7| GPIO_PIN_8 | GPIO_PIN_9 |\ GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14 |\ GPIO_PIN_15; HAL_GPIO_Init(GPIOE, &gpio_init_structure); /* GPIOF configuration */ gpio_init_structure.Pin = GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2| GPIO_PIN_3 | GPIO_PIN_4 |\ GPIO_PIN_5 | GPIO_PIN_11 | GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14 |\ GPIO_PIN_15; HAL_GPIO_Init(GPIOF, &gpio_init_structure); /* GPIOG configuration */ gpio_init_structure.Pin = GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_4| GPIO_PIN_5 | GPIO_PIN_8 |\ GPIO_PIN_15; HAL_GPIO_Init(GPIOG, &gpio_init_structure); /* GPIOH configuration */ gpio_init_structure.Pin = GPIO_PIN_3 | GPIO_PIN_5; HAL_GPIO_Init(GPIOH, &gpio_init_structure); #endif /* Configure common DMA parameters */ dma_handle.Init.Channel = SDRAM_DMAx_CHANNEL; dma_handle.Init.Direction = DMA_MEMORY_TO_MEMORY; dma_handle.Init.PeriphInc = DMA_PINC_ENABLE; dma_handle.Init.MemInc = DMA_MINC_ENABLE; dma_handle.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD; dma_handle.Init.MemDataAlignment = DMA_MDATAALIGN_WORD; dma_handle.Init.Mode = DMA_NORMAL; dma_handle.Init.Priority = DMA_PRIORITY_HIGH; dma_handle.Init.FIFOMode = DMA_FIFOMODE_DISABLE; dma_handle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL; dma_handle.Init.MemBurst = DMA_MBURST_SINGLE; dma_handle.Init.PeriphBurst = DMA_PBURST_SINGLE; dma_handle.Instance = SDRAM_DMAx_STREAM; /* Associate the DMA handle */ __HAL_LINKDMA(hsdram, hdma, dma_handle); /* Deinitialize the stream for new transfer */ _HAL_DMA_DeInit(&dma_handle); /* Configure the DMA stream */ _HAL_DMA_Init(&dma_handle); /* NVIC configuration for DMA transfer complete interrupt */ NVIC_SetPriority(SDRAM_DMAx_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 5, 0)); /* Enable interrupt */ NVIC_EnableIRQ(SDRAM_DMAx_IRQn); }
/*__STATIC_INLINE*/ uint32_t CMSIS_STUB_NVIC_GetPriorityGrouping(void) { return NVIC_GetPriorityGrouping(); }
void init_dma() { RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN; // Enable clock /** We have two DMA streams for ADC1. (DMA2_Stream0 and DMA2_Stream4) We take DMA2 Stream4. See reference manual 9.3.3 channel selection (p. 166) */ // 1. Disable DMA-Stream DMA2_Stream4->CR &= ~DMA_SxCR_EN; while(DMA2_Stream4->CR & DMA_SxCR_EN); // we wait until it is disabled. uint32_t tmp_CR = 0; //DMA2_Stream4->CR; // 2. perihperal port register address DMA2_Stream4->PAR = (uint32_t)&ADC1->DR; // 3. memory address DMA2_Stream4->M0AR = (uint32_t)&adc_buffer[0]; DMA2_Stream4->M1AR = (uint32_t)&adc_buffer[1]; // 4. total number of data items DMA2_Stream4->NDTR = NUM_TEMP_SENSORS * OVERSAMPLE; // 5. DMA channel // channel 0 tmp_CR &= ~(DMA_SxCR_CHSEL); // 6. // 7. priority // Very high tmp_CR |= DMA_SxCR_PL; // 8. FIFO DMA2_Stream4->FCR &= ~(DMA_SxFCR_DMDIS); // 9. config the rest /* * halfword for memory and periphal: the 12bit ADC is 16bit right aligned * memory inc.: we read any adc and doing a step of 16bits after each conversion * circular mode: repeat until inf * double buffer mode: we write to one adress and read the other */ tmp_CR &= ~(DMA_SxCR_DIR); // | DMA_SxCR_DBM tmp_CR |= DMA_SxCR_MSIZE_0 | DMA_SxCR_PSIZE_0 | DMA_SxCR_MINC | DMA_SxCR_CIRC | DMA_SxCR_DBM | DMA_SxCR_TCIE; DMA2_Stream4->CR = tmp_CR; // 10. Enable DMA-Stream DMA2_Stream4->CR |= DMA_SxCR_EN; while(!(DMA2_Stream4->CR & DMA_SxCR_EN)); NVIC_SetPriority(DMA2_Stream4_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 3, 1)); }
void main_task(void *pvParameters) { (void) pvParameters; vTaskDelay(500 / portTICK_RATE_MS); SPIMaster spi5(SPI5, SPI_BAUDRATEPRESCALER_32, 0x2000, { {MEMS_SPI_SCK_PIN, GPIO_MODE_AF_PP, GPIO_PULLDOWN, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI5}, {MEMS_SPI_MISO_PIN, GPIO_MODE_AF_PP, GPIO_PULLDOWN, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI5}, {MEMS_SPI_MOSI_PIN, GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI5}, }, { {GYRO_CS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_MEDIUM, 0}, {ACCEL_CS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_MEDIUM, 0}, }); SPIMaster spi2(SPI2, SPI_BAUDRATEPRESCALER_32, 0x2000, { {EXT_MEMS_SPI_SCK_PIN, GPIO_MODE_AF_PP, GPIO_PULLDOWN, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI2}, {EXT_MEMS_SPI_MISO_PIN, GPIO_MODE_AF_PP, GPIO_PULLDOWN, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI2}, {EXT_MEMS_SPI_MOSI_PIN, GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI2}, }, { {EXT_GYRO_CS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_MEDIUM, 0}, {LPS25HB_PRESSURE_CS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_MEDIUM, 0}, {BMP280_PRESSURE_CS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_MEDIUM, 0}, }); L3GD20 gyro(spi5, 0); LPS25HB lps25hb(spi2, 1); BMP280 bmp2(spi2, 2); LSM303D accel(spi5, 1); uint8_t gyro_wtm = 5; uint8_t acc_wtm = 8; TimeStamp console_update_time; TimeStamp sample_dt; TimeStamp led_toggle_ts; FlightControl flight_ctl; static bool print_to_console = false; LowPassFilter<Vector3f, float> gyro_lpf({0.5}); LowPassFilter<Vector3f, float> acc_lpf_alt({0.9}); LowPassFilter<Vector3f, float> acc_lpf_att({0.990}); LowPassFilter<float, float> pressure_lpf({0.6}); attitudetracker att; /* * Apply the boot configuration from flash memory. */ dronestate_boot_config(*drone_state); L3GD20Reader gyro_reader(gyro, GYRO_INT2_PIN, gyro_align); LSM303Reader acc_reader(accel, ACC_INT2_PIN, acc_align); UartRpcServer rpcserver(*drone_state, configdata, acc_reader.mag_calibrator_); bmp2.set_oversamp_pressure(BMP280_OVERSAMP_16X); bmp2.set_work_mode(BMP280_ULTRA_HIGH_RESOLUTION_MODE); bmp2.set_filter(BMP280_FILTER_COEFF_OFF); Bmp280Reader bmp_reader(bmp2); HAL_NVIC_SetPriority(DMA1_Stream6_IRQn, 1, 1); HAL_NVIC_EnableIRQ (DMA1_Stream6_IRQn); HAL_NVIC_SetPriority(DMA1_Stream5_IRQn, 1, 0); HAL_NVIC_EnableIRQ (DMA1_Stream5_IRQn); #ifndef ENABLE_UART_TASK uart2.uart_dmarx_start(); #endif printf("Priority Group: %lu\n", NVIC_GetPriorityGrouping()); printf("SysTick_IRQn priority: %lu\n", NVIC_GetPriority(SysTick_IRQn) << __NVIC_PRIO_BITS); printf("configKERNEL_INTERRUPT_PRIORITY: %d\n", configKERNEL_INTERRUPT_PRIORITY); printf("configMAX_SYSCALL_INTERRUPT_PRIORITY: %d\n", configMAX_SYSCALL_INTERRUPT_PRIORITY); printf("LPS25HB Device id: %d\n", lps25hb.Get_DeviceID()); vTaskDelay(500 / portTICK_RATE_MS); gyro_reader.init(gyro_wtm); gyro_reader.enable_int2(false); vTaskDelay(500 / portTICK_RATE_MS); acc_reader.init(acc_wtm); acc_reader.enable_int2(false); acc_reader.mag_calibrator_.set_bias(drone_state->mag_bias_); acc_reader.mag_calibrator_.set_scale_factor(drone_state->mag_scale_factor_); vTaskDelay(500 / portTICK_RATE_MS); printf("Calibrating..."); gyro_reader.enable_int2(true); gyro_reader.calculate_static_bias_filtered(2400); printf(" Done!\n"); flight_ctl.start_receiver(); printf("Entering main loop...\n"); gyro_reader.enable_int2(true); sample_dt.time_stamp(); lps25hb.Set_FifoMode(LPS25HB_FIFO_STREAM_MODE); lps25hb.Set_FifoModeUse(LPS25HB_ENABLE); lps25hb.Set_Odr(LPS25HB_ODR_25HZ); lps25hb.Set_Bdu(LPS25HB_BDU_NO_UPDATE); LPS25HB_FIFOTypeDef_st fifo_config; memset(&fifo_config, 0, sizeof(fifo_config)); lps25hb.Get_FifoConfig(&fifo_config); #ifdef USE_LPS25HB float base_pressure = lps25hb.Get_PressureHpa(); for (int i = 0; i < 100; i++) { while (lps25hb.Get_FifoStatus().FIFO_EMPTY) vTaskDelay(50 / portTICK_RATE_MS); base_pressure = pressure_lpf.do_filter(lps25hb.Get_PressureHpa()); } #endif bmp_reader.calibrate(); // Infinite loop PerfCounter idle_time; while (1) { drone_state->iteration_++; if (drone_state->iteration_ % 120 == 0) { led1.toggle(); } if (drone_state->iteration_ % 4 == 0) { #ifdef USE_LPS25HB drone_state->temperature_ = lps25hb.Get_TemperatureCelsius(); while (!lps25hb.Get_FifoStatus().FIFO_EMPTY) { drone_state->pressure_hpa_ = pressure_lpf.do_filter(lps25hb.Get_PressureHpa()); float alt = (powf(base_pressure/drone_state->pressure_hpa_, 0.1902f) - 1.0f) * ((lps25hb.Get_TemperatureCelsius()) + 273.15f)/0.0065; drone_state->altitude_ = Distance::from_meters(alt); } #else bmp_reader.pressure_filter_.set_alpha(drone_state->altitude_lpf_); drone_state->altitude_ = bmp_reader.get_altitude(true); drone_state->pressure_hpa_ = bmp_reader.get_pressure().hpa(); drone_state->temperature_ = bmp_reader.get_temperature(false).celsius(); #endif } idle_time.begin_measure(); gyro_reader.wait_for_data(); idle_time.end_measure(); drone_state->dt_ = sample_dt.elapsed(); sample_dt.time_stamp(); if (drone_state->base_throttle_ > 0.1) att.accelerometer_correction_speed(drone_state->accelerometer_correction_speed_); else att.accelerometer_correction_speed(3.0f); att.gyro_drift_pid(drone_state->gyro_drift_kp_, drone_state->gyro_drift_ki_, drone_state->gyro_drift_kd_); att.gyro_drift_leak_rate(drone_state->gyro_drift_leak_rate_); size_t fifosize = gyro_reader.size(); for (size_t i = 0; i < fifosize; i++) drone_state->gyro_raw_ = gyro_lpf.do_filter(gyro_reader.read_sample()); if (drone_state->gyro_raw_.length_squared() > 0 && drone_state->dt_.microseconds() > 0) { drone_state->gyro_ = (drone_state->gyro_raw_ - gyro_reader.bias()) * drone_state->gyro_factor_; att.track_gyroscope(DEG2RAD(drone_state->gyro_) * 1.0f, drone_state->dt_.seconds_float()); } fifosize = acc_reader.size(); for (size_t i = 0; i < fifosize; i++) { Vector3f acc_sample = acc_reader.read_sample_acc(); acc_lpf_att.do_filter(acc_sample); acc_lpf_alt.do_filter(acc_sample); } drone_state->accel_raw_ = acc_lpf_att.output(); drone_state->accel_alt_ = acc_lpf_alt.output(); drone_state->accel_ = (drone_state->accel_raw_ - drone_state->accelerometer_adjustment_).normalize(); #define ALLOW_ACCELEROMETER_OFF #ifdef ALLOW_ACCELEROMETER_OFF if (drone_state->track_accelerometer_) { att.track_accelerometer(drone_state->accel_, drone_state->dt_.seconds_float()); } #else att.track_accelerometer(drone_state->accel_, drone_state->dt_.seconds_float()); #endif #define REALTIME_DATA 0 #if REALTIME_DATA std::cout << drone_state->gyro_.transpose() << drone_state->accel_.transpose() << drone_state->pid_torque_.transpose(); std::cout << drone_state->dt_.seconds_float() << std::endl; #endif drone_state->mag_raw_ = acc_reader.read_sample_mag(); drone_state->mag_ = drone_state->mag_raw_.normalize(); if (drone_state->track_magnetometer_) { att.track_magnetometer(drone_state->mag_, drone_state->dt_.seconds_float()); } drone_state->attitude_ = att.get_attitude(); drone_state->gyro_drift_error_ = RAD2DEG(att.get_drift_error()); flight_ctl.update_state(*drone_state); flight_ctl.send_throttle_to_motors(); if (print_to_console && console_update_time.elapsed() > TimeSpan::from_milliseconds(300)) { Vector3f drift_err = att.get_drift_error(); console_update_time.time_stamp(); printf("Gyro : %5.3f %5.3f %5.3f\n", drone_state->gyro_.at(0), drone_state->gyro_.at(1), drone_state->gyro_.at(2)); printf("Drift Err : %5.3f %5.3f %5.3f\n", RAD2DEG(drift_err.at(0)), RAD2DEG(drift_err.at(1)), RAD2DEG(drift_err.at(2))); printf("Gyro Raw : %5.3f %5.3f %5.3f\n", drone_state->gyro_raw_.at(0), drone_state->gyro_raw_.at(1), drone_state->gyro_raw_.at(2)); printf("Accel : %5.3f %5.3f %5.3f\n", drone_state->accel_.at(0), drone_state->accel_.at(1), drone_state->accel_.at(2)); printf("Mag : %5.3f %5.3f %5.3f\n", drone_state->mag_.at(0), drone_state->mag_.at(1), drone_state->mag_.at(2)); printf("dT : %lu uSec\n", (uint32_t)drone_state->dt_.microseconds()); printf("Q : %5.3f %5.3f %5.3f %5.3f\n\n", drone_state->attitude_.w, drone_state->attitude_.x, drone_state->attitude_.y, drone_state->attitude_.z); #if 1 printf("Motors : %1.2f %1.2f %1.2f %1.2f\n", drone_state->motors_[0], drone_state->motors_[1], drone_state->motors_[2], drone_state->motors_[3]); printf("Throttle : %1.2f\n", drone_state->base_throttle_); printf("Armed : %d\n", drone_state->motors_armed_); printf("Altitude : %4.2f m\n", drone_state->altitude_.meters()); printf("GPS : Lon: %3.4f Lat: %3.4f Sat %lu Alt: %4.2f m\n", drone_state->longitude_.degrees(), drone_state->latitude_.degrees(), drone_state->satellite_count_, drone_state->gps_altitude_.meters()); printf("Battery : %2.1f V, %2.0f%%\n", drone_state->battery_voltage_.volts(), drone_state->battery_percentage_); #endif } #if 0 if (led_toggle_ts.elapsed() > TimeSpan::from_seconds(1)) { led_toggle_ts.time_stamp(); led0.toggle(); } #endif #ifndef ENABLE_UART_TASK rpcserver.jsonrpc_request_handler(&uart2); #endif } }