/*
* 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
}
}
