void mcu_arch_init(void) { #if LUFTBOOT PRINT_CONFIG_MSG("We are running luftboot, the interrupt vector is being relocated.") SCB_VTOR = 0x00002000; #endif #if EXT_CLK == 8000000 PRINT_CONFIG_MSG("Using 8MHz external clock to PLL it to 72MHz.") rcc_clock_setup_in_hse_8mhz_out_72mhz(); #elif EXT_CLK == 12000000 PRINT_CONFIG_MSG("Using 12MHz external clock to PLL it to 72MHz.") rcc_clock_setup_in_hse_12mhz_out_72mhz(); #else #error EXT_CLK is either set to an unsupported frequency or not defined at all. Please check! #endif }
void ahrs_dcm_update_accel(struct Int32Vect3 *accel) { ACCELS_FLOAT_OF_BFP(accel_float, *accel); // DCM filter uses g-force as positive // accelerometer measures [0 0 -g] in a static case accel_float.x = -accel_float.x; accel_float.y = -accel_float.y; accel_float.z = -accel_float.z; ahrs_dcm.gps_age ++; if (ahrs_dcm.gps_age < 50) { //Remove centrifugal acceleration and longitudinal acceleration #if USE_AHRS_GPS_ACCELERATIONS PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses GPS acceleration.") accel_float.x += ahrs_dcm.gps_acceleration; // Longitudinal acceleration #endif accel_float.y += ahrs_dcm.gps_speed * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ accel_float.z -= ahrs_dcm.gps_speed * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY } else { ahrs_dcm.gps_speed = 0; ahrs_dcm.gps_acceleration = 0; ahrs_dcm.gps_age = 100; } Drift_correction(); }
void mcu_arch_init(void) { #if LUFTBOOT PRINT_CONFIG_MSG("We are running luftboot, the interrupt vector is being relocated.") #if defined STM32F4 SCB_VTOR = 0x00004000; #else SCB_VTOR = 0x00002000; #endif #endif #if EXT_CLK == 8000000 #if defined(STM32F1) PRINT_CONFIG_MSG("Using 8MHz external clock to PLL it to 72MHz.") rcc_clock_setup_in_hse_8mhz_out_72mhz(); #elif defined(STM32F4) PRINT_CONFIG_MSG("Using 8MHz external clock to PLL it to 168MHz.") rcc_clock_setup_hse_3v3(&rcc_hse_8mhz_3v3[RCC_CLOCK_3V3_168MHZ]); #endif #elif EXT_CLK == 12000000 #if defined(STM32F1) PRINT_CONFIG_MSG("Using 12MHz external clock to PLL it to 72MHz.") rcc_clock_setup_in_hse_12mhz_out_72mhz(); #elif defined(STM32F4) PRINT_CONFIG_MSG("Using 12MHz external clock to PLL it to 168MHz.") rcc_clock_setup_hse_3v3(&rcc_hse_12mhz_3v3[RCC_CLOCK_3V3_168MHZ]); #endif #elif EXT_CLK == 16000000 #if defined(STM32F4) PRINT_CONFIG_MSG("Using 16MHz external clock to PLL it to 168MHz.") rcc_clock_setup_hse_3v3(&rcc_hse_16mhz_3v3[RCC_CLOCK_3V3_168MHZ]); #endif #elif EXT_CLK == 24000000 #if defined(STM32F4) PRINT_CONFIG_MSG("Using 24MHz external clock to PLL it to 168MHz.") rcc_clock_setup_hse_3v3(&rcc_hse_24mhz_3v3[RCC_CLOCK_3V3_168MHZ]); #elif defined(STM32F1) rcc_clock_setup_in_hse_24mhz_out_24mhz_pprz(); #endif #elif EXT_CLK == 25000000 #if defined(STM32F4) PRINT_CONFIG_MSG("Using 25MHz external clock to PLL it to 168MHz.") rcc_clock_setup_hse_3v3(&rcc_hse_25mhz_3v3[RCC_CLOCK_3V3_168MHZ]); #endif #else #error EXT_CLK is either set to an unsupported frequency or not defined at all. Please check! #endif /* Configure priority grouping 0 bits for pre-emption priority and 4 bits for sub-priority. * this was previously in i2c driver * FIXME is it really needed ? */ scb_set_priority_grouping(SCB_AIRCR_PRIGROUP_NOGROUP_SUB16); }
void actuators_pwm_arch_init(void) { /*----------------------------------- * Configure timer peripheral clocks *-----------------------------------*/ /* TIM3, TIM4 and TIM5 clock enable */ rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM3EN); #if REMAP_SERVOS_5AND6 rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM5EN); #else rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM4EN); #endif #if USE_SERVOS_7AND8 rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM4EN); #endif /*---------------- * Configure GPIO *----------------*/ /* GPIO A,B and C clock enable */ rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPAEN | RCC_APB2ENR_IOPBEN | RCC_APB2ENR_IOPCEN | RCC_APB2ENR_AFIOEN); /* TIM3 GPIO for PWM1..4 */ AFIO_MAPR |= AFIO_MAPR_TIM3_REMAP_FULL_REMAP; gpio_set_mode(GPIO_BANK_TIM3_FR, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_TIM3_FR_CH1 | GPIO_TIM3_FR_CH2 | GPIO_TIM3_FR_CH3 | GPIO_TIM3_FR_CH4); /* TIM4 GPIO for PWM7..8 */ #if USE_SERVOS_7AND8 gpio_set_mode(GPIO_BANK_TIM4, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_TIM4_CH1 | GPIO_TIM4_CH2); #endif /* TIM4/5 GPIO for PWM6..7 */ #if REMAP_SERVOS_5AND6 gpio_set_mode(GPIO_BANK_TIM5, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_TIM5_CH1 | GPIO_TIM5_CH2); #else gpio_set_mode(GPIO_BANK_TIM4, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_TIM4_CH3 | GPIO_TIM4_CH4); #endif /*--------------- * Timer 3 setup *---------------*/ timer_reset(TIM3); /* Timer global mode: * - No divider. * - Alignement edge. * - Direction up. */ timer_set_mode(TIM3, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); timer_set_prescaler(TIM3, (PCLK / ONE_MHZ_CLK) - 1); // 1uS timer_disable_preload(TIM3); timer_continuous_mode(TIM3); timer_set_period(TIM3, (ONE_MHZ_CLK / SERVO_HZ) - 1); /* Disable outputs. */ timer_disable_oc_output(TIM3, TIM_OC1); timer_disable_oc_output(TIM3, TIM_OC2); timer_disable_oc_output(TIM3, TIM_OC3); timer_disable_oc_output(TIM3, TIM_OC4); /* -- Channel configuration -- */ actuators_pwm_arch_channel_init(TIM3, TIM_OC1); actuators_pwm_arch_channel_init(TIM3, TIM_OC2); actuators_pwm_arch_channel_init(TIM3, TIM_OC3); actuators_pwm_arch_channel_init(TIM3, TIM_OC4); /* * Set initial output compare values. * Note: Maybe we should preload the compare registers with some sensible * values before we enable the timer? */ //timer_set_oc_value(TIM3, TIM_OC1, 1000); //timer_set_oc_value(TIM3, TIM_OC2, 1000); //timer_set_oc_value(TIM3, TIM_OC3, 1000); //timer_set_oc_value(TIM3, TIM_OC4, 1000); /* -- Enable timer -- */ /* * ARR reload enable. * Note: In our case it does not matter much if we do preload or not. As it * is unlikely we will want to change the frequency of the timer during * runtime anyways. */ timer_enable_preload(TIM3); /* Counter enable. */ timer_enable_counter(TIM3); #if (!REMAP_SERVOS_5AND6 || USE_SERVOS_7AND8) #if !REMAP_SERVOS_5AND6 PRINT_CONFIG_MSG("Not remapping servos 5 and 6 using PB8 and PB9 -> TIM4") #endif #if USE_SERVOS_7AND8 PRINT_CONFIG_MSG("Enabling sevros 7 and 8 on PB6, PB7 -> TIM4") #endif /*--------------- * Timer 4 setup *---------------*/ timer_reset(TIM4); /* Timer global mode: * - No divider. * - Alignement edge. * - Direction up. */ timer_set_mode(TIM4, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); timer_set_prescaler(TIM4, (PCLK / ONE_MHZ_CLK) - 1); // 1uS timer_disable_preload(TIM4); timer_continuous_mode(TIM4); #ifdef SERVO_HZ_SECONDARY timer_set_period(TIM4, (ONE_MHZ_CLK / SERVO_HZ_SECONDARY) - 1); #else timer_set_period(TIM4, (ONE_MHZ_CLK / SERVO_HZ) - 1); #endif /* Disable outputs. */ #if USE_SERVOS_7AND8 timer_disable_oc_output(TIM4, TIM_OC1); timer_disable_oc_output(TIM4, TIM_OC2); #endif #if !REMAP_SERVOS_5AND6 timer_disable_oc_output(TIM4, TIM_OC3); timer_disable_oc_output(TIM4, TIM_OC4); #endif /* -- Channel configuration -- */ #if USE_SERVOS_7AND8 actuators_pwm_arch_channel_init(TIM4, TIM_OC1); actuators_pwm_arch_channel_init(TIM4, TIM_OC2); #endif #if !REMAP_SERVOS_5AND6 actuators_pwm_arch_channel_init(TIM4, TIM_OC3); actuators_pwm_arch_channel_init(TIM4, TIM_OC4); #endif /* * Set initial output compare values. * Note: Maybe we should preload the compare registers with some sensible * values before we enable the timer? */ #if USE_SERVOS_7AND8 //timer_set_oc_value(TIM4, TIM_OC1, 1000); //timer_set_oc_value(TIM4, TIM_OC2, 1000); #endif #if ! REMAP_SERVOS_5AND6 //timer_set_oc_value(TIM4, TIM_OC3, 1000); //timer_set_oc_value(TIM4, TIM_OC4, 1000); #endif /* -- Enable timer -- */ /* * ARR reload enable. * Note: In our case it does not matter much if we do preload or not. As it * is unlikely we will want to change the frequency of the timer during * runtime anyways. */ timer_enable_preload(TIM4); /* Counter enable. */ timer_enable_counter(TIM4); #endif #if REMAP_SERVOS_5AND6 PRINT_CONFIG_MSG("Remapping servo outputs 5 and 6 to PA0,PA1 -> TIM5") /*--------------- * Timer 5 setup *---------------*/ timer_reset(TIM5); /* Timer global mode: * - No divider. * - Alignement edge. * - Direction up. */ timer_set_mode(TIM5, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); timer_set_prescaler(TIM5, (PCLK / ONE_MHZ_CLK) - 1); // 1uS timer_disable_preload(TIM5); timer_continuous_mode(TIM5); #ifdef SERVO_HZ_SECONDARY timer_set_period(TIM5, (ONE_MHZ_CLK / SERVO_HZ_SECONDARY) - 1); #else timer_set_period(TIM5, (ONE_MHZ_CLK / SERVO_HZ) - 1); #endif /* Disable outputs. */ timer_disable_oc_output(TIM5, TIM_OC1); timer_disable_oc_output(TIM5, TIM_OC2); /* -- Channel configuration -- */ actuators_pwm_arch_channel_init(TIM5, TIM_OC1); actuators_pwm_arch_channel_init(TIM5, TIM_OC2); /* * Set the capture compare value for OC1. * Note: Maybe we should preload the compare registers with some sensible * values before we enable the timer? */ //timer_set_oc_value(TIM5, TIM_OC1, 1000); //timer_set_oc_value(TIM5, TIM_OC2, 1000); /* -- Enable timer -- */ /* * ARR reload enable. * Note: In our case it does not matter much if we do preload or not. As it * is unlikely we will want to change the frequency of the timer during * runtime anyways. */ timer_enable_preload(TIM5); /* Counter enable. */ timer_enable_counter(TIM5); #endif }
/** * Enable selected channels on specified ADC. * Usage: * * adc_init_single(ADC1, 1, 1, 0, 0); * * ... would enable ADC1, enabling channels 1 and 2, * but not 3 and 4. */ static inline void adc_init_single(uint32_t adc, uint8_t chan1, uint8_t chan2, uint8_t chan3, uint8_t chan4) { uint8_t num_channels, rank; uint8_t channels[4]; // Paranoia, must be down for 2+ ADC clock cycles before calibration adc_off(adc); /* enable adc clock */ if (adc == ADC1) { #ifdef USE_AD1 num_channels = NB_ADC1_CHANNELS; ADC1_GPIO_INIT(); #endif } else if (adc == ADC2) { #ifdef USE_AD2 num_channels = NB_ADC2_CHANNELS; ADC2_GPIO_INIT(); #endif } /* Configure ADC */ /* Explicitly setting most registers, reset/default values are correct for most */ /* Set CR1 register. */ /* Clear AWDEN */ adc_disable_analog_watchdog_regular(adc); /* Clear JAWDEN */ adc_disable_analog_watchdog_injected(adc); /* Clear DISCEN */ adc_disable_discontinuous_mode_regular(adc); /* Clear JDISCEN */ adc_disable_discontinuous_mode_injected(adc); /* Clear JAUTO */ adc_disable_automatic_injected_group_conversion(adc); /* Set SCAN */ adc_enable_scan_mode(adc); /* Enable ADC<X> JEOC interrupt (Set JEOCIE) */ adc_enable_eoc_interrupt_injected(adc); /* Clear AWDIE */ adc_disable_awd_interrupt(adc); /* Clear EOCIE */ adc_disable_eoc_interrupt(adc); /* Set CR2 register. */ /* Clear TSVREFE */ adc_disable_temperature_sensor(adc); /* Clear EXTTRIG */ adc_disable_external_trigger_regular(adc); /* Clear ALIGN */ adc_set_right_aligned(adc); /* Clear DMA */ adc_disable_dma(adc); /* Clear CONT */ adc_set_single_conversion_mode(adc); rank = 0; if (chan1) { adc_set_sample_time(adc, adc_channel_map[0], ADC_SMPR1_SMP_41DOT5CYC); channels[rank] = adc_channel_map[0]; rank++; } if (chan2) { adc_set_sample_time(adc, adc_channel_map[1], ADC_SMPR1_SMP_41DOT5CYC); channels[rank] = adc_channel_map[1]; rank++; } if (chan3) { adc_set_sample_time(adc, adc_channel_map[2], ADC_SMPR1_SMP_41DOT5CYC); channels[rank] = adc_channel_map[2]; rank++; } if (chan4) { adc_set_sample_time(adc, adc_channel_map[3], ADC_SMPR1_SMP_41DOT5CYC); channels[rank] = adc_channel_map[3]; } adc_set_injected_sequence(adc, num_channels, channels); #if USE_AD_TIM4 PRINT_CONFIG_MSG("Info: Using TIM4 for ADC") adc_enable_external_trigger_injected(adc, ADC_CR2_JEXTSEL_TIM4_TRGO); #elif USE_AD_TIM1 PRINT_CONFIG_MSG("Info: Using TIM1 for ADC") adc_enable_external_trigger_injected(adc, ADC_CR2_JEXTSEL_TIM1_TRGO); #else PRINT_CONFIG_MSG("Info: Using default TIM2 for ADC") adc_enable_external_trigger_injected(adc, ADC_CR2_JEXTSEL_TIM2_TRGO); #endif /* Enable ADC<X> */ adc_power_on(adc); /* Enable ADC<X> reset calibaration register */ adc_reset_calibration(adc); /* Check the end of ADC<X> reset calibration */ while ((ADC_CR2(adc) & ADC_CR2_RSTCAL) != 0); /* Start ADC<X> calibaration */ adc_calibration(adc); /* Check the end of ADC<X> calibration */ while ((ADC_CR2(adc) & ADC_CR2_CAL) != 0); } // adc_init_single
/** * Adc init * * Initialize ADC drivers, buffers and start conversion in the background */ void adc_init(void) { /* Init GPIO ports for ADC operation */ #if USE_ADC_1 PRINT_CONFIG_MSG("Info: Using ADC_1"); gpio_setup_pin_analog(ADC_1_GPIO_PORT, ADC_1_GPIO_PIN); #endif #if USE_ADC_2 PRINT_CONFIG_MSG("Info: Using ADC_2"); gpio_setup_pin_analog(ADC_2_GPIO_PORT, ADC_2_GPIO_PIN); #endif #if USE_ADC_3 PRINT_CONFIG_MSG("Info: Using ADC_3"); gpio_setup_pin_analog(ADC_3_GPIO_PORT, ADC_3_GPIO_PIN); #endif #if USE_ADC_4 PRINT_CONFIG_MSG("Info: Using ADC_4"); gpio_setup_pin_analog(ADC_4_GPIO_PORT, ADC_4_GPIO_PIN); #endif #if USE_ADC_5 PRINT_CONFIG_MSG("Info: Using ADC_5"); gpio_setup_pin_analog(ADC_5_GPIO_PORT, ADC_5_GPIO_PIN); #endif #if USE_ADC_6 PRINT_CONFIG_MSG("Info: Using ADC_6"); gpio_setup_pin_analog(ADC_6_GPIO_PORT, ADC_6_GPIO_PIN); #endif #if USE_ADC_7 PRINT_CONFIG_MSG("Info: Using ADC_7"); gpio_setup_pin_analog(ADC_7_GPIO_PORT, ADC_7_GPIO_PIN); #endif #if USE_ADC_8 PRINT_CONFIG_MSG("Info: Using ADC_8"); gpio_setup_pin_analog(ADC_8_GPIO_PORT, ADC_8_GPIO_PIN); #endif #if USE_ADC_9 PRINT_CONFIG_MSG("Info: Using ADC_9"); gpio_setup_pin_analog(ADC_9_GPIO_PORT, ADC_9_GPIO_PIN); #endif // Configurtion register uint32_t sqr1, sqr2, sqr3; adc_regular_sequence(&sqr1, &sqr2, &sqr3, ADC_NUM_CHANNELS, adc_channel_map); #ifdef __STM32F10x_H uint32_t smpr1, smpr2; adc_sample_time_on_all_channels(&smpr1, &smpr2, ADC_SAMPLE_41P5); adcgrpcfg.cr2 = ADC_CR2_TSVREFE; #elif defined(__STM32F4xx_H) uint32_t smpr1, smpr2; adc_sample_time_on_all_channels(&smpr1, &smpr2, ADC_SAMPLE_480); adcgrpcfg.cr2 = ADC_CR2_SWSTART; #endif #if USE_ADC_WATCHDOG adc_watchdog.adc = NULL; adc_watchdog.cb = NULL; adc_watchdog.channel = 0; adc_watchdog.vmin = (1<<12)-1; // max adc #endif adcgrpcfg.circular = TRUE; adcgrpcfg.num_channels = ADC_NUM_CHANNELS; adcgrpcfg.end_cb = adc1callback; adcgrpcfg.error_cb = adcerrorcallback; adcgrpcfg.cr1 = 0; adcgrpcfg.smpr1 = smpr1; adcgrpcfg.smpr2 = smpr2; adcgrpcfg.sqr1 = sqr1; adcgrpcfg.sqr2 = sqr2; adcgrpcfg.sqr3 = sqr3; // Start ADC in continious conversion mode adcStart(&ADCD1, NULL); adcStartConversion(&ADCD1, &adcgrpcfg, adc_samples, ADC_BUF_DEPTH); }