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);
}
