From 4ae63f4c9807a508e02c67d955d3fe86852e10b7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 1 Dec 2024 18:34:26 +1100 Subject: [PATCH] global reformatting reformat with astyle for 2-space indent --- Inc/common.h | 32 +- Inc/eeprom.h | 116 +- Inc/functions.h | 4 +- Inc/signal.h | 2 +- Inc/targets.h | 6 +- Mcu/e230/Inc/IO.h | 2 +- Mcu/e230/Inc/serial_telemetry.h | 2 +- Mcu/e230/Src/ADC.c | 143 +- Mcu/e230/Src/IO.c | 78 +- Mcu/e230/Src/comparator.c | 75 +- Mcu/e230/Src/eeprom.c | 57 +- Mcu/e230/Src/gd32e23x_it.c | 94 +- Mcu/e230/Src/peripherals.c | 601 +++---- Mcu/e230/Src/phaseouts.c | 471 +++--- Mcu/e230/Src/serial_telemetry.c | 145 +- Mcu/e230/Src/system_gd32e23x.c | 404 ++--- Mcu/e230/Src/systick.c | 26 +- Mcu/f031/Inc/IO.h | 2 +- Mcu/f031/Inc/serial_telemetry.h | 2 +- Mcu/f031/Src/ADC.c | 207 +-- Mcu/f031/Src/IO.c | 226 +-- Mcu/f031/Src/comparator.c | 57 +- Mcu/f031/Src/eeprom.c | 90 +- Mcu/f031/Src/peripherals.c | 1003 ++++++------ Mcu/f031/Src/phaseouts.c | 439 ++--- Mcu/f031/Src/serial_telemetry.c | 187 +-- Mcu/f031/Src/stm32f0xx_it.c | 329 ++-- Mcu/f031/Src/system_stm32f0xx.c | 107 +- Mcu/f051/Inc/IO.h | 2 +- Mcu/f051/Inc/serial_telemetry.h | 2 +- Mcu/f051/Src/ADC.c | 213 +-- Mcu/f051/Src/IO.c | 118 +- Mcu/f051/Src/comparator.c | 66 +- Mcu/f051/Src/eeprom.c | 90 +- Mcu/f051/Src/peripherals.c | 886 ++++++----- Mcu/f051/Src/phaseouts.c | 437 ++--- Mcu/f051/Src/serial_telemetry.c | 314 ++-- Mcu/f051/Src/stm32f0xx_it.c | 274 ++-- Mcu/f051/Src/system_stm32f0xx.c | 107 +- Mcu/f415/Inc/IO.h | 2 +- Mcu/f415/Inc/serial_telemetry.h | 2 +- Mcu/f415/Inc/system_at32f415.h | 2 +- Mcu/f415/Src/ADC.c | 110 +- Mcu/f415/Src/IO.c | 110 +- Mcu/f415/Src/at32f415_it.c | 168 +- Mcu/f415/Src/comparator.c | 91 +- Mcu/f415/Src/eeprom.c | 66 +- Mcu/f415/Src/peripherals.c | 552 +++---- Mcu/f415/Src/phaseouts.c | 449 +++--- Mcu/f415/Src/serial_telemetry.c | 141 +- Mcu/f415/Src/system_at32f415.c | 208 +-- Mcu/f421/Inc/IO.h | 2 +- Mcu/f421/Inc/serial_telemetry.h | 2 +- Mcu/f421/Inc/system_at32f421.h | 2 +- Mcu/f421/Src/ADC.c | 121 +- Mcu/f421/Src/IO.c | 85 +- Mcu/f421/Src/WS2812.c | 38 +- Mcu/f421/Src/at32f421_it.c | 192 +-- Mcu/f421/Src/comparator.c | 52 +- Mcu/f421/Src/eeprom.c | 75 +- Mcu/f421/Src/peripherals.c | 436 ++--- Mcu/f421/Src/phaseouts.c | 461 +++--- Mcu/f421/Src/serial_telemetry.c | 137 +- Mcu/f421/Src/system_at32f421.c | 222 +-- Mcu/g071/Inc/serial_telemetry.h | 2 +- Mcu/g071/Src/ADC.c | 302 ++-- Mcu/g071/Src/IO.c | 100 +- Mcu/g071/Src/WS2812.c | 256 +-- Mcu/g071/Src/comparator.c | 70 +- Mcu/g071/Src/eeprom.c | 92 +- Mcu/g071/Src/peripherals.c | 1111 ++++++------- Mcu/g071/Src/phaseouts.c | 439 ++--- Mcu/g071/Src/serial_telemetry.c | 153 +- Mcu/g071/Src/stm32g0xx_it.c | 300 ++-- Mcu/g071/Src/system_stm32g0xx.c | 115 +- Mcu/g431/Inc/serial_telemetry.h | 2 +- Mcu/g431/Src/ADC.c | 132 +- Mcu/g431/Src/IO.c | 100 +- Mcu/g431/Src/comparator.c | 63 +- Mcu/g431/Src/eeprom.c | 92 +- Mcu/g431/Src/peripherals.c | 762 ++++----- Mcu/g431/Src/phaseouts.c | 445 +++--- Mcu/g431/Src/serial_telemetry.c | 217 +-- Mcu/g431/Src/stm32g4xx_it.c | 136 +- Mcu/g431/Src/system_stm32g4xx.c | 77 +- Mcu/l431/Inc/IO.h | 2 +- Mcu/l431/Inc/serial_telemetry.h | 2 +- Mcu/l431/Inc/stm32l4xx_it.h | 2 +- Src/DroneCAN/DroneCAN.c | 1648 +++++++++---------- Src/DroneCAN/filter.c | 70 +- Src/DroneCAN/sys_can.h | 26 +- Src/DroneCAN/sys_can_at32.c | 320 ++-- Src/DroneCAN/sys_can_stm32.c | 244 +-- Src/dshot.c | 368 ++--- Src/functions.c | 103 +- Src/main.c | 2649 ++++++++++++++++--------------- Src/signal.c | 368 ++--- Src/sounds.c | 363 ++--- 98 files changed, 11252 insertions(+), 11021 deletions(-) diff --git a/Inc/common.h b/Inc/common.h index 999c62c8..7648e03d 100644 --- a/Inc/common.h +++ b/Inc/common.h @@ -67,26 +67,26 @@ extern uint16_t send_extended_dshot; // }PID; typedef struct fastPID { - int32_t error; - uint32_t Kp; - uint32_t Ki; - uint32_t Kd; - int32_t integral; - int32_t derivative; - int32_t last_error; - int32_t pid_output; - int32_t integral_limit; - int32_t output_limit; + int32_t error; + uint32_t Kp; + uint32_t Ki; + uint32_t Kd; + int32_t integral; + int32_t derivative; + int32_t last_error; + int32_t pid_output; + int32_t integral_limit; + int32_t output_limit; } fastPID; /* input signal types */ enum inputType { - AUTO_IN = 0, - DSHOT_IN = 1, - SERVO_IN = 2, - SERIAL_IN = 3, - EDTARM_IN = 4, - DRONECAN_IN = 5, + AUTO_IN = 0, + DSHOT_IN = 1, + SERVO_IN = 2, + SERIAL_IN = 3, + EDTARM_IN = 4, + DRONECAN_IN = 5, }; diff --git a/Inc/eeprom.h b/Inc/eeprom.h index e412484b..d2dbc128 100644 --- a/Inc/eeprom.h +++ b/Inc/eeprom.h @@ -3,65 +3,65 @@ #pragma once typedef union EEprom_u { + struct { + uint8_t reserved_0; //0 + uint8_t eeprom_version; //1 + uint8_t reserved_1; //2 struct { - uint8_t reserved_0; //0 - uint8_t eeprom_version; //1 - uint8_t reserved_1; //2 - struct { - uint8_t major; //3 - uint8_t minor; //4 - } version; - char firmware_name[12]; //5-16 - uint8_t dir_reversed; // 17 - uint8_t bi_direction; // 18 - uint8_t use_sine_start; // 19 - uint8_t comp_pwm; // 20 - uint8_t variable_pwm; // 21 - uint8_t stuck_rotor_protection; // 22 - uint8_t advance_level; // 23 - uint8_t pwm_frequency; // 24 - uint8_t startup_power; // 25 - uint8_t motor_kv; // 26 - uint8_t motor_poles; // 27 - uint8_t brake_on_stop; // 28 - uint8_t stall_protection; // 29 - uint8_t beep_volume; // 30 - uint8_t telementry_on_interval; // 31 - struct { - uint8_t low_threshold; // 32 - uint8_t high_threshold; // 33 - uint8_t neutral; // 34 - uint8_t dead_band; // 35 - } servo; - uint8_t low_voltage_cut_off; // 36 - uint8_t low_cell_volt_cutoff; // 37 - uint8_t rc_car_reverse; // 38 - uint8_t use_hall_sensors; // 39 - uint8_t sine_mode_changeover_thottle_level; // 40 - uint8_t drag_brake_strength; // 41 - uint8_t driving_brake_strength; // 42 - struct { - uint8_t temperature; // 43 - uint8_t current; // 44 - } limits; - uint8_t sine_mode_power; // 45 - uint8_t input_type; // 46 - uint8_t auto_advance; // 47 - uint8_t reserved_2[4]; //48-51 - uint8_t tune[124]; // 52-175 - struct { - uint8_t can_node; // 176 - uint8_t esc_index; // 177 - uint8_t require_arming; // 178 - uint8_t telem_rate; // 179 - uint8_t require_zero_throttle; // 180 - uint8_t filter_hz; // 181 - uint8_t debug_rate; // 182 - uint8_t term_enable; // 183 - uint8_t reserved[8]; // 184-191 - } can; - }; - uint8_t buffer[192]; + uint8_t major; //3 + uint8_t minor; //4 + } version; + char firmware_name[12]; //5-16 + uint8_t dir_reversed; // 17 + uint8_t bi_direction; // 18 + uint8_t use_sine_start; // 19 + uint8_t comp_pwm; // 20 + uint8_t variable_pwm; // 21 + uint8_t stuck_rotor_protection; // 22 + uint8_t advance_level; // 23 + uint8_t pwm_frequency; // 24 + uint8_t startup_power; // 25 + uint8_t motor_kv; // 26 + uint8_t motor_poles; // 27 + uint8_t brake_on_stop; // 28 + uint8_t stall_protection; // 29 + uint8_t beep_volume; // 30 + uint8_t telementry_on_interval; // 31 + struct { + uint8_t low_threshold; // 32 + uint8_t high_threshold; // 33 + uint8_t neutral; // 34 + uint8_t dead_band; // 35 + } servo; + uint8_t low_voltage_cut_off; // 36 + uint8_t low_cell_volt_cutoff; // 37 + uint8_t rc_car_reverse; // 38 + uint8_t use_hall_sensors; // 39 + uint8_t sine_mode_changeover_thottle_level; // 40 + uint8_t drag_brake_strength; // 41 + uint8_t driving_brake_strength; // 42 + struct { + uint8_t temperature; // 43 + uint8_t current; // 44 + } limits; + uint8_t sine_mode_power; // 45 + uint8_t input_type; // 46 + uint8_t auto_advance; // 47 + uint8_t reserved_2[4]; //48-51 + uint8_t tune[124]; // 52-175 + struct { + uint8_t can_node; // 176 + uint8_t esc_index; // 177 + uint8_t require_arming; // 178 + uint8_t telem_rate; // 179 + uint8_t require_zero_throttle; // 180 + uint8_t filter_hz; // 181 + uint8_t debug_rate; // 182 + uint8_t term_enable; // 183 + uint8_t reserved[8]; // 184-191 + } can; + }; + uint8_t buffer[192]; } EEprom_t; extern EEprom_t eepromBuffer; diff --git a/Inc/functions.h b/Inc/functions.h index 6334015a..f30f72eb 100644 --- a/Inc/functions.h +++ b/Inc/functions.h @@ -17,8 +17,8 @@ void delayMillis(uint32_t millis); long map(long x, long in_min, long in_max, long out_min, long out_max); #ifdef ARTERY void gpio_mode_QUICK(gpio_type* gpio_periph, uint32_t mode, - uint32_t pull_up_down, uint32_t pin); + uint32_t pull_up_down, uint32_t pin); void gpio_mode_set(gpio_type* gpio_periph, uint32_t mode, uint32_t pull_up_down, - uint32_t pin); + uint32_t pin); #endif #endif /* FUNCTIONS_H_ */ diff --git a/Inc/signal.h b/Inc/signal.h index 85679841..1aad9b6a 100644 --- a/Inc/signal.h +++ b/Inc/signal.h @@ -18,7 +18,7 @@ extern char crawler_mode; extern uint16_t ADC_raw_volts; extern uint16_t servo_low_threshold; // anything below this point considered 0 extern uint16_t - servo_high_threshold; // anything above this point considered 2000 (max) +servo_high_threshold; // anything above this point considered 2000 (max) extern uint16_t servo_neutral; extern uint8_t servo_dead_band; extern char inputSet; diff --git a/Inc/targets.h b/Inc/targets.h index 39d724ae..8bb120d6 100644 --- a/Inc/targets.h +++ b/Inc/targets.h @@ -2147,12 +2147,12 @@ #endif -#ifdef HARDWARE_GROUP_F0_045 +#ifdef HARDWARE_GROUP_F0_045 #define PHASE_A_COMP COMP_PA0 // pa0 #define PHASE_B_COMP COMP_PA4 // pa4 #define PHASE_C_COMP COMP_PA5 // pa5 #endif -#ifdef HARDWARE_GROUP_F0_504 +#ifdef HARDWARE_GROUP_F0_504 #define PHASE_A_COMP COMP_PA5 // pa5 #define PHASE_B_COMP COMP_PA0 // pa0 #define PHASE_C_COMP COMP_PA4 // pa4 @@ -2172,7 +2172,7 @@ #define PHASE_B_COMP COMP_PA0 // pa0 #define PHASE_C_COMP COMP_PA5 // pa5 #endif -#ifdef HARDWARE_GROUP_F0_540 +#ifdef HARDWARE_GROUP_F0_540 #define PHASE_A_COMP COMP_PA5 // pa5 #define PHASE_B_COMP COMP_PA4 // pa4 #define PHASE_C_COMP COMP_PA0 // pa0 diff --git a/Mcu/e230/Inc/IO.h b/Mcu/e230/Inc/IO.h index 352e164a..317300f5 100644 --- a/Mcu/e230/Inc/IO.h +++ b/Mcu/e230/Inc/IO.h @@ -33,6 +33,6 @@ extern uint8_t degrees_celsius; extern uint16_t ADC_raw_volts; extern uint16_t servo_low_threshold; // anything below this point considered 0 extern uint16_t - servo_high_threshold; // anything above this point considered 2000 (max) +servo_high_threshold; // anything above this point considered 2000 (max) extern uint16_t servo_neutral; extern uint8_t servo_dead_band; diff --git a/Mcu/e230/Inc/serial_telemetry.h b/Mcu/e230/Inc/serial_telemetry.h index b4f1c5cf..fb8cd3b9 100644 --- a/Mcu/e230/Inc/serial_telemetry.h +++ b/Mcu/e230/Inc/serial_telemetry.h @@ -11,7 +11,7 @@ #define SERIAL_TELEMETRY_H_ void makeTelemPackage(uint8_t temp, uint16_t voltage, uint16_t current, - uint16_t consumption, uint16_t e_rpm); + uint16_t consumption, uint16_t e_rpm); void telem_UART_Init(void); void send_telem_DMA(); diff --git a/Mcu/e230/Src/ADC.c b/Mcu/e230/Src/ADC.c index 0e4a25be..6dc265eb 100644 --- a/Mcu/e230/Src/ADC.c +++ b/Mcu/e230/Src/ADC.c @@ -20,87 +20,88 @@ extern uint16_t ADC_raw_current; extern uint16_t ADC_raw_input; void ADC_DMA_Callback() -{ // read dma buffer and set extern variables +{ + // read dma buffer and set extern variables #ifdef USE_ADC_INPUT - ADC_raw_temp = ADCDataDMA[3]; - ADC_raw_volts = ADCDataDMA[1] / 2; - ADC_raw_current = ADCDataDMA[2]; - ADC_raw_input = ADCDataDMA[0]; + ADC_raw_temp = ADCDataDMA[3]; + ADC_raw_volts = ADCDataDMA[1] / 2; + ADC_raw_current = ADCDataDMA[2]; + ADC_raw_input = ADCDataDMA[0]; #else - ADC_raw_temp = ADCDataDMA[2]; + ADC_raw_temp = ADCDataDMA[2]; #ifdef PA6_VOLTAGE - ADC_raw_volts = ADCDataDMA[1]; - ADC_raw_current = ADCDataDMA[0]; + ADC_raw_volts = ADCDataDMA[1]; + ADC_raw_current = ADCDataDMA[0]; #else - ADC_raw_volts = ADCDataDMA[0]; - ADC_raw_current = ADCDataDMA[1]; + ADC_raw_volts = ADCDataDMA[0]; + ADC_raw_current = ADCDataDMA[1]; #endif #endif } void ADC_Init(void) { - rcu_periph_clock_enable(RCU_GPIOA); - rcu_periph_clock_enable(RCU_ADC); - rcu_periph_clock_enable(RCU_DMA); - rcu_adc_clock_config(RCU_ADCCK_APB2_DIV6); - - /* config the GPIO as analog mode */ - gpio_mode_set(GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, - GPIO_PIN_3 | GPIO_PIN_6); - - dma_parameter_struct dma_data_parameter; - - /* ADC DMA_channel configuration */ - dma_deinit(DMA_CH0); - - /* initialize DMA single data mode */ - dma_data_parameter.periph_addr = (uint32_t)(&ADC_RDATA); - dma_data_parameter.periph_inc = DMA_PERIPH_INCREASE_DISABLE; - dma_data_parameter.memory_addr = (uint32_t)(&ADCDataDMA); - dma_data_parameter.memory_inc = DMA_MEMORY_INCREASE_ENABLE; - dma_data_parameter.periph_width = DMA_PERIPHERAL_WIDTH_16BIT; - dma_data_parameter.memory_width = DMA_MEMORY_WIDTH_16BIT; - dma_data_parameter.direction = DMA_PERIPHERAL_TO_MEMORY; - dma_data_parameter.number = 3U; - dma_data_parameter.priority = DMA_PRIORITY_LOW; - dma_init(DMA_CH0, &dma_data_parameter); - - dma_circulation_enable(DMA_CH0); - - /* enable DMA channel */ - dma_channel_enable(DMA_CH0); - - adc_special_function_config(ADC_CONTINUOUS_MODE, DISABLE); - /* ADC scan function enable */ - adc_special_function_config(ADC_SCAN_MODE, ENABLE); - /* ADC data alignment config */ - adc_data_alignment_config(ADC_DATAALIGN_RIGHT); - /* ADC channel length config */ - adc_channel_length_config(ADC_REGULAR_CHANNEL, 3U); - - adc_tempsensor_vrefint_enable(); // enable the temp sensor - - /* ADC regular channel config */ - adc_regular_channel_config(0, ADC_CHANNEL_3, ADC_SAMPLETIME_55POINT5); - adc_regular_channel_config(1, ADC_CHANNEL_6, ADC_SAMPLETIME_55POINT5); - adc_regular_channel_config(2, ADC_CHANNEL_16, ADC_SAMPLETIME_55POINT5); - - /* ADC trigger config */ - adc_external_trigger_source_config(ADC_REGULAR_CHANNEL, - ADC_EXTTRIG_REGULAR_NONE); - adc_external_trigger_config(ADC_REGULAR_CHANNEL, ENABLE); - - /* enable ADC interface */ - adc_enable(); - delayMicros(1000); - /* ADC calibration and reset calibration */ - adc_calibration_enable(); - - /* ADC DMA function enable */ - adc_dma_mode_enable(); - /* ADC software trigger enable */ - adc_software_trigger_enable(ADC_REGULAR_CHANNEL); + rcu_periph_clock_enable(RCU_GPIOA); + rcu_periph_clock_enable(RCU_ADC); + rcu_periph_clock_enable(RCU_DMA); + rcu_adc_clock_config(RCU_ADCCK_APB2_DIV6); + + /* config the GPIO as analog mode */ + gpio_mode_set(GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, + GPIO_PIN_3 | GPIO_PIN_6); + + dma_parameter_struct dma_data_parameter; + + /* ADC DMA_channel configuration */ + dma_deinit(DMA_CH0); + + /* initialize DMA single data mode */ + dma_data_parameter.periph_addr = (uint32_t)(&ADC_RDATA); + dma_data_parameter.periph_inc = DMA_PERIPH_INCREASE_DISABLE; + dma_data_parameter.memory_addr = (uint32_t)(&ADCDataDMA); + dma_data_parameter.memory_inc = DMA_MEMORY_INCREASE_ENABLE; + dma_data_parameter.periph_width = DMA_PERIPHERAL_WIDTH_16BIT; + dma_data_parameter.memory_width = DMA_MEMORY_WIDTH_16BIT; + dma_data_parameter.direction = DMA_PERIPHERAL_TO_MEMORY; + dma_data_parameter.number = 3U; + dma_data_parameter.priority = DMA_PRIORITY_LOW; + dma_init(DMA_CH0, &dma_data_parameter); + + dma_circulation_enable(DMA_CH0); + + /* enable DMA channel */ + dma_channel_enable(DMA_CH0); + + adc_special_function_config(ADC_CONTINUOUS_MODE, DISABLE); + /* ADC scan function enable */ + adc_special_function_config(ADC_SCAN_MODE, ENABLE); + /* ADC data alignment config */ + adc_data_alignment_config(ADC_DATAALIGN_RIGHT); + /* ADC channel length config */ + adc_channel_length_config(ADC_REGULAR_CHANNEL, 3U); + + adc_tempsensor_vrefint_enable(); // enable the temp sensor + + /* ADC regular channel config */ + adc_regular_channel_config(0, ADC_CHANNEL_3, ADC_SAMPLETIME_55POINT5); + adc_regular_channel_config(1, ADC_CHANNEL_6, ADC_SAMPLETIME_55POINT5); + adc_regular_channel_config(2, ADC_CHANNEL_16, ADC_SAMPLETIME_55POINT5); + + /* ADC trigger config */ + adc_external_trigger_source_config(ADC_REGULAR_CHANNEL, + ADC_EXTTRIG_REGULAR_NONE); + adc_external_trigger_config(ADC_REGULAR_CHANNEL, ENABLE); + + /* enable ADC interface */ + adc_enable(); + delayMicros(1000); + /* ADC calibration and reset calibration */ + adc_calibration_enable(); + + /* ADC DMA function enable */ + adc_dma_mode_enable(); + /* ADC software trigger enable */ + adc_software_trigger_enable(ADC_REGULAR_CHANNEL); } diff --git a/Mcu/e230/Src/IO.c b/Mcu/e230/Src/IO.c index 471f5d97..cad96693 100644 --- a/Mcu/e230/Src/IO.c +++ b/Mcu/e230/Src/IO.c @@ -22,61 +22,67 @@ uint16_t change_time = 0; void receiveDshotDma() { - RCU_REG_VAL(RCU_TIMER2RST) |= BIT(RCU_BIT_POS(RCU_TIMER2RST)); - RCU_REG_VAL(RCU_TIMER2RST) &= ~BIT(RCU_BIT_POS(RCU_TIMER2RST)); - TIMER_CHCTL0(IC_TIMER_REGISTER) = 0x41; - TIMER_CHCTL2(IC_TIMER_REGISTER) = 0xa; - TIMER_PSC(IC_TIMER_REGISTER) = ic_timer_prescaler; - TIMER_CAR(IC_TIMER_REGISTER) = 0xFFFF; - TIMER_SWEVG(IC_TIMER_REGISTER) |= (uint32_t)TIMER_EVENT_SRC_UPG; - out_put = 0; - TIMER_CNT(IC_TIMER_REGISTER) = 0; - DMA_CHMADDR(INPUT_DMA_CHANNEL) = (uint32_t)&dma_buffer; - DMA_CHCNT(INPUT_DMA_CHANNEL) = (buffersize & DMA_CHANNEL_CNT_MASK); - TIMER_DMAINTEN(IC_TIMER_REGISTER) |= (uint32_t)TIMER_DMA_CH0D; - TIMER_CHCTL2(IC_TIMER_REGISTER) |= (uint32_t)TIMER_CCX_ENABLE; - TIMER_CTL0(IC_TIMER_REGISTER) |= (uint32_t)TIMER_CTL0_CEN; - DMA_CHCTL(INPUT_DMA_CHANNEL) = 0x0000098b; // just set the whole reg in one go to enable + RCU_REG_VAL(RCU_TIMER2RST) |= BIT(RCU_BIT_POS(RCU_TIMER2RST)); + RCU_REG_VAL(RCU_TIMER2RST) &= ~BIT(RCU_BIT_POS(RCU_TIMER2RST)); + TIMER_CHCTL0(IC_TIMER_REGISTER) = 0x41; + TIMER_CHCTL2(IC_TIMER_REGISTER) = 0xa; + TIMER_PSC(IC_TIMER_REGISTER) = ic_timer_prescaler; + TIMER_CAR(IC_TIMER_REGISTER) = 0xFFFF; + TIMER_SWEVG(IC_TIMER_REGISTER) |= (uint32_t)TIMER_EVENT_SRC_UPG; + out_put = 0; + TIMER_CNT(IC_TIMER_REGISTER) = 0; + DMA_CHMADDR(INPUT_DMA_CHANNEL) = (uint32_t)&dma_buffer; + DMA_CHCNT(INPUT_DMA_CHANNEL) = (buffersize & DMA_CHANNEL_CNT_MASK); + TIMER_DMAINTEN(IC_TIMER_REGISTER) |= (uint32_t)TIMER_DMA_CH0D; + TIMER_CHCTL2(IC_TIMER_REGISTER) |= (uint32_t)TIMER_CCX_ENABLE; + TIMER_CTL0(IC_TIMER_REGISTER) |= (uint32_t)TIMER_CTL0_CEN; + DMA_CHCTL(INPUT_DMA_CHANNEL) = 0x0000098b; // just set the whole reg in one go to enable } void sendDshotDma() { - RCU_REG_VAL(RCU_TIMER2RST) |= BIT(RCU_BIT_POS(RCU_TIMER2RST)); - RCU_REG_VAL(RCU_TIMER2RST) &= ~BIT(RCU_BIT_POS(RCU_TIMER2RST)); - TIMER_CHCTL0(IC_TIMER_REGISTER) = 0x60; - TIMER_CHCTL2(IC_TIMER_REGISTER) = 0x3; - TIMER_PSC(IC_TIMER_REGISTER) = output_timer_prescaler; - TIMER_CAR(IC_TIMER_REGISTER) = 100; - out_put = 1; - TIMER_SWEVG(IC_TIMER_REGISTER) |= (uint32_t)TIMER_EVENT_SRC_UPG; - DMA_CHMADDR(INPUT_DMA_CHANNEL) = (uint32_t)&gcr; - DMA_CHCNT(INPUT_DMA_CHANNEL) = ((23 + buffer_padding) & DMA_CHANNEL_CNT_MASK); - DMA_CHCTL(INPUT_DMA_CHANNEL) = 0x0000099b; - TIMER_DMAINTEN(IC_TIMER_REGISTER) |= (uint32_t)TIMER_DMA_CH0D; - TIMER_CHCTL2(IC_TIMER_REGISTER) |= (uint32_t)TIMER_CCX_ENABLE; - TIMER_CCHP(IC_TIMER_REGISTER) |= (uint32_t)TIMER_CCHP_POEN; - TIMER_CTL0(IC_TIMER_REGISTER) |= (uint32_t)TIMER_CTL0_CEN; + RCU_REG_VAL(RCU_TIMER2RST) |= BIT(RCU_BIT_POS(RCU_TIMER2RST)); + RCU_REG_VAL(RCU_TIMER2RST) &= ~BIT(RCU_BIT_POS(RCU_TIMER2RST)); + TIMER_CHCTL0(IC_TIMER_REGISTER) = 0x60; + TIMER_CHCTL2(IC_TIMER_REGISTER) = 0x3; + TIMER_PSC(IC_TIMER_REGISTER) = output_timer_prescaler; + TIMER_CAR(IC_TIMER_REGISTER) = 100; + out_put = 1; + TIMER_SWEVG(IC_TIMER_REGISTER) |= (uint32_t)TIMER_EVENT_SRC_UPG; + DMA_CHMADDR(INPUT_DMA_CHANNEL) = (uint32_t)&gcr; + DMA_CHCNT(INPUT_DMA_CHANNEL) = ((23 + buffer_padding) & DMA_CHANNEL_CNT_MASK); + DMA_CHCTL(INPUT_DMA_CHANNEL) = 0x0000099b; + TIMER_DMAINTEN(IC_TIMER_REGISTER) |= (uint32_t)TIMER_DMA_CH0D; + TIMER_CHCTL2(IC_TIMER_REGISTER) |= (uint32_t)TIMER_CCX_ENABLE; + TIMER_CCHP(IC_TIMER_REGISTER) |= (uint32_t)TIMER_CCHP_POEN; + TIMER_CTL0(IC_TIMER_REGISTER) |= (uint32_t)TIMER_CTL0_CEN; } -uint8_t getInputPinState() { return GPIO_ISTAT(INPUT_PIN_PORT) & (INPUT_PIN); } +uint8_t getInputPinState() +{ + return GPIO_ISTAT(INPUT_PIN_PORT) & (INPUT_PIN); +} void setInputPolarityRising() { - TIMER_CHCTL2(IC_TIMER_REGISTER) |= (uint32_t)(TIMER_IC_POLARITY_RISING); + TIMER_CHCTL2(IC_TIMER_REGISTER) |= (uint32_t)(TIMER_IC_POLARITY_RISING); } void setInputPullDown() { - gpio_mode_set(GPIOB, GPIO_MODE_AF, GPIO_PUPD_PULLDOWN, INPUT_PIN); + gpio_mode_set(GPIOB, GPIO_MODE_AF, GPIO_PUPD_PULLDOWN, INPUT_PIN); } void setInputPullUp() { - gpio_mode_set(GPIOB, GPIO_MODE_AF, GPIO_PUPD_PULLUP, INPUT_PIN); + gpio_mode_set(GPIOB, GPIO_MODE_AF, GPIO_PUPD_PULLUP, INPUT_PIN); } -void enableHalfTransferInt() { DMA_CHCTL(INPUT_DMA_CHANNEL) |= DMA_INT_HTF; } +void enableHalfTransferInt() +{ + DMA_CHCTL(INPUT_DMA_CHANNEL) |= DMA_INT_HTF; +} void setInputPullNone() { - gpio_mode_set(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, INPUT_PIN); + gpio_mode_set(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, INPUT_PIN); } diff --git a/Mcu/e230/Src/comparator.c b/Mcu/e230/Src/comparator.c index 33925263..8c6a5bcc 100644 --- a/Mcu/e230/Src/comparator.c +++ b/Mcu/e230/Src/comparator.c @@ -9,52 +9,55 @@ #include "targets.h" -uint8_t getCompOutputLevel() { return cmp_output_level_get(); } +uint8_t getCompOutputLevel() +{ + return cmp_output_level_get(); +} void maskPhaseInterrupts() { - // EXTI->IMR &= (0 << 21); - // LL_EXTI_ClearFlag_0_31(EXTI_LINE); + // EXTI->IMR &= (0 << 21); + // LL_EXTI_ClearFlag_0_31(EXTI_LINE); - EXTI_INTEN &= ~(uint32_t)EXTI_LINE; - EXTI_PD = (uint32_t)EXTI_LINE; + EXTI_INTEN &= ~(uint32_t)EXTI_LINE; + EXTI_PD = (uint32_t)EXTI_LINE; } void enableCompInterrupts() { - // EXTI->IMR |= (1 << 21); - EXTI_INTEN |= (uint32_t)EXTI_LINE; + // EXTI->IMR |= (1 << 21); + EXTI_INTEN |= (uint32_t)EXTI_LINE; } void changeCompInput() { - // TIM3->CNT = 0; - // HAL_COMP_Stop_IT(&hcomp1); // done in comparator interrupt - // routine - - if (step == 1 || step == 4) { // c floating - CMP_CS = PHASE_C_COMP; - // cmp_mode_init(CMP_HIGHSPEED, PHASE_C_COMP, CMP_HYSTERESIS_NO); - } - if (step == 2 || step == 5) { // a floating - CMP_CS = PHASE_A_COMP; - // cmp_mode_init(CMP_HIGHSPEED, PHASE_A_COMP, CMP_HYSTERESIS_NO); - } - if (step == 3 || step == 6) { // b floating - CMP_CS = PHASE_B_COMP; - // cmp_mode_init(CMP_HIGHSPEED, PHASE_B_COMP, - // CMP_HYSTERESIS_NO); - } - if (rising) { - // EXTI->RTSR = 0x0; - // EXTI->FTSR = 0x200000; - EXTI_RTEN &= ~(uint32_t)EXTI_LINE; - EXTI_FTEN |= (uint32_t)EXTI_LINE; - } else { - // falling bemf - // EXTI->FTSR = 0x0; - // EXTI->RTSR = 0x200000; - EXTI_RTEN |= (uint32_t)EXTI_LINE; - EXTI_FTEN &= ~(uint32_t)EXTI_LINE; - } + // TIM3->CNT = 0; + // HAL_COMP_Stop_IT(&hcomp1); // done in comparator interrupt + // routine + + if (step == 1 || step == 4) { // c floating + CMP_CS = PHASE_C_COMP; + // cmp_mode_init(CMP_HIGHSPEED, PHASE_C_COMP, CMP_HYSTERESIS_NO); + } + if (step == 2 || step == 5) { // a floating + CMP_CS = PHASE_A_COMP; + // cmp_mode_init(CMP_HIGHSPEED, PHASE_A_COMP, CMP_HYSTERESIS_NO); + } + if (step == 3 || step == 6) { // b floating + CMP_CS = PHASE_B_COMP; + // cmp_mode_init(CMP_HIGHSPEED, PHASE_B_COMP, + // CMP_HYSTERESIS_NO); + } + if (rising) { + // EXTI->RTSR = 0x0; + // EXTI->FTSR = 0x200000; + EXTI_RTEN &= ~(uint32_t)EXTI_LINE; + EXTI_FTEN |= (uint32_t)EXTI_LINE; + } else { + // falling bemf + // EXTI->FTSR = 0x0; + // EXTI->RTSR = 0x200000; + EXTI_RTEN |= (uint32_t)EXTI_LINE; + EXTI_FTEN &= ~(uint32_t)EXTI_LINE; + } } diff --git a/Mcu/e230/Src/eeprom.c b/Mcu/e230/Src/eeprom.c index 68045e8e..d0099aa5 100644 --- a/Mcu/e230/Src/eeprom.c +++ b/Mcu/e230/Src/eeprom.c @@ -11,36 +11,37 @@ // uint32_t FLASH_FKEY2 =0xCDEF89AB; void save_flash_nolib(uint8_t* data, int length, uint32_t add) -{ /// todo - uint32_t data_to_FLASH[length / 4]; - memset(data_to_FLASH, 0, length / 4); - for (int i = 0; i < length / 4; i++) { - data_to_FLASH[i] = data[i * 4 + 3] << 24 | data[i * 4 + 2] << 16 | data[i * 4 + 1] << 8 | data[i * 4]; // make 16 bit - } - volatile uint32_t data_length = length / 4; - - // unlock flash - - fmc_unlock(); - - // erase page if address even divisable by 1024 - if ((add % 1024) == 0) { - fmc_page_erase(add); - } - - volatile uint32_t index = 0; - while (index < data_length) { - fmc_word_program(add + (index * 4), data_to_FLASH[index]); - fmc_flag_clear(FMC_FLAG_END | FMC_FLAG_WPERR | FMC_FLAG_PGERR); - index++; - } - fmc_lock(); +{ + /// todo + uint32_t data_to_FLASH[length / 4]; + memset(data_to_FLASH, 0, length / 4); + for (int i = 0; i < length / 4; i++) { + data_to_FLASH[i] = data[i * 4 + 3] << 24 | data[i * 4 + 2] << 16 | data[i * 4 + 1] << 8 | data[i * 4]; // make 16 bit + } + volatile uint32_t data_length = length / 4; + + // unlock flash + + fmc_unlock(); + + // erase page if address even divisable by 1024 + if ((add % 1024) == 0) { + fmc_page_erase(add); + } + + volatile uint32_t index = 0; + while (index < data_length) { + fmc_word_program(add + (index * 4), data_to_FLASH[index]); + fmc_flag_clear(FMC_FLAG_END | FMC_FLAG_WPERR | FMC_FLAG_PGERR); + index++; + } + fmc_lock(); } void read_flash_bin(uint8_t* data, uint32_t add, int out_buff_len) { - // volatile uint32_t read_data; - for (int i = 0; i < out_buff_len; i++) { - data[i] = *(uint8_t*)(add + i); - } + // volatile uint32_t read_data; + for (int i = 0; i < out_buff_len; i++) { + data[i] = *(uint8_t*)(add + i); + } } diff --git a/Mcu/e230/Src/gd32e23x_it.c b/Mcu/e230/Src/gd32e23x_it.c index 9089db01..7a1f1e50 100644 --- a/Mcu/e230/Src/gd32e23x_it.c +++ b/Mcu/e230/Src/gd32e23x_it.c @@ -41,9 +41,9 @@ void NMI_Handler(void) { } */ void HardFault_Handler(void) { - /* if Hard Fault exception occurs, go to infinite loop */ - while (1) { - } + /* if Hard Fault exception occurs, go to infinite loop */ + while (1) { + } } /*! @@ -68,38 +68,41 @@ void PendSV_Handler(void) { } \param[out] none \retval none */ -void SysTick_Handler(void) { delay_decrement(); } +void SysTick_Handler(void) +{ + delay_decrement(); +} void DMA_Channel3_4_IRQHandler(void) { - if (dshot_telemetry && armed) { - DMA_INTC |= DMA_FLAG_ADD(DMA_INT_FLAG_G, DMA_CH3); - DMA_CHCTL(DMA_CH3) &= ~DMA_CHXCTL_CHEN; - if (out_put) { - receiveDshotDma(); - compute_dshot_flag = 2; - } else { - sendDshotDma(); - compute_dshot_flag = 1; - } - EXTI_SWIEV |= (uint32_t)EXTI_15; - return; - } - - if (dma_interrupt_flag_get(DMA_CH3, DMA_INT_FLAG_HTF)) { - if (servoPwm) { - TIMER_CHCTL2(TIMER2) |= (uint32_t)(TIMER_IC_POLARITY_FALLING); - dma_interrupt_flag_clear(DMA_CH3, DMA_INT_FLAG_HTF); - } + if (dshot_telemetry && armed) { + DMA_INTC |= DMA_FLAG_ADD(DMA_INT_FLAG_G, DMA_CH3); + DMA_CHCTL(DMA_CH3) &= ~DMA_CHXCTL_CHEN; + if (out_put) { + receiveDshotDma(); + compute_dshot_flag = 2; + } else { + sendDshotDma(); + compute_dshot_flag = 1; } - if (dma_interrupt_flag_get(DMA_CH3, DMA_INT_FLAG_FTF) == 1) { - dma_interrupt_flag_clear(DMA_CH3, DMA_INT_FLAG_G); - dma_channel_disable(DMA_CH3); - transfercomplete(); - EXTI_SWIEV |= (uint32_t)EXTI_15; - } else if (dma_interrupt_flag_get(DMA_CH3, DMA_INT_FLAG_ERR) == 1) { - dma_interrupt_flag_clear(DMA_CH3, DMA_INT_FLAG_G); + EXTI_SWIEV |= (uint32_t)EXTI_15; + return; + } + + if (dma_interrupt_flag_get(DMA_CH3, DMA_INT_FLAG_HTF)) { + if (servoPwm) { + TIMER_CHCTL2(TIMER2) |= (uint32_t)(TIMER_IC_POLARITY_FALLING); + dma_interrupt_flag_clear(DMA_CH3, DMA_INT_FLAG_HTF); } + } + if (dma_interrupt_flag_get(DMA_CH3, DMA_INT_FLAG_FTF) == 1) { + dma_interrupt_flag_clear(DMA_CH3, DMA_INT_FLAG_G); + dma_channel_disable(DMA_CH3); + transfercomplete(); + EXTI_SWIEV |= (uint32_t)EXTI_15; + } else if (dma_interrupt_flag_get(DMA_CH3, DMA_INT_FLAG_ERR) == 1) { + dma_interrupt_flag_clear(DMA_CH3, DMA_INT_FLAG_G); + } } /** @@ -108,10 +111,10 @@ void DMA_Channel3_4_IRQHandler(void) */ void ADC_CMP_IRQHandler(void) { - if (exti_interrupt_flag_get(EXTI_21)) { - exti_flag_clear(EXTI_21); - interruptRoutine(); - } + if (exti_interrupt_flag_get(EXTI_21)) { + exti_flag_clear(EXTI_21); + interruptRoutine(); + } } /** @@ -119,8 +122,8 @@ void ADC_CMP_IRQHandler(void) */ void TIMER13_IRQHandler(void) { - timer_interrupt_flag_clear(TIMER13, TIMER_INT_FLAG_UP); - tenKhzRoutine(); + timer_interrupt_flag_clear(TIMER13, TIMER_INT_FLAG_UP); + tenKhzRoutine(); } /** @@ -128,13 +131,16 @@ void TIMER13_IRQHandler(void) */ void TIMER15_IRQHandler(void) { - interrupt_time = TIMER_CNT(UTILITY_TIMER); - timer_interrupt_flag_clear(TIMER15, TIMER_INT_FLAG_UP); - PeriodElapsedCallback(); - interrupt_time = ((uint16_t)TIMER_CNT(UTILITY_TIMER)) - interrupt_time; + interrupt_time = TIMER_CNT(UTILITY_TIMER); + timer_interrupt_flag_clear(TIMER15, TIMER_INT_FLAG_UP); + PeriodElapsedCallback(); + interrupt_time = ((uint16_t)TIMER_CNT(UTILITY_TIMER)) - interrupt_time; } -void TIMER14_IRQHandler(void) { timer_flag_clear(TIMER14, TIMER_FLAG_UP); } +void TIMER14_IRQHandler(void) +{ + timer_flag_clear(TIMER14, TIMER_FLAG_UP); +} /** * @brief This function handles USART1 global interrupt / USART1 wake-up @@ -144,12 +150,12 @@ void USART1_IRQHandler(void) { } void TIMER2_IRQHandler(void) { - // sendDshotDma(); + // sendDshotDma(); } void EXTI4_15_IRQHandler(void) { - exti_flag_clear(EXTI_15); + exti_flag_clear(EXTI_15); - processDshot(); + processDshot(); } diff --git a/Mcu/e230/Src/peripherals.c b/Mcu/e230/Src/peripherals.c index a65c9c13..7e43f1ad 100644 --- a/Mcu/e230/Src/peripherals.c +++ b/Mcu/e230/Src/peripherals.c @@ -18,393 +18,402 @@ void initCorePeripherals(void) { #ifdef PA11_12_REMAP - SYSCFG_CFG0 |= SYSCFG_PA11_REMAP_PA12; + SYSCFG_CFG0 |= SYSCFG_PA11_REMAP_PA12; #endif - // fmc_wscnt_set(2); - // fmc_prefetch_enable(); - MX_GPIO_Init(); - MX_DMA_Init(); - TIM0_Init(); - TIMER5_Init(); - TIMER16_Init(); - COMP_Init(); - TIMER15_Init(); - TIMER13_Init(); - - // UN_TIM_Init(); + // fmc_wscnt_set(2); + // fmc_prefetch_enable(); + MX_GPIO_Init(); + MX_DMA_Init(); + TIM0_Init(); + TIMER5_Init(); + TIMER16_Init(); + COMP_Init(); + TIMER15_Init(); + TIMER13_Init(); + + // UN_TIM_Init(); #ifdef USE_SERIAL_TELEMETRY - telem_UART_Init(); + telem_UART_Init(); #endif } void COMP_Init(void) { - rcu_periph_clock_enable(RCU_GPIOA); + rcu_periph_clock_enable(RCU_GPIOA); - /* configure PA1 as comparator input */ - gpio_mode_set(GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO_PIN_1); - rcu_periph_clock_enable(RCU_CFGCMP); - gpio_mode_set(GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO_PIN_5); - /* configure comparator channel0 */ - cmp_mode_init(CMP_HIGHSPEED, CMP_PA5, CMP_HYSTERESIS_NO); + /* configure PA1 as comparator input */ + gpio_mode_set(GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO_PIN_1); + rcu_periph_clock_enable(RCU_CFGCMP); + gpio_mode_set(GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO_PIN_5); + /* configure comparator channel0 */ + cmp_mode_init(CMP_HIGHSPEED, CMP_PA5, CMP_HYSTERESIS_NO); - // cmp_enable(); - // delay_1ms(1); + // cmp_enable(); + // delay_1ms(1); - cmp_output_init(CMP_OUTPUT_NONE, CMP_OUTPUT_POLARITY_NOINVERTED); + cmp_output_init(CMP_OUTPUT_NONE, CMP_OUTPUT_POLARITY_NOINVERTED); - /* initialize exti line21 */ - exti_init(EXTI_21, EXTI_INTERRUPT, EXTI_TRIG_RISING); - exti_interrupt_flag_clear(EXTI_21); - /* configure ADC_CMP NVIC */ - nvic_irq_enable(ADC_CMP_IRQn, 0); + /* initialize exti line21 */ + exti_init(EXTI_21, EXTI_INTERRUPT, EXTI_TRIG_RISING); + exti_interrupt_flag_clear(EXTI_21); + /* configure ADC_CMP NVIC */ + nvic_irq_enable(ADC_CMP_IRQn, 0); } void MX_IWDG_Init(void) { - fwdgt_config(4000, FWDGT_PSC_DIV16); - fwdgt_enable(); + fwdgt_config(4000, FWDGT_PSC_DIV16); + fwdgt_enable(); } -void reloadWatchDogCounter() { fwdgt_counter_reload(); } +void reloadWatchDogCounter() +{ + fwdgt_counter_reload(); +} void TIM0_Init(void) { - timer_oc_parameter_struct timer_ocinitpara; - timer_parameter_struct timer_initpara; - timer_break_parameter_struct timer_breakpara; - - rcu_periph_clock_enable(RCU_TIMER0); - // TIMER_CAR(TIMER0) = 3000; - // TIMER_PSC(TIMER0) = 0; - - timer_deinit(TIMER0); - /* initialize TIMER init parameter struct */ - timer_struct_para_init(&timer_initpara); - /* TIMER0 configuration */ - timer_initpara.prescaler = 0; - timer_initpara.alignedmode = TIMER_COUNTER_EDGE; - timer_initpara.counterdirection = TIMER_COUNTER_UP; - timer_initpara.period = TIM1_AUTORELOAD; - timer_initpara.clockdivision = TIMER_CKDIV_DIV1; - timer_initpara.repetitioncounter = 0; - timer_init(TIMER0, &timer_initpara); - - timer_channel_output_struct_para_init(&timer_ocinitpara); - /* CH0/CH0N, CH1/CH1N and CH2/CH2N configuration in timing mode */ - timer_ocinitpara.outputstate = TIMER_CCX_DISABLE; - timer_ocinitpara.outputnstate = TIMER_CCXN_DISABLE; - timer_ocinitpara.ocpolarity = TIMER_OC_POLARITY_HIGH; - timer_ocinitpara.ocnpolarity = TIMER_OCN_POLARITY_HIGH; - timer_ocinitpara.ocidlestate = TIMER_OC_IDLE_STATE_LOW; - timer_ocinitpara.ocnidlestate = TIMER_OCN_IDLE_STATE_LOW; - - timer_channel_output_config(TIMER0, TIMER_CH_0, &timer_ocinitpara); - timer_channel_output_config(TIMER0, TIMER_CH_1, &timer_ocinitpara); - timer_channel_output_config(TIMER0, TIMER_CH_2, &timer_ocinitpara); - - /* configure TIMER channel 0 */ + timer_oc_parameter_struct timer_ocinitpara; + timer_parameter_struct timer_initpara; + timer_break_parameter_struct timer_breakpara; + + rcu_periph_clock_enable(RCU_TIMER0); + // TIMER_CAR(TIMER0) = 3000; + // TIMER_PSC(TIMER0) = 0; + + timer_deinit(TIMER0); + /* initialize TIMER init parameter struct */ + timer_struct_para_init(&timer_initpara); + /* TIMER0 configuration */ + timer_initpara.prescaler = 0; + timer_initpara.alignedmode = TIMER_COUNTER_EDGE; + timer_initpara.counterdirection = TIMER_COUNTER_UP; + timer_initpara.period = TIM1_AUTORELOAD; + timer_initpara.clockdivision = TIMER_CKDIV_DIV1; + timer_initpara.repetitioncounter = 0; + timer_init(TIMER0, &timer_initpara); + + timer_channel_output_struct_para_init(&timer_ocinitpara); + /* CH0/CH0N, CH1/CH1N and CH2/CH2N configuration in timing mode */ + timer_ocinitpara.outputstate = TIMER_CCX_DISABLE; + timer_ocinitpara.outputnstate = TIMER_CCXN_DISABLE; + timer_ocinitpara.ocpolarity = TIMER_OC_POLARITY_HIGH; + timer_ocinitpara.ocnpolarity = TIMER_OCN_POLARITY_HIGH; + timer_ocinitpara.ocidlestate = TIMER_OC_IDLE_STATE_LOW; + timer_ocinitpara.ocnidlestate = TIMER_OCN_IDLE_STATE_LOW; + + timer_channel_output_config(TIMER0, TIMER_CH_0, &timer_ocinitpara); + timer_channel_output_config(TIMER0, TIMER_CH_1, &timer_ocinitpara); + timer_channel_output_config(TIMER0, TIMER_CH_2, &timer_ocinitpara); + + /* configure TIMER channel 0 */ #ifdef HARDWARE_GROUP_GD_B // reversed pa7 pa8 - timer_channel_output_mode_config(TIMER0, TIMER_CH_0, TIMER_OC_MODE_PWM1); + timer_channel_output_mode_config(TIMER0, TIMER_CH_0, TIMER_OC_MODE_PWM1); #else - timer_channel_output_mode_config(TIMER0, TIMER_CH_0, TIMER_OC_MODE_PWM0); + timer_channel_output_mode_config(TIMER0, TIMER_CH_0, TIMER_OC_MODE_PWM0); #endif - timer_channel_output_shadow_config(TIMER0, TIMER_CH_0, - TIMER_OC_SHADOW_ENABLE); - - /* configure TIMER channel 1 */ - - timer_channel_output_mode_config(TIMER0, TIMER_CH_1, TIMER_OC_MODE_PWM0); - timer_channel_output_shadow_config(TIMER0, TIMER_CH_1, - TIMER_OC_SHADOW_ENABLE); - - /* configure TIMER channel 2 */ - - timer_channel_output_mode_config(TIMER0, TIMER_CH_2, TIMER_OC_MODE_PWM0); - timer_channel_output_shadow_config(TIMER0, TIMER_CH_2, - TIMER_OC_SHADOW_ENABLE); - - timer_break_struct_para_init(&timer_breakpara); - /* automatic output enable, break, dead time and lock configuration*/ - timer_breakpara.runoffstate = TIMER_ROS_STATE_DISABLE; - timer_breakpara.ideloffstate = TIMER_IOS_STATE_DISABLE; - timer_breakpara.deadtime = DEAD_TIME; - timer_breakpara.breakpolarity = TIMER_BREAK_POLARITY_HIGH; - timer_breakpara.outputautostate = TIMER_OUTAUTO_DISABLE; - timer_breakpara.protectmode = TIMER_CCHP_PROT_OFF; - timer_breakpara.breakstate = TIMER_BREAK_DISABLE; - timer_break_config(TIMER0, &timer_breakpara); - - // timer_primary_output_config(TIMER0, ENABLE); - - /* TIMER0 channel control update interrupt enable */ - timer_interrupt_disable(TIMER0, TIMER_INT_CMT); - /* TIMER0 break interrupt disable */ - timer_interrupt_disable(TIMER0, TIMER_INT_BRK); - - /* TIMER0 counter enable */ - // timer_enable(TIMER0); - timer_auto_reload_shadow_enable(TIMER0); - - rcu_periph_clock_enable(RCU_GPIOA); - rcu_periph_clock_enable(RCU_GPIOB); - - /*configure PA8/PA9/PA10(TIMER0/CH0/CH1/CH2) as alternate function*/ - gpio_mode_set(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_AF, GPIO_PUPD_NONE, - PHASE_A_GPIO_LOW); - gpio_output_options_set(PHASE_A_GPIO_PORT_LOW, GPIO_OTYPE_PP, - GPIO_OSPEED_50MHZ, PHASE_A_GPIO_LOW); - - gpio_mode_set(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_AF, GPIO_PUPD_NONE, - PHASE_B_GPIO_LOW); - gpio_output_options_set(PHASE_B_GPIO_PORT_LOW, GPIO_OTYPE_PP, - GPIO_OSPEED_50MHZ, PHASE_B_GPIO_LOW); - - gpio_mode_set(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_AF, GPIO_PUPD_NONE, - PHASE_C_GPIO_LOW); - gpio_output_options_set(PHASE_C_GPIO_PORT_LOW, GPIO_OTYPE_PP, - GPIO_OSPEED_50MHZ, PHASE_C_GPIO_LOW); - - gpio_af_set(PHASE_A_GPIO_PORT_LOW, GPIO_AF_2, PHASE_A_GPIO_LOW); - gpio_af_set(PHASE_B_GPIO_PORT_LOW, GPIO_AF_2, PHASE_B_GPIO_LOW); - gpio_af_set(PHASE_C_GPIO_PORT_LOW, GPIO_AF_2, PHASE_C_GPIO_LOW); - - /*configure PB13/PB14/PB15(TIMER0/CH0N/CH1N/CH2N) as alternate function*/ - gpio_mode_set(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_AF, GPIO_PUPD_NONE, - PHASE_A_GPIO_HIGH); - gpio_output_options_set(GPIOB, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, - PHASE_A_GPIO_HIGH); - - gpio_mode_set(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_AF, GPIO_PUPD_NONE, - PHASE_B_GPIO_HIGH); - gpio_output_options_set(GPIOB, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, - PHASE_B_GPIO_HIGH); - - gpio_mode_set(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_AF, GPIO_PUPD_NONE, - PHASE_C_GPIO_HIGH); - gpio_output_options_set(GPIOB, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, - PHASE_C_GPIO_HIGH); - - gpio_af_set(PHASE_A_GPIO_PORT_HIGH, GPIO_AF_2, PHASE_A_GPIO_HIGH); - gpio_af_set(PHASE_B_GPIO_PORT_HIGH, GPIO_AF_2, PHASE_B_GPIO_HIGH); - gpio_af_set(PHASE_C_GPIO_PORT_HIGH, GPIO_AF_2, PHASE_C_GPIO_HIGH); + timer_channel_output_shadow_config(TIMER0, TIMER_CH_0, + TIMER_OC_SHADOW_ENABLE); + + /* configure TIMER channel 1 */ + + timer_channel_output_mode_config(TIMER0, TIMER_CH_1, TIMER_OC_MODE_PWM0); + timer_channel_output_shadow_config(TIMER0, TIMER_CH_1, + TIMER_OC_SHADOW_ENABLE); + + /* configure TIMER channel 2 */ + + timer_channel_output_mode_config(TIMER0, TIMER_CH_2, TIMER_OC_MODE_PWM0); + timer_channel_output_shadow_config(TIMER0, TIMER_CH_2, + TIMER_OC_SHADOW_ENABLE); + + timer_break_struct_para_init(&timer_breakpara); + /* automatic output enable, break, dead time and lock configuration*/ + timer_breakpara.runoffstate = TIMER_ROS_STATE_DISABLE; + timer_breakpara.ideloffstate = TIMER_IOS_STATE_DISABLE; + timer_breakpara.deadtime = DEAD_TIME; + timer_breakpara.breakpolarity = TIMER_BREAK_POLARITY_HIGH; + timer_breakpara.outputautostate = TIMER_OUTAUTO_DISABLE; + timer_breakpara.protectmode = TIMER_CCHP_PROT_OFF; + timer_breakpara.breakstate = TIMER_BREAK_DISABLE; + timer_break_config(TIMER0, &timer_breakpara); + + // timer_primary_output_config(TIMER0, ENABLE); + + /* TIMER0 channel control update interrupt enable */ + timer_interrupt_disable(TIMER0, TIMER_INT_CMT); + /* TIMER0 break interrupt disable */ + timer_interrupt_disable(TIMER0, TIMER_INT_BRK); + + /* TIMER0 counter enable */ + // timer_enable(TIMER0); + timer_auto_reload_shadow_enable(TIMER0); + + rcu_periph_clock_enable(RCU_GPIOA); + rcu_periph_clock_enable(RCU_GPIOB); + + /*configure PA8/PA9/PA10(TIMER0/CH0/CH1/CH2) as alternate function*/ + gpio_mode_set(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_AF, GPIO_PUPD_NONE, + PHASE_A_GPIO_LOW); + gpio_output_options_set(PHASE_A_GPIO_PORT_LOW, GPIO_OTYPE_PP, + GPIO_OSPEED_50MHZ, PHASE_A_GPIO_LOW); + + gpio_mode_set(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_AF, GPIO_PUPD_NONE, + PHASE_B_GPIO_LOW); + gpio_output_options_set(PHASE_B_GPIO_PORT_LOW, GPIO_OTYPE_PP, + GPIO_OSPEED_50MHZ, PHASE_B_GPIO_LOW); + + gpio_mode_set(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_AF, GPIO_PUPD_NONE, + PHASE_C_GPIO_LOW); + gpio_output_options_set(PHASE_C_GPIO_PORT_LOW, GPIO_OTYPE_PP, + GPIO_OSPEED_50MHZ, PHASE_C_GPIO_LOW); + + gpio_af_set(PHASE_A_GPIO_PORT_LOW, GPIO_AF_2, PHASE_A_GPIO_LOW); + gpio_af_set(PHASE_B_GPIO_PORT_LOW, GPIO_AF_2, PHASE_B_GPIO_LOW); + gpio_af_set(PHASE_C_GPIO_PORT_LOW, GPIO_AF_2, PHASE_C_GPIO_LOW); + + /*configure PB13/PB14/PB15(TIMER0/CH0N/CH1N/CH2N) as alternate function*/ + gpio_mode_set(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_AF, GPIO_PUPD_NONE, + PHASE_A_GPIO_HIGH); + gpio_output_options_set(GPIOB, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, + PHASE_A_GPIO_HIGH); + + gpio_mode_set(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_AF, GPIO_PUPD_NONE, + PHASE_B_GPIO_HIGH); + gpio_output_options_set(GPIOB, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, + PHASE_B_GPIO_HIGH); + + gpio_mode_set(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_AF, GPIO_PUPD_NONE, + PHASE_C_GPIO_HIGH); + gpio_output_options_set(GPIOB, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, + PHASE_C_GPIO_HIGH); + + gpio_af_set(PHASE_A_GPIO_PORT_HIGH, GPIO_AF_2, PHASE_A_GPIO_HIGH); + gpio_af_set(PHASE_B_GPIO_PORT_HIGH, GPIO_AF_2, PHASE_B_GPIO_HIGH); + gpio_af_set(PHASE_C_GPIO_PORT_HIGH, GPIO_AF_2, PHASE_C_GPIO_HIGH); } void TIMER5_Init(void) { - rcu_periph_clock_enable(RCU_TIMER5); - TIMER_CAR(TIMER5) = 0xFFFF; - TIMER_PSC(TIMER5) = 35; + rcu_periph_clock_enable(RCU_TIMER5); + TIMER_CAR(TIMER5) = 0xFFFF; + TIMER_PSC(TIMER5) = 35; } void TIMER13_Init(void) { - rcu_periph_clock_enable(RCU_TIMER13); - TIMER_CAR(TIMER13) = 1000000 / LOOP_FREQUENCY_HZ; - TIMER_PSC(TIMER13) = 71; - NVIC_SetPriority(TIMER13_IRQn, 3); - NVIC_EnableIRQ(TIMER13_IRQn); - timer_enable(TIMER13); + rcu_periph_clock_enable(RCU_TIMER13); + TIMER_CAR(TIMER13) = 1000000 / LOOP_FREQUENCY_HZ; + TIMER_PSC(TIMER13) = 71; + NVIC_SetPriority(TIMER13_IRQn, 3); + NVIC_EnableIRQ(TIMER13_IRQn); + timer_enable(TIMER13); } void TIMER15_Init(void) { - rcu_periph_clock_enable(RCU_TIMER15); - TIMER_CAR(TIMER15) = 0xFFFF; - TIMER_PSC(TIMER15) = 35; - timer_auto_reload_shadow_enable(TIMER15); - timer_enable(TIMER15); - - NVIC_SetPriority(TIMER15_IRQn, 1); - NVIC_EnableIRQ(TIMER15_IRQn); + rcu_periph_clock_enable(RCU_TIMER15); + TIMER_CAR(TIMER15) = 0xFFFF; + TIMER_PSC(TIMER15) = 35; + timer_auto_reload_shadow_enable(TIMER15); + timer_enable(TIMER15); + + NVIC_SetPriority(TIMER15_IRQn, 1); + NVIC_EnableIRQ(TIMER15_IRQn); } void TIMER16_Init(void) { - rcu_periph_clock_enable(RCU_TIMER16); - TIMER_CAR(TIMER16) = 0xFFFF; - TIMER_PSC(TIMER16) = 71; - timer_auto_reload_shadow_enable(TIMER16); - timer_enable(TIMER16); + rcu_periph_clock_enable(RCU_TIMER16); + TIMER_CAR(TIMER16) = 0xFFFF; + TIMER_PSC(TIMER16) = 71; + timer_auto_reload_shadow_enable(TIMER16); + timer_enable(TIMER16); } void MX_DMA_Init(void) { - rcu_periph_clock_enable(RCU_DMA); + rcu_periph_clock_enable(RCU_DMA); - NVIC_SetPriority(DMA_Channel1_2_IRQn, 3); - NVIC_EnableIRQ(DMA_Channel1_2_IRQn); + NVIC_SetPriority(DMA_Channel1_2_IRQn, 3); + NVIC_EnableIRQ(DMA_Channel1_2_IRQn); - // NVIC_SetPriority(DMA_Channel3_4_IRQn, 1); - // NVIC_EnableIRQ(DMA_Channel3_4_IRQn); + // NVIC_SetPriority(DMA_Channel3_4_IRQn, 1); + // NVIC_EnableIRQ(DMA_Channel3_4_IRQn); } void MX_GPIO_Init(void) { } void UN_TIM_Init(void) { - rcu_periph_clock_enable(RCU_GPIOB); - rcu_periph_clock_enable(RCU_DMA); - - gpio_output_options_set(GPIOB, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_4); - gpio_af_set(GPIOB, GPIO_AF_1, GPIO_PIN_4); - - dma_periph_address_config(INPUT_DMA_CHANNEL, - (uint32_t)&TIMER_CH0CV(IC_TIMER_REGISTER)); - dma_memory_address_config(INPUT_DMA_CHANNEL, (uint32_t)&dma_buffer); - - // dma_transfer_direction_config(INPUT_DMA_CHANNEL, - // DMA_PERIPHERAL_TO_MEMORY); - // DMA_CHCTL(INPUT_DMA_CHANNEL) |= DMA_PRIORITY_LOW; - // dma_circulation_disable(INPUT_DMA_CHANNEL); - // dma_periph_increase_disable(INPUT_DMA_CHANNEL); - // dma_memory_increase_enable(INPUT_DMA_CHANNEL); - // dma_periph_width_config(INPUT_DMA_CHANNEL, DMA_MEMORY_WIDTH_16BIT); - // dma_memory_width_config(INPUT_DMA_CHANNEL, DMA_MEMORY_WIDTH_32BIT); - - DMA_CHCTL(INPUT_DMA_CHANNEL) = 0x0000098a; - - NVIC_SetPriority(IC_DMA_IRQ_NAME, 1); - NVIC_EnableIRQ(IC_DMA_IRQ_NAME); - rcu_periph_clock_enable(RCU_TIMER2); - TIMER_CAR(TIMER2) = 0xFFFF; - TIMER_PSC(TIMER2) = 10; - /* enable a TIMER */ - - // LL_TIM_DisableARRPreload(IC_TIMER_REGISTER); - timer_auto_reload_shadow_disable(TIMER2); - - timer_ic_parameter_struct timer_icinitpara; - timer_channel_input_struct_para_init(&timer_icinitpara); - /* TIMER2 CH0 PWM input capture configuration */ - timer_icinitpara.icpolarity = TIMER_IC_POLARITY_BOTH_EDGE; - timer_icinitpara.icselection = TIMER_IC_SELECTION_DIRECTTI; - timer_icinitpara.icprescaler = TIMER_IC_PSC_DIV1; - timer_icinitpara.icfilter = 0x0; - timer_input_pwm_capture_config(TIMER2, TIMER_CH_0, &timer_icinitpara); - - // NVIC_SetPriority(TIMER2_IRQn, 0); - // NVIC_EnableIRQ(TIMER2_IRQn); - timer_enable(TIMER2); - - gpio_mode_set(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO_PIN_4); + rcu_periph_clock_enable(RCU_GPIOB); + rcu_periph_clock_enable(RCU_DMA); + + gpio_output_options_set(GPIOB, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_4); + gpio_af_set(GPIOB, GPIO_AF_1, GPIO_PIN_4); + + dma_periph_address_config(INPUT_DMA_CHANNEL, + (uint32_t)&TIMER_CH0CV(IC_TIMER_REGISTER)); + dma_memory_address_config(INPUT_DMA_CHANNEL, (uint32_t)&dma_buffer); + + // dma_transfer_direction_config(INPUT_DMA_CHANNEL, + // DMA_PERIPHERAL_TO_MEMORY); + // DMA_CHCTL(INPUT_DMA_CHANNEL) |= DMA_PRIORITY_LOW; + // dma_circulation_disable(INPUT_DMA_CHANNEL); + // dma_periph_increase_disable(INPUT_DMA_CHANNEL); + // dma_memory_increase_enable(INPUT_DMA_CHANNEL); + // dma_periph_width_config(INPUT_DMA_CHANNEL, DMA_MEMORY_WIDTH_16BIT); + // dma_memory_width_config(INPUT_DMA_CHANNEL, DMA_MEMORY_WIDTH_32BIT); + + DMA_CHCTL(INPUT_DMA_CHANNEL) = 0x0000098a; + + NVIC_SetPriority(IC_DMA_IRQ_NAME, 1); + NVIC_EnableIRQ(IC_DMA_IRQ_NAME); + rcu_periph_clock_enable(RCU_TIMER2); + TIMER_CAR(TIMER2) = 0xFFFF; + TIMER_PSC(TIMER2) = 10; + /* enable a TIMER */ + + // LL_TIM_DisableARRPreload(IC_TIMER_REGISTER); + timer_auto_reload_shadow_disable(TIMER2); + + timer_ic_parameter_struct timer_icinitpara; + timer_channel_input_struct_para_init(&timer_icinitpara); + /* TIMER2 CH0 PWM input capture configuration */ + timer_icinitpara.icpolarity = TIMER_IC_POLARITY_BOTH_EDGE; + timer_icinitpara.icselection = TIMER_IC_SELECTION_DIRECTTI; + timer_icinitpara.icprescaler = TIMER_IC_PSC_DIV1; + timer_icinitpara.icfilter = 0x0; + timer_input_pwm_capture_config(TIMER2, TIMER_CH_0, &timer_icinitpara); + + // NVIC_SetPriority(TIMER2_IRQn, 0); + // NVIC_EnableIRQ(TIMER2_IRQn); + timer_enable(TIMER2); + + gpio_mode_set(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO_PIN_4); } #ifdef USE_RGB_LED // has 3 color led void LED_GPIO_init() { - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - - /* GPIO Ports Clock Enable */ - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); - - LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_8); - LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_5); - LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_3); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_8; - GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_5; - GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_3; - GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + + /* GPIO Ports Clock Enable */ + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); + + LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_8); + LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_5); + LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_3); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_8; + GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_5; + GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_3; + GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); } #endif -void setPWMCompare1(uint16_t compareone) { TIMER_CH0CV(TIMER0) = compareone; } -void setPWMCompare2(uint16_t comparetwo) { TIMER_CH1CV(TIMER0) = comparetwo; } +void setPWMCompare1(uint16_t compareone) +{ + TIMER_CH0CV(TIMER0) = compareone; +} +void setPWMCompare2(uint16_t comparetwo) +{ + TIMER_CH1CV(TIMER0) = comparetwo; +} void setPWMCompare3(uint16_t comparethree) { - TIMER_CH2CV(TIMER0) = comparethree; + TIMER_CH2CV(TIMER0) = comparethree; } void generatePwmTimerEvent() { - timer_event_software_generate(TIMER0, TIMER_EVENT_SRC_UPG); + timer_event_software_generate(TIMER0, TIMER_EVENT_SRC_UPG); } void resetInputCaptureTimer() { - TIMER_PSC(IC_TIMER_REGISTER) = 0; - TIMER_CNT(IC_TIMER_REGISTER) = 0; + TIMER_PSC(IC_TIMER_REGISTER) = 0; + TIMER_CNT(IC_TIMER_REGISTER) = 0; } void initAfterJump() { - __enable_irq(); - fmc_wscnt_set(2); - fmc_prefetch_enable(); + __enable_irq(); + fmc_wscnt_set(2); + fmc_prefetch_enable(); } void enableCorePeripherals() { - timer_channel_output_state_config(TIMER0, TIMER_CH_0, TIMER_CCX_ENABLE); - timer_channel_output_state_config(TIMER0, TIMER_CH_1, TIMER_CCX_ENABLE); - timer_channel_output_state_config(TIMER0, TIMER_CH_2, TIMER_CCX_ENABLE); - timer_channel_complementary_output_state_config(TIMER0, TIMER_CH_0, - TIMER_CCXN_ENABLE); - timer_channel_complementary_output_state_config(TIMER0, TIMER_CH_1, - TIMER_CCXN_ENABLE); - timer_channel_complementary_output_state_config(TIMER0, TIMER_CH_2, - TIMER_CCXN_ENABLE); - - /* Enable counter */ - TIMER_CTL0(TIMER0) |= (uint32_t)TIMER_CTL0_CEN; - timer_primary_output_config(TIMER0, ENABLE); - - /* Force update generation */ - // timer_event_software_generate(TIMER0, TIMER_EVENT_SRC_UPG); + timer_channel_output_state_config(TIMER0, TIMER_CH_0, TIMER_CCX_ENABLE); + timer_channel_output_state_config(TIMER0, TIMER_CH_1, TIMER_CCX_ENABLE); + timer_channel_output_state_config(TIMER0, TIMER_CH_2, TIMER_CCX_ENABLE); + timer_channel_complementary_output_state_config(TIMER0, TIMER_CH_0, + TIMER_CCXN_ENABLE); + timer_channel_complementary_output_state_config(TIMER0, TIMER_CH_1, + TIMER_CCXN_ENABLE); + timer_channel_complementary_output_state_config(TIMER0, TIMER_CH_2, + TIMER_CCXN_ENABLE); + + /* Enable counter */ + TIMER_CTL0(TIMER0) |= (uint32_t)TIMER_CTL0_CEN; + timer_primary_output_config(TIMER0, ENABLE); + + /* Force update generation */ + // timer_event_software_generate(TIMER0, TIMER_EVENT_SRC_UPG); #ifndef BRUSHED_MODE - TIMER_CTL0(COM_TIMER) |= (uint32_t)TIMER_CTL0_CEN; // commutation_timer priority 0 - timer_event_software_generate(COM_TIMER, TIMER_EVENT_SRC_UPG); - TIMER_DMAINTEN(COM_TIMER) &= (~(uint32_t)TIMER_INT_UP); // disable interrupt until after polling mode + TIMER_CTL0(COM_TIMER) |= (uint32_t)TIMER_CTL0_CEN; // commutation_timer priority 0 + timer_event_software_generate(COM_TIMER, TIMER_EVENT_SRC_UPG); + TIMER_DMAINTEN(COM_TIMER) &= (~(uint32_t)TIMER_INT_UP); // disable interrupt until after polling mode #endif - TIMER_CTL0(UTILITY_TIMER) |= (uint32_t)TIMER_CTL0_CEN; - TIMER_CTL0(INTERVAL_TIMER) |= (uint32_t)TIMER_CTL0_CEN; - timer_event_software_generate(INTERVAL_TIMER, TIMER_EVENT_SRC_UPG); - - // // 10khz timer - TIMER_CTL0(TEN_KHZ_TIMER) |= (uint32_t)TIMER_CTL0_CEN; - timer_event_software_generate(TEN_KHZ_TIMER, TIMER_EVENT_SRC_UPG); - TIMER_DMAINTEN(TEN_KHZ_TIMER) |= (uint32_t)TIMER_INT_UP; + TIMER_CTL0(UTILITY_TIMER) |= (uint32_t)TIMER_CTL0_CEN; + TIMER_CTL0(INTERVAL_TIMER) |= (uint32_t)TIMER_CTL0_CEN; + timer_event_software_generate(INTERVAL_TIMER, TIMER_EVENT_SRC_UPG); + + // // 10khz timer + TIMER_CTL0(TEN_KHZ_TIMER) |= (uint32_t)TIMER_CTL0_CEN; + timer_event_software_generate(TEN_KHZ_TIMER, TIMER_EVENT_SRC_UPG); + TIMER_DMAINTEN(TEN_KHZ_TIMER) |= (uint32_t)TIMER_INT_UP; #ifdef USE_ADC - ADC_Init(); + ADC_Init(); #endif - cmp_enable(); - delayMicros(1000); + cmp_enable(); + delayMicros(1000); #ifdef USE_ADC_INPUT #else - timer_channel_output_state_config(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, - TIMER_CCX_ENABLE); - TIMER_CTL0(IC_TIMER_REGISTER) |= (uint32_t)TIMER_CTL0_CEN; // enable counter + timer_channel_output_state_config(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, + TIMER_CCX_ENABLE); + TIMER_CTL0(IC_TIMER_REGISTER) |= (uint32_t)TIMER_CTL0_CEN; // enable counter #endif - rcu_periph_clock_disable(RCU_GPIOB); - rcu_periph_clock_disable(RCU_DMA); + rcu_periph_clock_disable(RCU_GPIOB); + rcu_periph_clock_disable(RCU_DMA); - NVIC_SetPriority(EXTI4_15_IRQn, 2); - NVIC_EnableIRQ(EXTI4_15_IRQn); - EXTI_INTEN |= (uint32_t)EXTI_15; + NVIC_SetPriority(EXTI4_15_IRQn, 2); + NVIC_EnableIRQ(EXTI4_15_IRQn); + EXTI_INTEN |= (uint32_t)EXTI_15; - UN_TIM_Init(); + UN_TIM_Init(); } diff --git a/Mcu/e230/Src/phaseouts.c b/Mcu/e230/Src/phaseouts.c index 2504c3aa..5978b8ef 100644 --- a/Mcu/e230/Src/phaseouts.c +++ b/Mcu/e230/Src/phaseouts.c @@ -31,85 +31,86 @@ extern char prop_brake_active; #endif void gpio_mode_QUICK(uint32_t gpio_periph, uint32_t mode, uint32_t pull_up_down, - uint32_t pin) + uint32_t pin) { - GPIO_CTL(gpio_periph) = ((((((GPIO_CTL(gpio_periph)))) & (~(((pin * pin) * (0x3UL << (0U)))))) | (((pin * pin) * mode)))); + GPIO_CTL(gpio_periph) = ((((((GPIO_CTL(gpio_periph)))) & (~(((pin * pin) * (0x3UL << (0U)))))) | (((pin * pin) * mode)))); } void proportionalBrake() -{ // alternate all channels between braking (ABC LOW) - // and coasting (ABC float) put lower channel into - // alternate mode and turn upper OFF for each - // channel - // turn all HIGH channels off for ABC - - // gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, - // GPIO_PUPD_NONE, PHASE_A_GPIO_HIGH); - gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, - PHASE_A_GPIO_HIGH); - GPIO_BC(PHASE_A_GPIO_PORT_HIGH) = PHASE_A_GPIO_HIGH; - - gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, - PHASE_B_GPIO_HIGH); - GPIO_BC(PHASE_B_GPIO_PORT_HIGH) = PHASE_B_GPIO_HIGH; - - gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, - PHASE_C_GPIO_HIGH); - GPIO_BC(PHASE_C_GPIO_PORT_HIGH) = PHASE_C_GPIO_HIGH; - - // set low channel to PWM, duty cycle will now control braking - gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_AF, GPIO_PUPD_NONE, - PHASE_A_GPIO_LOW); - gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_AF, GPIO_PUPD_NONE, - PHASE_B_GPIO_LOW); - gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_AF, GPIO_PUPD_NONE, - PHASE_C_GPIO_LOW); +{ + // alternate all channels between braking (ABC LOW) + // and coasting (ABC float) put lower channel into + // alternate mode and turn upper OFF for each + // channel + // turn all HIGH channels off for ABC + + // gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, + // GPIO_PUPD_NONE, PHASE_A_GPIO_HIGH); + gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, + PHASE_A_GPIO_HIGH); + GPIO_BC(PHASE_A_GPIO_PORT_HIGH) = PHASE_A_GPIO_HIGH; + + gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, + PHASE_B_GPIO_HIGH); + GPIO_BC(PHASE_B_GPIO_PORT_HIGH) = PHASE_B_GPIO_HIGH; + + gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, + PHASE_C_GPIO_HIGH); + GPIO_BC(PHASE_C_GPIO_PORT_HIGH) = PHASE_C_GPIO_HIGH; + + // set low channel to PWM, duty cycle will now control braking + gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_AF, GPIO_PUPD_NONE, + PHASE_A_GPIO_LOW); + gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_AF, GPIO_PUPD_NONE, + PHASE_B_GPIO_LOW); + gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_AF, GPIO_PUPD_NONE, + PHASE_C_GPIO_LOW); } void phaseBPWM() { - if (!eepromBuffer.comp_pwm) { // for future - gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, - PHASE_B_GPIO_LOW); - GPIO_BC(PHASE_B_GPIO_PORT_LOW) = PHASE_B_GPIO_LOW; - } else { - gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_AF, GPIO_PUPD_NONE, - PHASE_B_GPIO_LOW); // low - // GPIO_CTL(PHASE_B_GPIO_PORT_LOW) |= (2 << - //(PHASE_B_GPIO_LOW - //<<2)); GPIO_CTL(PHASE_B_GPIO_PORT_LOW) = - //((((((GPIO_CTL(PHASE_B_GPIO_PORT_LOW)))) & (~(((PHASE_B_GPIO_LOW * - // PHASE_B_GPIO_LOW) * (0x3UL << (0U)))))) | (((PHASE_B_GPIO_LOW * - // PHASE_B_GPIO_LOW) * 2)))); - } - gpio_mode_QUICK( - PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_AF, GPIO_PUPD_NONE, - PHASE_B_GPIO_HIGH); // high - // GPIO_CTL(PHASE_B_GPIO_PORT_HIGH) = - //((((((GPIO_CTL(PHASE_B_GPIO_PORT_HIGH)))) & (~(((PHASE_B_GPIO_HIGH * - // PHASE_B_GPIO_HIGH) * (0x3UL << (0U)))))) | (((PHASE_B_GPIO_HIGH * - // PHASE_B_GPIO_HIGH) * 2)))); + if (!eepromBuffer.comp_pwm) { // for future + gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, + PHASE_B_GPIO_LOW); + GPIO_BC(PHASE_B_GPIO_PORT_LOW) = PHASE_B_GPIO_LOW; + } else { + gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_AF, GPIO_PUPD_NONE, + PHASE_B_GPIO_LOW); // low + // GPIO_CTL(PHASE_B_GPIO_PORT_LOW) |= (2 << + //(PHASE_B_GPIO_LOW + //<<2)); GPIO_CTL(PHASE_B_GPIO_PORT_LOW) = + //((((((GPIO_CTL(PHASE_B_GPIO_PORT_LOW)))) & (~(((PHASE_B_GPIO_LOW * + // PHASE_B_GPIO_LOW) * (0x3UL << (0U)))))) | (((PHASE_B_GPIO_LOW * + // PHASE_B_GPIO_LOW) * 2)))); + } + gpio_mode_QUICK( + PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_AF, GPIO_PUPD_NONE, + PHASE_B_GPIO_HIGH); // high + // GPIO_CTL(PHASE_B_GPIO_PORT_HIGH) = + //((((((GPIO_CTL(PHASE_B_GPIO_PORT_HIGH)))) & (~(((PHASE_B_GPIO_HIGH * + // PHASE_B_GPIO_HIGH) * (0x3UL << (0U)))))) | (((PHASE_B_GPIO_HIGH * + // PHASE_B_GPIO_HIGH) * 2)))); } void phaseBFLOAT() { - gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, - PHASE_B_GPIO_LOW); - GPIO_BC(PHASE_B_GPIO_PORT_LOW) = PHASE_B_GPIO_LOW; - gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, - PHASE_B_GPIO_HIGH); - GPIO_BC(PHASE_B_GPIO_PORT_HIGH) = PHASE_B_GPIO_HIGH; + gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, + PHASE_B_GPIO_LOW); + GPIO_BC(PHASE_B_GPIO_PORT_LOW) = PHASE_B_GPIO_LOW; + gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, + PHASE_B_GPIO_HIGH); + GPIO_BC(PHASE_B_GPIO_PORT_HIGH) = PHASE_B_GPIO_HIGH; } void phaseBLOW() { - // low mosfet on - gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, - PHASE_B_GPIO_LOW); - GPIO_BOP(PHASE_B_GPIO_PORT_LOW) = PHASE_B_GPIO_LOW; - gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, - PHASE_B_GPIO_HIGH); - GPIO_BC(PHASE_B_GPIO_PORT_HIGH) = PHASE_B_GPIO_HIGH; + // low mosfet on + gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, + PHASE_B_GPIO_LOW); + GPIO_BOP(PHASE_B_GPIO_PORT_LOW) = PHASE_B_GPIO_LOW; + gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, + PHASE_B_GPIO_HIGH); + GPIO_BC(PHASE_B_GPIO_PORT_HIGH) = PHASE_B_GPIO_HIGH; } //////////////////////////////PHASE @@ -117,38 +118,38 @@ void phaseBLOW() void phaseCPWM() { - if (!eepromBuffer.comp_pwm) { - gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, - PHASE_C_GPIO_LOW); - GPIO_BC(PHASE_C_GPIO_PORT_LOW) = PHASE_C_GPIO_LOW; - } else { - gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_AF, GPIO_PUPD_NONE, - PHASE_C_GPIO_LOW); - ; - } - gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_AF, GPIO_PUPD_NONE, - PHASE_C_GPIO_HIGH); + if (!eepromBuffer.comp_pwm) { + gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, + PHASE_C_GPIO_LOW); + GPIO_BC(PHASE_C_GPIO_PORT_LOW) = PHASE_C_GPIO_LOW; + } else { + gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_AF, GPIO_PUPD_NONE, + PHASE_C_GPIO_LOW); + ; + } + gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_AF, GPIO_PUPD_NONE, + PHASE_C_GPIO_HIGH); } void phaseCFLOAT() { - // floating - gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, - PHASE_C_GPIO_LOW); - GPIO_BC(PHASE_C_GPIO_PORT_LOW) = PHASE_C_GPIO_LOW; - gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, - PHASE_C_GPIO_HIGH); - GPIO_BC(PHASE_C_GPIO_PORT_HIGH) = PHASE_C_GPIO_HIGH; + // floating + gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, + PHASE_C_GPIO_LOW); + GPIO_BC(PHASE_C_GPIO_PORT_LOW) = PHASE_C_GPIO_LOW; + gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, + PHASE_C_GPIO_HIGH); + GPIO_BC(PHASE_C_GPIO_PORT_HIGH) = PHASE_C_GPIO_HIGH; } void phaseCLOW() { - gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, - PHASE_C_GPIO_LOW); - GPIO_BOP(PHASE_C_GPIO_PORT_LOW) = PHASE_C_GPIO_LOW; - gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, - PHASE_C_GPIO_HIGH); - GPIO_BC(PHASE_C_GPIO_PORT_HIGH) = PHASE_C_GPIO_HIGH; + gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, + PHASE_C_GPIO_LOW); + GPIO_BOP(PHASE_C_GPIO_PORT_LOW) = PHASE_C_GPIO_LOW; + gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, + PHASE_C_GPIO_HIGH); + GPIO_BC(PHASE_C_GPIO_PORT_HIGH) = PHASE_C_GPIO_HIGH; } ///////////////////////////////////////////////PHASE 3 @@ -156,36 +157,36 @@ void phaseCLOW() void phaseAPWM() { - if (!eepromBuffer.comp_pwm) { - gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, - PHASE_A_GPIO_LOW); - GPIO_BC(PHASE_A_GPIO_PORT_LOW) = PHASE_A_GPIO_LOW; - } else { - gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_AF, GPIO_PUPD_NONE, - PHASE_A_GPIO_LOW); - } - gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_AF, GPIO_PUPD_NONE, - PHASE_A_GPIO_HIGH); + if (!eepromBuffer.comp_pwm) { + gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, + PHASE_A_GPIO_LOW); + GPIO_BC(PHASE_A_GPIO_PORT_LOW) = PHASE_A_GPIO_LOW; + } else { + gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_AF, GPIO_PUPD_NONE, + PHASE_A_GPIO_LOW); + } + gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_AF, GPIO_PUPD_NONE, + PHASE_A_GPIO_HIGH); } void phaseAFLOAT() { - gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, - PHASE_A_GPIO_LOW); - GPIO_BC(PHASE_A_GPIO_PORT_LOW) = PHASE_A_GPIO_LOW; - gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, - PHASE_A_GPIO_HIGH); - GPIO_BC(PHASE_A_GPIO_PORT_HIGH) = PHASE_A_GPIO_HIGH; + gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, + PHASE_A_GPIO_LOW); + GPIO_BC(PHASE_A_GPIO_PORT_LOW) = PHASE_A_GPIO_LOW; + gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, + PHASE_A_GPIO_HIGH); + GPIO_BC(PHASE_A_GPIO_PORT_HIGH) = PHASE_A_GPIO_HIGH; } void phaseALOW() { - gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, - PHASE_A_GPIO_LOW); - GPIO_BOP(PHASE_A_GPIO_PORT_LOW) = PHASE_A_GPIO_LOW; - gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, - PHASE_A_GPIO_HIGH); - GPIO_BC(PHASE_A_GPIO_PORT_HIGH) = PHASE_A_GPIO_HIGH; + gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, + PHASE_A_GPIO_LOW); + GPIO_BOP(PHASE_A_GPIO_PORT_LOW) = PHASE_A_GPIO_LOW; + gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, + PHASE_A_GPIO_HIGH); + GPIO_BC(PHASE_A_GPIO_PORT_HIGH) = PHASE_A_GPIO_HIGH; } #else @@ -193,38 +194,38 @@ void phaseALOW() //////////////////////////////////PHASE 1////////////////////// void phaseBPWM() { - if (!eepromBuffer.comp_pwm) { // for future - // gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, - // GPIO_PUPD_NONE, PHASE_B_GPIO_LOW); - // GPIO_BC(PHASE_B_GPIO_PORT_LOW) = PHASE_B_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); // high pwm + if (!eepromBuffer.comp_pwm) { // for future + // gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, + // GPIO_PUPD_NONE, PHASE_B_GPIO_LOW); + // GPIO_BC(PHASE_B_GPIO_PORT_LOW) = PHASE_B_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); // high pwm } void phaseBFLOAT() { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable off - PHASE_B_GPIO_PORT_ENABLE->BRR = PHASE_B_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); // pwm off - PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable off + PHASE_B_GPIO_PORT_ENABLE->BRR = PHASE_B_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); // pwm off + PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; } void phaseBLOW() { - // low mosfet on - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); // pwm off - PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; + // low mosfet on + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); // pwm off + PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; } //////////////////////////////PHASE @@ -232,38 +233,38 @@ void phaseBLOW() void phaseCPWM() { - if (!eepromBuffer.comp_pwm) { - // gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, - // GPIO_PUPD_NONE, - // PHASE_C_GPIO_LOW); GPIO_BC(PHASE_C_GPIO_PORT_LOW) = PHASE_C_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + // gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, + // GPIO_PUPD_NONE, + // PHASE_C_GPIO_LOW); GPIO_BC(PHASE_C_GPIO_PORT_LOW) = PHASE_C_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); } void phaseCFLOAT() { - // floating - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable off - PHASE_C_GPIO_PORT_ENABLE->BRR = PHASE_C_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; + // floating + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable off + PHASE_C_GPIO_PORT_ENABLE->BRR = PHASE_C_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; } void phaseCLOW() { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; } ///////////////////////////////////////////////PHASE 3 @@ -271,113 +272,115 @@ void phaseCLOW() void phaseAPWM() { - if (!eepromBuffer.comp_pwm) { - // gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, - // GPIO_PUPD_NONE, - // PHASE_A_GPIO_LOW); GPIO_BC(PHASE_A_GPIO_PORT_LOW) = PHASE_A_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + // gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, + // GPIO_PUPD_NONE, + // PHASE_A_GPIO_LOW); GPIO_BC(PHASE_A_GPIO_PORT_LOW) = PHASE_A_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); } void phaseAFLOAT() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BRR = PHASE_A_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BRR = PHASE_A_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; } void phaseALOW() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; } #endif void allOff() { - phaseAFLOAT(); - phaseBFLOAT(); - phaseCFLOAT(); + phaseAFLOAT(); + phaseBFLOAT(); + phaseCFLOAT(); } void comStep(int newStep) { - switch (newStep) { - case 1: // A-B - phaseCFLOAT(); - phaseBLOW(); - phaseAPWM(); - break; - - case 2: // C-B - phaseAFLOAT(); - phaseBLOW(); - phaseCPWM(); - break; - - case 3: // C-A - phaseBFLOAT(); - phaseALOW(); - phaseCPWM(); - break; - - case 4: // B-A - phaseCFLOAT(); - phaseALOW(); - phaseBPWM(); - break; - - case 5: // B-C - phaseAFLOAT(); - phaseCLOW(); - phaseBPWM(); - break; - - case 6: // A-C - phaseBFLOAT(); - phaseCLOW(); - phaseAPWM(); - break; - } -} + switch (newStep) { + case 1: // A-B + phaseCFLOAT(); + phaseBLOW(); + phaseAPWM(); + break; -void fullBrake() -{ // full braking shorting all low sides - phaseALOW(); + case 2: // C-B + phaseAFLOAT(); phaseBLOW(); + phaseCPWM(); + break; + + case 3: // C-A + phaseBFLOAT(); + phaseALOW(); + phaseCPWM(); + break; + + case 4: // B-A + phaseCFLOAT(); + phaseALOW(); + phaseBPWM(); + break; + + case 5: // B-C + phaseAFLOAT(); phaseCLOW(); + phaseBPWM(); + break; + + case 6: // A-C + phaseBFLOAT(); + phaseCLOW(); + phaseAPWM(); + break; + } +} + +void fullBrake() +{ + // full braking shorting all low sides + phaseALOW(); + phaseBLOW(); + phaseCLOW(); } void allpwm() -{ // for stepper_sine - phaseAPWM(); - phaseBPWM(); - phaseCPWM(); +{ + // for stepper_sine + phaseAPWM(); + phaseBPWM(); + phaseCPWM(); } void twoChannelForward() { - phaseAPWM(); - phaseBLOW(); - phaseCPWM(); + phaseAPWM(); + phaseBLOW(); + phaseCPWM(); } void twoChannelReverse() { - phaseALOW(); - phaseBPWM(); - phaseCLOW(); + phaseALOW(); + phaseBPWM(); + phaseCLOW(); } diff --git a/Mcu/e230/Src/serial_telemetry.c b/Mcu/e230/Src/serial_telemetry.c index 99d5d4b7..4510a278 100644 --- a/Mcu/e230/Src/serial_telemetry.c +++ b/Mcu/e230/Src/serial_telemetry.c @@ -14,95 +14,98 @@ uint8_t nbDataToTransmit = sizeof(aTxBuffer); void telem_UART_Init(void) { - dma_parameter_struct dma_init_struct; - rcu_periph_clock_enable(RCU_GPIOB); - rcu_periph_clock_enable(RCU_DMA); - rcu_periph_clock_enable(RCU_USART0); - - dma_deinit(DMA_CH1); - - gpio_af_set(GPIOB, GPIO_AF_0, GPIO_PIN_6); - - gpio_mode_set(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO_PIN_6); - gpio_output_options_set(GPIOB, GPIO_OTYPE_PP, GPIO_OSPEED_10MHZ, GPIO_PIN_6); - - dma_struct_para_init(&dma_init_struct); - - dma_init_struct.direction = DMA_MEMORY_TO_PERIPHERAL; - dma_init_struct.memory_addr = (uint32_t)aTxBuffer; - dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE; - dma_init_struct.memory_width = DMA_MEMORY_WIDTH_8BIT; - dma_init_struct.number = nbDataToTransmit; - dma_init_struct.periph_addr = (uint32_t)&USART_TDATA(USART0); - dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE; - dma_init_struct.periph_width = DMA_PERIPHERAL_WIDTH_8BIT; - dma_init_struct.priority = DMA_PRIORITY_LOW; - dma_init(DMA_CH1, &dma_init_struct); - - dma_circulation_disable(DMA_CH1); - - usart_dma_transmit_config(USART0, USART_DENT_ENABLE); - /* enable DMA channel1 transfer complete interrupt */ - // dma_interrupt_enable(DMA_CH1, DMA_INT_FTF); - /* enable DMA channel1 */ - dma_channel_enable(DMA_CH1); - usart_halfduplex_enable(USART0); - /* USART configure */ - // usart_deinit(USART0); - usart_baudrate_set(USART0, 115200U); - usart_receive_config(USART0, USART_RECEIVE_ENABLE); - usart_transmit_config(USART0, USART_TRANSMIT_ENABLE); - - usart_enable(USART0); + dma_parameter_struct dma_init_struct; + rcu_periph_clock_enable(RCU_GPIOB); + rcu_periph_clock_enable(RCU_DMA); + rcu_periph_clock_enable(RCU_USART0); + + dma_deinit(DMA_CH1); + + gpio_af_set(GPIOB, GPIO_AF_0, GPIO_PIN_6); + + gpio_mode_set(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO_PIN_6); + gpio_output_options_set(GPIOB, GPIO_OTYPE_PP, GPIO_OSPEED_10MHZ, GPIO_PIN_6); + + dma_struct_para_init(&dma_init_struct); + + dma_init_struct.direction = DMA_MEMORY_TO_PERIPHERAL; + dma_init_struct.memory_addr = (uint32_t)aTxBuffer; + dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE; + dma_init_struct.memory_width = DMA_MEMORY_WIDTH_8BIT; + dma_init_struct.number = nbDataToTransmit; + dma_init_struct.periph_addr = (uint32_t)&USART_TDATA(USART0); + dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE; + dma_init_struct.periph_width = DMA_PERIPHERAL_WIDTH_8BIT; + dma_init_struct.priority = DMA_PRIORITY_LOW; + dma_init(DMA_CH1, &dma_init_struct); + + dma_circulation_disable(DMA_CH1); + + usart_dma_transmit_config(USART0, USART_DENT_ENABLE); + /* enable DMA channel1 transfer complete interrupt */ + // dma_interrupt_enable(DMA_CH1, DMA_INT_FTF); + /* enable DMA channel1 */ + dma_channel_enable(DMA_CH1); + usart_halfduplex_enable(USART0); + /* USART configure */ + // usart_deinit(USART0); + usart_baudrate_set(USART0, 115200U); + usart_receive_config(USART0, USART_RECEIVE_ENABLE); + usart_transmit_config(USART0, USART_TRANSMIT_ENABLE); + + usart_enable(USART0); } void send_telem_DMA() -{ // set data length and enable channel to start transfer - - usart_receive_config(USART0, USART_TRANSMIT_DISABLE); - usart_transmit_config(USART0, USART_TRANSMIT_ENABLE); - dma_channel_disable(DMA_CH1); - DMA_CHCNT(DMA_CH1) = nbDataToTransmit; - usart_dma_transmit_config(USART0, USART_DENT_ENABLE); - dma_channel_enable(DMA_CH1); - usart_receive_config(USART0, USART_RECEIVE_ENABLE); - // usart_transmit_config(USART0, USART_TRANSMIT_DISABLE); +{ + // set data length and enable channel to start transfer + + usart_receive_config(USART0, USART_TRANSMIT_DISABLE); + usart_transmit_config(USART0, USART_TRANSMIT_ENABLE); + dma_channel_disable(DMA_CH1); + DMA_CHCNT(DMA_CH1) = nbDataToTransmit; + usart_dma_transmit_config(USART0, USART_DENT_ENABLE); + dma_channel_enable(DMA_CH1); + usart_receive_config(USART0, USART_RECEIVE_ENABLE); + // usart_transmit_config(USART0, USART_TRANSMIT_DISABLE); } uint8_t update_crc8(uint8_t crc, uint8_t crc_seed) { - uint8_t crc_u, i; - crc_u = crc; - crc_u ^= crc_seed; - for (i = 0; i < 8; i++) - crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); - return (crc_u); + uint8_t crc_u, i; + crc_u = crc; + crc_u ^= crc_seed; + for (i = 0; i < 8; i++) { + crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); + } + return (crc_u); } uint8_t get_crc8(uint8_t* Buf, uint8_t BufLen) { - uint8_t crc = 0, i; - for (i = 0; i < BufLen; i++) - crc = update_crc8(Buf[i], crc); - return (crc); + uint8_t crc = 0, i; + for (i = 0; i < BufLen; i++) { + crc = update_crc8(Buf[i], crc); + } + return (crc); } void makeTelemPackage(uint8_t temp, uint16_t voltage, uint16_t current, - uint16_t consumption, uint16_t e_rpm) + uint16_t consumption, uint16_t e_rpm) { - aTxBuffer[0] = temp; // temperature + aTxBuffer[0] = temp; // temperature - aTxBuffer[1] = (voltage >> 8) & 0xFF; // voltage hB - aTxBuffer[2] = voltage & 0xFF; // voltage lowB + aTxBuffer[1] = (voltage >> 8) & 0xFF; // voltage hB + aTxBuffer[2] = voltage & 0xFF; // voltage lowB - aTxBuffer[3] = (current >> 8) & 0xFF; // current - aTxBuffer[4] = current & 0xFF; // divide by 10 for Amps + aTxBuffer[3] = (current >> 8) & 0xFF; // current + aTxBuffer[4] = current & 0xFF; // divide by 10 for Amps - aTxBuffer[5] = (consumption >> 8) & 0xFF; // consumption - aTxBuffer[6] = consumption & 0xFF; // in mah + aTxBuffer[5] = (consumption >> 8) & 0xFF; // consumption + aTxBuffer[6] = consumption & 0xFF; // in mah - aTxBuffer[7] = (e_rpm >> 8) & 0xFF; // - aTxBuffer[8] = e_rpm & 0xFF; // eRpM *100 + aTxBuffer[7] = (e_rpm >> 8) & 0xFF; // + aTxBuffer[8] = e_rpm & 0xFF; // eRpM *100 - aTxBuffer[9] = get_crc8(aTxBuffer, 9); + aTxBuffer[9] = get_crc8(aTxBuffer, 9); } diff --git a/Mcu/e230/Src/system_gd32e23x.c b/Mcu/e230/Src/system_gd32e23x.c index 5c3b9269..56772758 100644 --- a/Mcu/e230/Src/system_gd32e23x.c +++ b/Mcu/e230/Src/system_gd32e23x.c @@ -96,31 +96,31 @@ static void system_clock_config(void); */ void SystemInit(void) { - /* enable IRC8M */ - RCU_CTL0 |= RCU_CTL0_IRC8MEN; - while (0U == (RCU_CTL0 & RCU_CTL0_IRC8MSTB)) { - } - - RCU_MODIFY(0x80); - RCU_CFG0 &= ~RCU_CFG0_SCS; - RCU_CTL0 &= ~(RCU_CTL0_HXTALEN | RCU_CTL0_CKMEN | RCU_CTL0_PLLEN | RCU_CTL0_HXTALBPS); - /* reset RCU */ - RCU_CFG0 &= ~(RCU_CFG0_SCS | RCU_CFG0_AHBPSC | RCU_CFG0_APB1PSC | RCU_CFG0_APB2PSC | RCU_CFG0_ADCPSC | RCU_CFG0_CKOUTSEL | RCU_CFG0_CKOUTDIV | RCU_CFG0_PLLDV); - RCU_CFG0 &= ~(RCU_CFG0_PLLSEL | RCU_CFG0_PLLMF | RCU_CFG0_PLLMF4 | RCU_CFG0_PLLDV); - RCU_CFG1 &= ~(RCU_CFG1_PREDV); - RCU_CFG2 &= ~(RCU_CFG2_USART0SEL | RCU_CFG2_ADCSEL); - RCU_CFG2 &= ~RCU_CFG2_IRC28MDIV; - RCU_CFG2 &= ~RCU_CFG2_ADCPSC2; - RCU_CTL1 &= ~RCU_CTL1_IRC28MEN; - RCU_INT = 0x00000000U; - - /* configure system clock */ - system_clock_config(); + /* enable IRC8M */ + RCU_CTL0 |= RCU_CTL0_IRC8MEN; + while (0U == (RCU_CTL0 & RCU_CTL0_IRC8MSTB)) { + } + + RCU_MODIFY(0x80); + RCU_CFG0 &= ~RCU_CFG0_SCS; + RCU_CTL0 &= ~(RCU_CTL0_HXTALEN | RCU_CTL0_CKMEN | RCU_CTL0_PLLEN | RCU_CTL0_HXTALBPS); + /* reset RCU */ + RCU_CFG0 &= ~(RCU_CFG0_SCS | RCU_CFG0_AHBPSC | RCU_CFG0_APB1PSC | RCU_CFG0_APB2PSC | RCU_CFG0_ADCPSC | RCU_CFG0_CKOUTSEL | RCU_CFG0_CKOUTDIV | RCU_CFG0_PLLDV); + RCU_CFG0 &= ~(RCU_CFG0_PLLSEL | RCU_CFG0_PLLMF | RCU_CFG0_PLLMF4 | RCU_CFG0_PLLDV); + RCU_CFG1 &= ~(RCU_CFG1_PREDV); + RCU_CFG2 &= ~(RCU_CFG2_USART0SEL | RCU_CFG2_ADCSEL); + RCU_CFG2 &= ~RCU_CFG2_IRC28MDIV; + RCU_CFG2 &= ~RCU_CFG2_ADCPSC2; + RCU_CTL1 &= ~RCU_CTL1_IRC28MEN; + RCU_INT = 0x00000000U; + + /* configure system clock */ + system_clock_config(); #ifdef VECT_TAB_SRAM - nvic_vector_table_set(NVIC_VECTTAB_RAM, VECT_TAB_OFFSET); + nvic_vector_table_set(NVIC_VECTTAB_RAM, VECT_TAB_OFFSET); #else - nvic_vector_table_set(NVIC_VECTTAB_FLASH, VECT_TAB_OFFSET); + nvic_vector_table_set(NVIC_VECTTAB_FLASH, VECT_TAB_OFFSET); #endif } @@ -133,15 +133,15 @@ void SystemInit(void) static void system_clock_config(void) { #ifdef __SYSTEM_CLOCK_8M_HXTAL - system_clock_8m_hxtal(); + system_clock_8m_hxtal(); #elif defined(__SYSTEM_CLOCK_72M_PLL_HXTAL) - system_clock_72m_hxtal(); + system_clock_72m_hxtal(); #elif defined(__SYSTEM_CLOCK_72M_PLL_IRC8M_DIV2) - system_clock_72m_irc8m(); + system_clock_72m_irc8m(); #elif defined(__SYSTEM_CLOCK_72M_PLL_IRC48M_DIV2) - system_clock_72m_irc48m(); + system_clock_72m_irc48m(); #else - system_clock_8m_irc8m(); + system_clock_8m_irc8m(); #endif /* __SYSTEM_CLOCK_8M_HXTAL */ } @@ -154,39 +154,39 @@ static void system_clock_config(void) */ static void system_clock_8m_hxtal(void) { - uint32_t timeout = 0U; - uint32_t stab_flag = 0U; - - /* enable HXTAL */ - RCU_CTL0 |= RCU_CTL0_HXTALEN; - - /* wait until HXTAL is stable or the startup time is longer than - * HXTAL_STARTUP_TIMEOUT */ - do { - timeout++; - stab_flag = (RCU_CTL0 & RCU_CTL0_HXTALSTB); - } while ((0U == stab_flag) && (HXTAL_STARTUP_TIMEOUT != timeout)); - /* if fail */ - if (0U == (RCU_CTL0 & RCU_CTL0_HXTALSTB)) { - while (1) { - } - } - - /* HXTAL is stable */ - /* AHB = SYSCLK */ - RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; - /* APB2 = AHB */ - RCU_CFG0 |= RCU_APB2_CKAHB_DIV1; - /* APB1 = AHB */ - RCU_CFG0 |= RCU_APB1_CKAHB_DIV1; - - /* select HXTAL as system clock */ - RCU_CFG0 &= ~RCU_CFG0_SCS; - RCU_CFG0 |= RCU_CKSYSSRC_HXTAL; - - /* wait until HXTAL is selected as system clock */ - while (0U == (RCU_CFG0 & RCU_SCSS_HXTAL)) { + uint32_t timeout = 0U; + uint32_t stab_flag = 0U; + + /* enable HXTAL */ + RCU_CTL0 |= RCU_CTL0_HXTALEN; + + /* wait until HXTAL is stable or the startup time is longer than + * HXTAL_STARTUP_TIMEOUT */ + do { + timeout++; + stab_flag = (RCU_CTL0 & RCU_CTL0_HXTALSTB); + } while ((0U == stab_flag) && (HXTAL_STARTUP_TIMEOUT != timeout)); + /* if fail */ + if (0U == (RCU_CTL0 & RCU_CTL0_HXTALSTB)) { + while (1) { } + } + + /* HXTAL is stable */ + /* AHB = SYSCLK */ + RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; + /* APB2 = AHB */ + RCU_CFG0 |= RCU_APB2_CKAHB_DIV1; + /* APB1 = AHB */ + RCU_CFG0 |= RCU_APB1_CKAHB_DIV1; + + /* select HXTAL as system clock */ + RCU_CFG0 &= ~RCU_CFG0_SCS; + RCU_CFG0 |= RCU_CKSYSSRC_HXTAL; + + /* wait until HXTAL is selected as system clock */ + while (0U == (RCU_CFG0 & RCU_SCSS_HXTAL)) { + } } #elif defined(__SYSTEM_CLOCK_72M_PLL_HXTAL) @@ -196,52 +196,52 @@ static void system_clock_8m_hxtal(void) */ static void system_clock_72m_hxtal(void) { - uint32_t timeout = 0U; - uint32_t stab_flag = 0U; - - /* enable HXTAL */ - RCU_CTL0 |= RCU_CTL0_HXTALEN; - - /* wait until HXTAL is stable or the startup time is longer than - * HXTAL_STARTUP_TIMEOUT */ - do { - timeout++; - stab_flag = (RCU_CTL0 & RCU_CTL0_HXTALSTB); - } while ((0U == stab_flag) && (HXTAL_STARTUP_TIMEOUT != timeout)); - /* if fail */ - if (0U == (RCU_CTL0 & RCU_CTL0_HXTALSTB)) { - while (1) { - } + uint32_t timeout = 0U; + uint32_t stab_flag = 0U; + + /* enable HXTAL */ + RCU_CTL0 |= RCU_CTL0_HXTALEN; + + /* wait until HXTAL is stable or the startup time is longer than + * HXTAL_STARTUP_TIMEOUT */ + do { + timeout++; + stab_flag = (RCU_CTL0 & RCU_CTL0_HXTALSTB); + } while ((0U == stab_flag) && (HXTAL_STARTUP_TIMEOUT != timeout)); + /* if fail */ + if (0U == (RCU_CTL0 & RCU_CTL0_HXTALSTB)) { + while (1) { } + } - FMC_WS = (FMC_WS & (~FMC_WS_WSCNT)) | WS_WSCNT_2; + FMC_WS = (FMC_WS & (~FMC_WS_WSCNT)) | WS_WSCNT_2; - /* HXTAL is stable */ - /* AHB = SYSCLK */ - RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; - /* APB2 = AHB */ - RCU_CFG0 |= RCU_APB2_CKAHB_DIV1; - /* APB1 = AHB */ - RCU_CFG0 |= RCU_APB1_CKAHB_DIV1; + /* HXTAL is stable */ + /* AHB = SYSCLK */ + RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; + /* APB2 = AHB */ + RCU_CFG0 |= RCU_APB2_CKAHB_DIV1; + /* APB1 = AHB */ + RCU_CFG0 |= RCU_APB1_CKAHB_DIV1; - /* PLL = HXTAL * 9 = 72 MHz */ - RCU_CFG0 &= ~(RCU_CFG0_PLLSEL | RCU_CFG0_PLLMF | RCU_CFG0_PLLDV); - RCU_CFG0 |= (RCU_PLLSRC_HXTAL | RCU_PLL_MUL9); + /* PLL = HXTAL * 9 = 72 MHz */ + RCU_CFG0 &= ~(RCU_CFG0_PLLSEL | RCU_CFG0_PLLMF | RCU_CFG0_PLLDV); + RCU_CFG0 |= (RCU_PLLSRC_HXTAL | RCU_PLL_MUL9); - /* enable PLL */ - RCU_CTL0 |= RCU_CTL0_PLLEN; + /* enable PLL */ + RCU_CTL0 |= RCU_CTL0_PLLEN; - /* wait until PLL is stable */ - while (0U == (RCU_CTL0 & RCU_CTL0_PLLSTB)) { - } + /* wait until PLL is stable */ + while (0U == (RCU_CTL0 & RCU_CTL0_PLLSTB)) { + } - /* select PLL as system clock */ - RCU_CFG0 &= ~RCU_CFG0_SCS; - RCU_CFG0 |= RCU_CKSYSSRC_PLL; + /* select PLL as system clock */ + RCU_CFG0 &= ~RCU_CFG0_SCS; + RCU_CFG0 |= RCU_CKSYSSRC_PLL; - /* wait until PLL is selected as system clock */ - while (0U == (RCU_CFG0 & RCU_SCSS_PLL)) { - } + /* wait until PLL is selected as system clock */ + while (0U == (RCU_CFG0 & RCU_SCSS_PLL)) { + } } #elif defined(__SYSTEM_CLOCK_72M_PLL_IRC8M_DIV2) @@ -251,51 +251,51 @@ static void system_clock_72m_hxtal(void) */ static void system_clock_72m_irc8m(void) { - uint32_t timeout = 0U; - uint32_t stab_flag = 0U; - - /* enable IRC8M */ - RCU_CTL0 |= RCU_CTL0_IRC8MEN; - - /* wait until IRC8M is stable or the startup time is longer than - * IRC8M_STARTUP_TIMEOUT */ - do { - timeout++; - stab_flag = (RCU_CTL0 & RCU_CTL0_IRC8MSTB); - } while ((0U == stab_flag) && (IRC8M_STARTUP_TIMEOUT != timeout)); - - /* if fail */ - if (0U == (RCU_CTL0 & RCU_CTL0_IRC8MSTB)) { - while (1) { - } - } - - FMC_WS = (FMC_WS & (~FMC_WS_WSCNT)) | WS_WSCNT_2; - - /* AHB = SYSCLK */ - RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; - /* APB2 = AHB */ - RCU_CFG0 |= RCU_APB2_CKAHB_DIV1; - /* APB1 = AHB */ - RCU_CFG0 |= RCU_APB1_CKAHB_DIV1; - /* PLL = (IRC8M/2) * 18 = 72 MHz */ - RCU_CFG0 &= ~(RCU_CFG0_PLLSEL | RCU_CFG0_PLLMF); - RCU_CFG0 |= (RCU_PLLSRC_IRC8M_DIV2 | RCU_PLL_MUL18); - - /* enable PLL */ - RCU_CTL0 |= RCU_CTL0_PLLEN; - - /* wait until PLL is stable */ - while (0U == (RCU_CTL0 & RCU_CTL0_PLLSTB)) { - } - - /* select PLL as system clock */ - RCU_CFG0 &= ~RCU_CFG0_SCS; - RCU_CFG0 |= RCU_CKSYSSRC_PLL; - - /* wait until PLL is selected as system clock */ - while (0U == (RCU_CFG0 & RCU_SCSS_PLL)) { + uint32_t timeout = 0U; + uint32_t stab_flag = 0U; + + /* enable IRC8M */ + RCU_CTL0 |= RCU_CTL0_IRC8MEN; + + /* wait until IRC8M is stable or the startup time is longer than + * IRC8M_STARTUP_TIMEOUT */ + do { + timeout++; + stab_flag = (RCU_CTL0 & RCU_CTL0_IRC8MSTB); + } while ((0U == stab_flag) && (IRC8M_STARTUP_TIMEOUT != timeout)); + + /* if fail */ + if (0U == (RCU_CTL0 & RCU_CTL0_IRC8MSTB)) { + while (1) { } + } + + FMC_WS = (FMC_WS & (~FMC_WS_WSCNT)) | WS_WSCNT_2; + + /* AHB = SYSCLK */ + RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; + /* APB2 = AHB */ + RCU_CFG0 |= RCU_APB2_CKAHB_DIV1; + /* APB1 = AHB */ + RCU_CFG0 |= RCU_APB1_CKAHB_DIV1; + /* PLL = (IRC8M/2) * 18 = 72 MHz */ + RCU_CFG0 &= ~(RCU_CFG0_PLLSEL | RCU_CFG0_PLLMF); + RCU_CFG0 |= (RCU_PLLSRC_IRC8M_DIV2 | RCU_PLL_MUL18); + + /* enable PLL */ + RCU_CTL0 |= RCU_CTL0_PLLEN; + + /* wait until PLL is stable */ + while (0U == (RCU_CTL0 & RCU_CTL0_PLLSTB)) { + } + + /* select PLL as system clock */ + RCU_CFG0 &= ~RCU_CFG0_SCS; + RCU_CFG0 |= RCU_CKSYSSRC_PLL; + + /* wait until PLL is selected as system clock */ + while (0U == (RCU_CFG0 & RCU_SCSS_PLL)) { + } } #else @@ -307,20 +307,20 @@ static void system_clock_72m_irc8m(void) */ static void system_clock_8m_irc8m(void) { - /* AHB = SYSCLK */ - RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; - /* APB2 = AHB */ - RCU_CFG0 |= RCU_APB2_CKAHB_DIV1; - /* APB1 = AHB */ - RCU_CFG0 |= RCU_APB1_CKAHB_DIV1; - - /* select IRC8M as system clock */ - RCU_CFG0 &= ~RCU_CFG0_SCS; - RCU_CFG0 |= RCU_CKSYSSRC_IRC8M; - - /* wait until IRC8M is selected as system clock */ - while (0U != (RCU_CFG0 & RCU_SCSS_IRC8M)) { - } + /* AHB = SYSCLK */ + RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; + /* APB2 = AHB */ + RCU_CFG0 |= RCU_APB2_CKAHB_DIV1; + /* APB1 = AHB */ + RCU_CFG0 |= RCU_APB1_CKAHB_DIV1; + + /* select IRC8M as system clock */ + RCU_CFG0 &= ~RCU_CFG0_SCS; + RCU_CFG0 |= RCU_CKSYSSRC_IRC8M; + + /* wait until IRC8M is selected as system clock */ + while (0U != (RCU_CFG0 & RCU_SCSS_IRC8M)) { + } } #endif /* __SYSTEM_CLOCK_8M_HXTAL */ @@ -330,52 +330,52 @@ static void system_clock_8m_irc8m(void) */ void SystemCoreClockUpdate(void) { - uint32_t sws = 0U; - uint32_t pllmf = 0U, pllmf4 = 0U, pllsel = 0U, prediv = 0U, idx = 0U, - clk_exp = 0U; - /* exponent of AHB clock divider */ - const uint8_t ahb_exp[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9 }; - - sws = GET_BITS(RCU_CFG0, 2, 3); - switch (sws) { - /* IRC8M is selected as CK_SYS */ - case SEL_IRC8M: - SystemCoreClock = IRC8M_VALUE; - break; - /* HXTAL is selected as CK_SYS */ - case SEL_HXTAL: - SystemCoreClock = HXTAL_VALUE; - break; - /* PLL is selected as CK_SYS */ - case SEL_PLL: - /* get the value of PLLMF[3:0] */ - pllmf = GET_BITS(RCU_CFG0, 18, 21); - pllmf4 = GET_BITS(RCU_CFG0, 27, 27); - /* high 16 bits */ - if (1U == pllmf4) { - pllmf += 17U; - } else if (15U == pllmf) { - pllmf = 16U; - } else { - pllmf += 2U; - } - - /* PLL clock source selection, HXTAL or IRC8M/2 */ - pllsel = GET_BITS(RCU_CFG0, 16, 16); - if (0U != pllsel) { - prediv = (GET_BITS(RCU_CFG1, 0, 3) + 1U); - SystemCoreClock = (HXTAL_VALUE / prediv) * pllmf; - } else { - SystemCoreClock = (IRC8M_VALUE >> 1) * pllmf; - } - break; - /* IRC8M is selected as CK_SYS */ - default: - SystemCoreClock = IRC8M_VALUE; - break; + uint32_t sws = 0U; + uint32_t pllmf = 0U, pllmf4 = 0U, pllsel = 0U, prediv = 0U, idx = 0U, + clk_exp = 0U; + /* exponent of AHB clock divider */ + const uint8_t ahb_exp[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9 }; + + sws = GET_BITS(RCU_CFG0, 2, 3); + switch (sws) { + /* IRC8M is selected as CK_SYS */ + case SEL_IRC8M: + SystemCoreClock = IRC8M_VALUE; + break; + /* HXTAL is selected as CK_SYS */ + case SEL_HXTAL: + SystemCoreClock = HXTAL_VALUE; + break; + /* PLL is selected as CK_SYS */ + case SEL_PLL: + /* get the value of PLLMF[3:0] */ + pllmf = GET_BITS(RCU_CFG0, 18, 21); + pllmf4 = GET_BITS(RCU_CFG0, 27, 27); + /* high 16 bits */ + if (1U == pllmf4) { + pllmf += 17U; + } else if (15U == pllmf) { + pllmf = 16U; + } else { + pllmf += 2U; + } + + /* PLL clock source selection, HXTAL or IRC8M/2 */ + pllsel = GET_BITS(RCU_CFG0, 16, 16); + if (0U != pllsel) { + prediv = (GET_BITS(RCU_CFG1, 0, 3) + 1U); + SystemCoreClock = (HXTAL_VALUE / prediv) * pllmf; + } else { + SystemCoreClock = (IRC8M_VALUE >> 1) * pllmf; } - /* calculate AHB clock frequency */ - idx = GET_BITS(RCU_CFG0, 4, 7); - clk_exp = ahb_exp[idx]; - SystemCoreClock >>= clk_exp; + break; + /* IRC8M is selected as CK_SYS */ + default: + SystemCoreClock = IRC8M_VALUE; + break; + } + /* calculate AHB clock frequency */ + idx = GET_BITS(RCU_CFG0, 4, 7); + clk_exp = ahb_exp[idx]; + SystemCoreClock >>= clk_exp; } diff --git a/Mcu/e230/Src/systick.c b/Mcu/e230/Src/systick.c index a30d7c77..f4178344 100644 --- a/Mcu/e230/Src/systick.c +++ b/Mcu/e230/Src/systick.c @@ -47,14 +47,14 @@ static volatile uint32_t delay; */ void systick_config(void) { - /* setup systick timer for 1000Hz interrupts */ - if (SysTick_Config(SystemCoreClock / 1000U)) { - /* capture error */ - while (1) { - } + /* setup systick timer for 1000Hz interrupts */ + if (SysTick_Config(SystemCoreClock / 1000U)) { + /* capture error */ + while (1) { } - /* configure the systick handler priority */ - NVIC_SetPriority(SysTick_IRQn, 0x00U); + } + /* configure the systick handler priority */ + NVIC_SetPriority(SysTick_IRQn, 0x00U); } /*! @@ -65,10 +65,10 @@ void systick_config(void) */ void delay_1ms(uint32_t count) { - delay = count; + delay = count; - while (0U != delay) { - } + while (0U != delay) { + } } /*! @@ -79,7 +79,7 @@ void delay_1ms(uint32_t count) */ void delay_decrement(void) { - if (0U != delay) { - delay--; - } + if (0U != delay) { + delay--; + } } diff --git a/Mcu/f031/Inc/IO.h b/Mcu/f031/Inc/IO.h index 352e164a..317300f5 100644 --- a/Mcu/f031/Inc/IO.h +++ b/Mcu/f031/Inc/IO.h @@ -33,6 +33,6 @@ extern uint8_t degrees_celsius; extern uint16_t ADC_raw_volts; extern uint16_t servo_low_threshold; // anything below this point considered 0 extern uint16_t - servo_high_threshold; // anything above this point considered 2000 (max) +servo_high_threshold; // anything above this point considered 2000 (max) extern uint16_t servo_neutral; extern uint8_t servo_dead_band; diff --git a/Mcu/f031/Inc/serial_telemetry.h b/Mcu/f031/Inc/serial_telemetry.h index b4f1c5cf..fb8cd3b9 100644 --- a/Mcu/f031/Inc/serial_telemetry.h +++ b/Mcu/f031/Inc/serial_telemetry.h @@ -11,7 +11,7 @@ #define SERIAL_TELEMETRY_H_ void makeTelemPackage(uint8_t temp, uint16_t voltage, uint16_t current, - uint16_t consumption, uint16_t e_rpm); + uint16_t consumption, uint16_t e_rpm); void telem_UART_Init(void); void send_telem_DMA(); diff --git a/Mcu/f031/Src/ADC.c b/Mcu/f031/Src/ADC.c index fb4743df..15225092 100644 --- a/Mcu/f031/Src/ADC.c +++ b/Mcu/f031/Src/ADC.c @@ -18,160 +18,163 @@ extern uint16_t ADC_raw_current; extern uint16_t ADC_raw_input; void ADC_DMA_Callback() -{ // read dma buffer and set extern variables +{ + // read dma buffer and set extern variables #ifdef USE_ADC_INPUT - ADC_raw_temp = ADCDataDMA[3]; - ADC_raw_volts = ADCDataDMA[1] / 2; - ADC_raw_current = ADCDataDMA[2]; - ADC_raw_input = ADCDataDMA[0]; + ADC_raw_temp = ADCDataDMA[3]; + ADC_raw_volts = ADCDataDMA[1] / 2; + ADC_raw_current = ADCDataDMA[2]; + ADC_raw_input = ADCDataDMA[0]; #else - ADC_raw_temp = ADCDataDMA[2]; - ADC_raw_volts = ADCDataDMA[1]; - ADC_raw_current = ADCDataDMA[0]; + ADC_raw_temp = ADCDataDMA[2]; + ADC_raw_volts = ADCDataDMA[1]; + ADC_raw_current = ADCDataDMA[0]; #endif } void enableADC_DMA() -{ // enables channel +{ + // enables channel - // NVIC_SetPriority(DMA1_Channel2_3_IRQn, 3); - // NVIC_EnableIRQ(DMA1_Channel2_3_IRQn); + // NVIC_SetPriority(DMA1_Channel2_3_IRQn, 3); + // NVIC_EnableIRQ(DMA1_Channel2_3_IRQn); - LL_DMA_ConfigAddresses( - DMA1, LL_DMA_CHANNEL_2, - LL_ADC_DMA_GetRegAddr(ADC1, LL_ADC_DMA_REG_REGULAR_DATA), - (uint32_t)&ADCDataDMA, LL_DMA_DIRECTION_PERIPH_TO_MEMORY); + LL_DMA_ConfigAddresses( + DMA1, LL_DMA_CHANNEL_2, + LL_ADC_DMA_GetRegAddr(ADC1, LL_ADC_DMA_REG_REGULAR_DATA), + (uint32_t)&ADCDataDMA, LL_DMA_DIRECTION_PERIPH_TO_MEMORY); - /* Set DMA transfer size */ + /* Set DMA transfer size */ #ifdef USE_ADC_INPUT - LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_2, 4); + LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_2, 4); #else - LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_2, 3); + LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_2, 3); #endif - /* Enable DMA transfer interruption: transfer complete */ - // LL_DMA_EnableIT_TC(DMA1, LL_DMA_CHANNEL_2); + /* Enable DMA transfer interruption: transfer complete */ + // LL_DMA_EnableIT_TC(DMA1, LL_DMA_CHANNEL_2); - /* Enable DMA transfer interruption: transfer error */ - // LL_DMA_EnableIT_TE(DMA1, LL_DMA_CHANNEL_2); - LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_2); + /* Enable DMA transfer interruption: transfer error */ + // LL_DMA_EnableIT_TE(DMA1, LL_DMA_CHANNEL_2); + LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_2); } void activateADC() -{ // called right after enable regular conversions are - // started by software and DMA interrupt happens at end of - // transfer +{ + // called right after enable regular conversions are + // started by software and DMA interrupt happens at end of + // transfer - __IO uint32_t wait_loop_index = 0; + __IO uint32_t wait_loop_index = 0; - LL_ADC_StartCalibration(ADC1); + LL_ADC_StartCalibration(ADC1); - /* Poll for ADC effectively calibrated */ + /* Poll for ADC effectively calibrated */ - while (LL_ADC_IsCalibrationOnGoing(ADC1) != 0) { - } + while (LL_ADC_IsCalibrationOnGoing(ADC1) != 0) { + } - /* Delay between ADC end of calibration and ADC enable. */ - /* Note: Variable divided by 2 to compensate partially */ - /* CPU processing cycles (depends on compilation optimization). */ - wait_loop_index = (LL_ADC_DELAY_CALIB_ENABLE_ADC_CYCLES >> 1); - while (wait_loop_index != 0) { - wait_loop_index--; - } + /* Delay between ADC end of calibration and ADC enable. */ + /* Note: Variable divided by 2 to compensate partially */ + /* CPU processing cycles (depends on compilation optimization). */ + wait_loop_index = (LL_ADC_DELAY_CALIB_ENABLE_ADC_CYCLES >> 1); + while (wait_loop_index != 0) { + wait_loop_index--; + } - /* Enable ADC */ - LL_ADC_Enable(ADC1); + /* Enable ADC */ + LL_ADC_Enable(ADC1); - /* Poll for ADC ready to convert */ + /* Poll for ADC ready to convert */ - while (LL_ADC_IsActiveFlag_ADRDY(ADC1) == 0) { - } + while (LL_ADC_IsActiveFlag_ADRDY(ADC1) == 0) { + } } void ADC_Init(void) { - LL_ADC_InitTypeDef ADC_InitStruct = { 0 }; - LL_ADC_REG_InitTypeDef ADC_REG_InitStruct = { 0 }; + LL_ADC_InitTypeDef ADC_InitStruct = { 0 }; + LL_ADC_REG_InitTypeDef ADC_REG_InitStruct = { 0 }; - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - /* Peripheral clock enable */ - LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_ADC1); + /* Peripheral clock enable */ + LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_ADC1); - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); - /**ADC GPIO Configuration + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); + /**ADC GPIO Configuration - */ + */ #ifdef USE_ADC_INPUT - GPIO_InitStruct.Pin = LL_GPIO_PIN_2; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + GPIO_InitStruct.Pin = LL_GPIO_PIN_2; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); #endif - GPIO_InitStruct.Pin = VOLTAGE_SENSE_ADC_PIN; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + GPIO_InitStruct.Pin = VOLTAGE_SENSE_ADC_PIN; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - GPIO_InitStruct.Pin = CURRENT_SENSE_ADC_PIN; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + GPIO_InitStruct.Pin = CURRENT_SENSE_ADC_PIN; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_2, - LL_DMA_DIRECTION_PERIPH_TO_MEMORY); + LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_2, + LL_DMA_DIRECTION_PERIPH_TO_MEMORY); - LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_2, LL_DMA_PRIORITY_LOW); + LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_2, LL_DMA_PRIORITY_LOW); - LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_2, LL_DMA_MODE_CIRCULAR); + LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_2, LL_DMA_MODE_CIRCULAR); - LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_2, LL_DMA_PERIPH_NOINCREMENT); + LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_2, LL_DMA_PERIPH_NOINCREMENT); - LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_2, LL_DMA_MEMORY_INCREMENT); + LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_2, LL_DMA_MEMORY_INCREMENT); - LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_2, LL_DMA_PDATAALIGN_HALFWORD); + LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_2, LL_DMA_PDATAALIGN_HALFWORD); - LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_2, LL_DMA_MDATAALIGN_HALFWORD); + LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_2, LL_DMA_MDATAALIGN_HALFWORD); - LL_SYSCFG_SetRemapDMA_ADC(LL_SYSCFG_ADC1_RMP_DMA1_CH2); + LL_SYSCFG_SetRemapDMA_ADC(LL_SYSCFG_ADC1_RMP_DMA1_CH2); #ifdef USE_ADC_INPUT - LL_ADC_REG_SetSequencerChAdd(ADC1, LL_ADC_CHANNEL_2); + LL_ADC_REG_SetSequencerChAdd(ADC1, LL_ADC_CHANNEL_2); #endif #ifndef USE_TIMER_2_CHANNEL_1 - LL_ADC_REG_SetSequencerChAdd(ADC1, VOLTAGE_ADC_CHANNEL); - /** Configure Regular Channel - */ + LL_ADC_REG_SetSequencerChAdd(ADC1, VOLTAGE_ADC_CHANNEL); + /** Configure Regular Channel + */ #endif - LL_ADC_REG_SetSequencerChAdd(ADC1, CURRENT_ADC_CHANNEL); - /** Configure Regular Channel - */ - LL_ADC_REG_SetSequencerChAdd(ADC1, LL_ADC_CHANNEL_TEMPSENSOR); - /** Configure Internal Channel - */ - LL_ADC_SetCommonPathInternalCh(__LL_ADC_COMMON_INSTANCE(ADC1), - LL_ADC_PATH_INTERNAL_TEMPSENSOR); - /** Configure the global features of the ADC (Clock, Resolution, Data - * Alignment and number of conversion) - */ - ADC_InitStruct.Clock = LL_ADC_CLOCK_SYNC_PCLK_DIV4; - ADC_InitStruct.Resolution = LL_ADC_RESOLUTION_12B; - ADC_InitStruct.DataAlignment = LL_ADC_DATA_ALIGN_RIGHT; - ADC_InitStruct.LowPowerMode = LL_ADC_LP_MODE_NONE; - LL_ADC_Init(ADC1, &ADC_InitStruct); - ADC_REG_InitStruct.TriggerSource = LL_ADC_REG_TRIG_SOFTWARE; - ADC_REG_InitStruct.SequencerDiscont = LL_ADC_REG_SEQ_DISCONT_DISABLE; - ADC_REG_InitStruct.ContinuousMode = LL_ADC_REG_CONV_SINGLE; - ADC_REG_InitStruct.DMATransfer = LL_ADC_REG_DMA_TRANSFER_LIMITED; - ADC_REG_InitStruct.Overrun = LL_ADC_REG_OVR_DATA_PRESERVED; - LL_ADC_REG_Init(ADC1, &ADC_REG_InitStruct); - LL_ADC_REG_SetSequencerScanDirection(ADC1, LL_ADC_REG_SEQ_SCAN_DIR_FORWARD); - LL_ADC_SetSamplingTimeCommonChannels(ADC1, LL_ADC_SAMPLINGTIME_28CYCLES_5); - LL_ADC_DisableIT_EOC(ADC1); - // LL_ADC_REG_SetTriggerEdge(ADC1, LL_ADC_REG_TRIG_EXT_RISING); + LL_ADC_REG_SetSequencerChAdd(ADC1, CURRENT_ADC_CHANNEL); + /** Configure Regular Channel + */ + LL_ADC_REG_SetSequencerChAdd(ADC1, LL_ADC_CHANNEL_TEMPSENSOR); + /** Configure Internal Channel + */ + LL_ADC_SetCommonPathInternalCh(__LL_ADC_COMMON_INSTANCE(ADC1), + LL_ADC_PATH_INTERNAL_TEMPSENSOR); + /** Configure the global features of the ADC (Clock, Resolution, Data + * Alignment and number of conversion) + */ + ADC_InitStruct.Clock = LL_ADC_CLOCK_SYNC_PCLK_DIV4; + ADC_InitStruct.Resolution = LL_ADC_RESOLUTION_12B; + ADC_InitStruct.DataAlignment = LL_ADC_DATA_ALIGN_RIGHT; + ADC_InitStruct.LowPowerMode = LL_ADC_LP_MODE_NONE; + LL_ADC_Init(ADC1, &ADC_InitStruct); + ADC_REG_InitStruct.TriggerSource = LL_ADC_REG_TRIG_SOFTWARE; + ADC_REG_InitStruct.SequencerDiscont = LL_ADC_REG_SEQ_DISCONT_DISABLE; + ADC_REG_InitStruct.ContinuousMode = LL_ADC_REG_CONV_SINGLE; + ADC_REG_InitStruct.DMATransfer = LL_ADC_REG_DMA_TRANSFER_LIMITED; + ADC_REG_InitStruct.Overrun = LL_ADC_REG_OVR_DATA_PRESERVED; + LL_ADC_REG_Init(ADC1, &ADC_REG_InitStruct); + LL_ADC_REG_SetSequencerScanDirection(ADC1, LL_ADC_REG_SEQ_SCAN_DIR_FORWARD); + LL_ADC_SetSamplingTimeCommonChannels(ADC1, LL_ADC_SAMPLINGTIME_28CYCLES_5); + LL_ADC_DisableIT_EOC(ADC1); + // LL_ADC_REG_SetTriggerEdge(ADC1, LL_ADC_REG_TRIG_EXT_RISING); } diff --git a/Mcu/f031/Src/IO.c b/Mcu/f031/Src/IO.c index 600fc820..a61892d1 100644 --- a/Mcu/f031/Src/IO.c +++ b/Mcu/f031/Src/IO.c @@ -25,184 +25,184 @@ uint8_t buffer_padding = 0; void changeToOutput() { - LL_DMA_SetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL, - LL_DMA_DIRECTION_MEMORY_TO_PERIPH); + LL_DMA_SetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL, + LL_DMA_DIRECTION_MEMORY_TO_PERIPH); #ifdef USE_TIMER_2_CHANNEL_3 - LL_APB1_GRP1_ForceReset(LL_APB1_GRP1_PERIPH_TIM2); // de-init timer 2 - LL_APB1_GRP1_ReleaseReset(LL_APB1_GRP1_PERIPH_TIM2); - IC_TIMER_REGISTER->CCMR2 = 0x60; - IC_TIMER_REGISTER->CCER = 0x200; + LL_APB1_GRP1_ForceReset(LL_APB1_GRP1_PERIPH_TIM2); // de-init timer 2 + LL_APB1_GRP1_ReleaseReset(LL_APB1_GRP1_PERIPH_TIM2); + IC_TIMER_REGISTER->CCMR2 = 0x60; + IC_TIMER_REGISTER->CCER = 0x200; #endif #ifdef USE_TIMER_2_CHANNEL_4 - LL_APB1_GRP1_ForceReset(LL_APB1_GRP1_PERIPH_TIM2); // de-init timer 2 - LL_APB1_GRP1_ReleaseReset(LL_APB1_GRP1_PERIPH_TIM2); - IC_TIMER_REGISTER->CCMR2 = 0x6000; - IC_TIMER_REGISTER->CCER = 0x2000; + LL_APB1_GRP1_ForceReset(LL_APB1_GRP1_PERIPH_TIM2); // de-init timer 2 + LL_APB1_GRP1_ReleaseReset(LL_APB1_GRP1_PERIPH_TIM2); + IC_TIMER_REGISTER->CCMR2 = 0x6000; + IC_TIMER_REGISTER->CCER = 0x2000; #endif #ifdef USE_TIMER_16 - LL_APB1_GRP2_ForceReset(LL_APB1_GRP2_PERIPH_TIM16); - LL_APB1_GRP2_ReleaseReset(LL_APB1_GRP2_PERIPH_TIM16); - IC_TIMER_REGISTER->CCMR1 = 0x60; - IC_TIMER_REGISTER->CCER = 0x3; + LL_APB1_GRP2_ForceReset(LL_APB1_GRP2_PERIPH_TIM16); + LL_APB1_GRP2_ReleaseReset(LL_APB1_GRP2_PERIPH_TIM16); + IC_TIMER_REGISTER->CCMR1 = 0x60; + IC_TIMER_REGISTER->CCER = 0x3; #endif #ifdef USE_TIMER_2_CHANNEL_1 - LL_APB1_GRP1_ForceReset(LL_APB1_GRP1_PERIPH_TIM2); // de-init timer 2 - LL_APB1_GRP1_ReleaseReset(LL_APB1_GRP1_PERIPH_TIM2); - IC_TIMER_REGISTER->CCMR1 = 0x60; - IC_TIMER_REGISTER->CCER = 0x3; + LL_APB1_GRP1_ForceReset(LL_APB1_GRP1_PERIPH_TIM2); // de-init timer 2 + LL_APB1_GRP1_ReleaseReset(LL_APB1_GRP1_PERIPH_TIM2); + IC_TIMER_REGISTER->CCMR1 = 0x60; + IC_TIMER_REGISTER->CCER = 0x3; #endif - IC_TIMER_REGISTER->PSC = output_timer_prescaler; - IC_TIMER_REGISTER->ARR = 61; - out_put = 1; - LL_TIM_GenerateEvent_UPDATE(IC_TIMER_REGISTER); + IC_TIMER_REGISTER->PSC = output_timer_prescaler; + IC_TIMER_REGISTER->ARR = 61; + out_put = 1; + LL_TIM_GenerateEvent_UPDATE(IC_TIMER_REGISTER); } void changeToInput() { - LL_DMA_SetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL, - LL_DMA_DIRECTION_PERIPH_TO_MEMORY); + LL_DMA_SetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL, + LL_DMA_DIRECTION_PERIPH_TO_MEMORY); #ifdef USE_TIMER_2_CHANNEL_3 - LL_APB1_GRP1_ForceReset(LL_APB1_GRP1_PERIPH_TIM2); // de-init timer 2 - LL_APB1_GRP1_ReleaseReset(LL_APB1_GRP1_PERIPH_TIM2); - IC_TIMER_REGISTER->CCMR2 = 0x41; - IC_TIMER_REGISTER->CCER = 0xa00; + LL_APB1_GRP1_ForceReset(LL_APB1_GRP1_PERIPH_TIM2); // de-init timer 2 + LL_APB1_GRP1_ReleaseReset(LL_APB1_GRP1_PERIPH_TIM2); + IC_TIMER_REGISTER->CCMR2 = 0x41; + IC_TIMER_REGISTER->CCER = 0xa00; #endif #ifdef USE_TIMER_2_CHANNEL_4 - LL_APB1_GRP1_ForceReset(LL_APB1_GRP1_PERIPH_TIM2); // de-init timer 2 - LL_APB1_GRP1_ReleaseReset(LL_APB1_GRP1_PERIPH_TIM2); - IC_TIMER_REGISTER->CCMR2 = 0x100; - IC_TIMER_REGISTER->CCER = 0xa000; + LL_APB1_GRP1_ForceReset(LL_APB1_GRP1_PERIPH_TIM2); // de-init timer 2 + LL_APB1_GRP1_ReleaseReset(LL_APB1_GRP1_PERIPH_TIM2); + IC_TIMER_REGISTER->CCMR2 = 0x100; + IC_TIMER_REGISTER->CCER = 0xa000; #endif #ifdef USE_TIMER_16 - LL_APB1_GRP2_ForceReset(LL_APB1_GRP2_PERIPH_TIM16); - LL_APB1_GRP2_ReleaseReset(LL_APB1_GRP2_PERIPH_TIM16); - IC_TIMER_REGISTER->CCMR1 = 0x41; - IC_TIMER_REGISTER->CCER = 0xa; + LL_APB1_GRP2_ForceReset(LL_APB1_GRP2_PERIPH_TIM16); + LL_APB1_GRP2_ReleaseReset(LL_APB1_GRP2_PERIPH_TIM16); + IC_TIMER_REGISTER->CCMR1 = 0x41; + IC_TIMER_REGISTER->CCER = 0xa; #endif #ifdef USE_TIMER_2_CHANNEL_1 - LL_APB1_GRP1_ForceReset(LL_APB1_GRP1_PERIPH_TIM2); // de-init timer 2 - LL_APB1_GRP1_ReleaseReset(LL_APB1_GRP1_PERIPH_TIM2); - IC_TIMER_REGISTER->CCMR1 = 0x41; - IC_TIMER_REGISTER->CCER = 0xa; + LL_APB1_GRP1_ForceReset(LL_APB1_GRP1_PERIPH_TIM2); // de-init timer 2 + LL_APB1_GRP1_ReleaseReset(LL_APB1_GRP1_PERIPH_TIM2); + IC_TIMER_REGISTER->CCMR1 = 0x41; + IC_TIMER_REGISTER->CCER = 0xa; #endif - IC_TIMER_REGISTER->PSC = ic_timer_prescaler; - IC_TIMER_REGISTER->ARR = 0xFFFF; - LL_TIM_GenerateEvent_UPDATE(IC_TIMER_REGISTER); - out_put = 0; - // TIM2->CCER = 0xa; + IC_TIMER_REGISTER->PSC = ic_timer_prescaler; + IC_TIMER_REGISTER->ARR = 0xFFFF; + LL_TIM_GenerateEvent_UPDATE(IC_TIMER_REGISTER); + out_put = 0; + // TIM2->CCER = 0xa; } void sendDshotDma() { - changeToOutput(); + changeToOutput(); #ifdef USE_TIMER_2_CHANNEL_1 - LL_DMA_ConfigAddresses( - DMA1, INPUT_DMA_CHANNEL, (uint32_t)&gcr, - (uint32_t)&IC_TIMER_REGISTER->CCR1, - LL_DMA_GetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL)); + LL_DMA_ConfigAddresses( + DMA1, INPUT_DMA_CHANNEL, (uint32_t)&gcr, + (uint32_t)&IC_TIMER_REGISTER->CCR1, + LL_DMA_GetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL)); #endif #ifdef USE_TIMER_2_CHANNEL_4 - LL_DMA_ConfigAddresses( - DMA1, INPUT_DMA_CHANNEL, (uint32_t)&gcr, - (uint32_t)&IC_TIMER_REGISTER->CCR4, - LL_DMA_GetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL)); + LL_DMA_ConfigAddresses( + DMA1, INPUT_DMA_CHANNEL, (uint32_t)&gcr, + (uint32_t)&IC_TIMER_REGISTER->CCR4, + LL_DMA_GetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL)); #endif #ifdef USE_TIMER_2_CHANNEL_3 - LL_DMA_ConfigAddresses( - DMA1, INPUT_DMA_CHANNEL, (uint32_t)&gcr, - (uint32_t)&IC_TIMER_REGISTER->CCR3, - LL_DMA_GetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL)); + LL_DMA_ConfigAddresses( + DMA1, INPUT_DMA_CHANNEL, (uint32_t)&gcr, + (uint32_t)&IC_TIMER_REGISTER->CCR3, + LL_DMA_GetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL)); #endif #ifdef USE_TIMER_16 - LL_DMA_ConfigAddresses( - DMA1, INPUT_DMA_CHANNEL, (uint32_t)&gcr, - (uint32_t)&IC_TIMER_REGISTER->CCR1, - LL_DMA_GetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL)); + LL_DMA_ConfigAddresses( + DMA1, INPUT_DMA_CHANNEL, (uint32_t)&gcr, + (uint32_t)&IC_TIMER_REGISTER->CCR1, + LL_DMA_GetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL)); #endif - LL_DMA_SetDataLength(DMA1, INPUT_DMA_CHANNEL, 23 + buffer_padding); - LL_DMA_EnableIT_TC(DMA1, INPUT_DMA_CHANNEL); - LL_DMA_EnableIT_TE(DMA1, INPUT_DMA_CHANNEL); - LL_DMA_EnableChannel(DMA1, INPUT_DMA_CHANNEL); + LL_DMA_SetDataLength(DMA1, INPUT_DMA_CHANNEL, 23 + buffer_padding); + LL_DMA_EnableIT_TC(DMA1, INPUT_DMA_CHANNEL); + LL_DMA_EnableIT_TE(DMA1, INPUT_DMA_CHANNEL); + LL_DMA_EnableChannel(DMA1, INPUT_DMA_CHANNEL); #ifdef USE_TIMER_2_CHANNEL_4 - LL_TIM_EnableDMAReq_CC4(IC_TIMER_REGISTER); + LL_TIM_EnableDMAReq_CC4(IC_TIMER_REGISTER); #endif #ifdef USE_TIMER_2_CHANNEL_3 - LL_TIM_EnableDMAReq_CC3(IC_TIMER_REGISTER); + LL_TIM_EnableDMAReq_CC3(IC_TIMER_REGISTER); #endif #ifdef USE_TIMER_16 - LL_TIM_EnableDMAReq_CC1(IC_TIMER_REGISTER); + LL_TIM_EnableDMAReq_CC1(IC_TIMER_REGISTER); #endif #ifdef USE_TIMER_2_CHANNEL_1 - LL_TIM_EnableDMAReq_CC1(IC_TIMER_REGISTER); + LL_TIM_EnableDMAReq_CC1(IC_TIMER_REGISTER); #endif - LL_TIM_CC_EnableChannel(IC_TIMER_REGISTER, IC_TIMER_CHANNEL); - LL_TIM_EnableAllOutputs(IC_TIMER_REGISTER); - LL_TIM_EnableCounter(IC_TIMER_REGISTER); + LL_TIM_CC_EnableChannel(IC_TIMER_REGISTER, IC_TIMER_CHANNEL); + LL_TIM_EnableAllOutputs(IC_TIMER_REGISTER); + LL_TIM_EnableCounter(IC_TIMER_REGISTER); } void receiveDshotDma() { - changeToInput(); - IC_TIMER_REGISTER->CNT = 0; + changeToInput(); + IC_TIMER_REGISTER->CNT = 0; #ifdef USE_TIMER_2_CHANNEL_4 - LL_DMA_ConfigAddresses( - DMA1, INPUT_DMA_CHANNEL, (uint32_t)&IC_TIMER_REGISTER->CCR4, - (uint32_t)&dma_buffer, - LL_DMA_GetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL)); + LL_DMA_ConfigAddresses( + DMA1, INPUT_DMA_CHANNEL, (uint32_t)&IC_TIMER_REGISTER->CCR4, + (uint32_t)&dma_buffer, + LL_DMA_GetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL)); #endif #ifdef USE_TIMER_2_CHANNEL_3 - LL_DMA_ConfigAddresses( - DMA1, INPUT_DMA_CHANNEL, (uint32_t)&IC_TIMER_REGISTER->CCR3, - (uint32_t)&dma_buffer, - LL_DMA_GetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL)); + LL_DMA_ConfigAddresses( + DMA1, INPUT_DMA_CHANNEL, (uint32_t)&IC_TIMER_REGISTER->CCR3, + (uint32_t)&dma_buffer, + LL_DMA_GetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL)); #endif #ifdef USE_TIMER_16 - LL_DMA_ConfigAddresses( - DMA1, INPUT_DMA_CHANNEL, (uint32_t)&IC_TIMER_REGISTER->CCR1, - (uint32_t)&dma_buffer, - LL_DMA_GetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL)); + LL_DMA_ConfigAddresses( + DMA1, INPUT_DMA_CHANNEL, (uint32_t)&IC_TIMER_REGISTER->CCR1, + (uint32_t)&dma_buffer, + LL_DMA_GetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL)); #endif #ifdef USE_TIMER_2_CHANNEL_1 - LL_DMA_ConfigAddresses( - DMA1, INPUT_DMA_CHANNEL, (uint32_t)&IC_TIMER_REGISTER->CCR1, - (uint32_t)&dma_buffer, - LL_DMA_GetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL)); + LL_DMA_ConfigAddresses( + DMA1, INPUT_DMA_CHANNEL, (uint32_t)&IC_TIMER_REGISTER->CCR1, + (uint32_t)&dma_buffer, + LL_DMA_GetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL)); #endif - LL_DMA_SetDataLength(DMA1, INPUT_DMA_CHANNEL, buffersize); - LL_DMA_EnableIT_TC(DMA1, INPUT_DMA_CHANNEL); - LL_DMA_EnableIT_TE(DMA1, INPUT_DMA_CHANNEL); - LL_DMA_EnableChannel(DMA1, INPUT_DMA_CHANNEL); + LL_DMA_SetDataLength(DMA1, INPUT_DMA_CHANNEL, buffersize); + LL_DMA_EnableIT_TC(DMA1, INPUT_DMA_CHANNEL); + LL_DMA_EnableIT_TE(DMA1, INPUT_DMA_CHANNEL); + LL_DMA_EnableChannel(DMA1, INPUT_DMA_CHANNEL); #ifdef USE_TIMER_2_CHANNEL_4 - LL_TIM_EnableDMAReq_CC4(IC_TIMER_REGISTER); - LL_TIM_EnableDMAReq_CC4(IC_TIMER_REGISTER); + LL_TIM_EnableDMAReq_CC4(IC_TIMER_REGISTER); + LL_TIM_EnableDMAReq_CC4(IC_TIMER_REGISTER); #endif #ifdef USE_TIMER_2_CHANNEL_3 - LL_TIM_EnableDMAReq_CC3(IC_TIMER_REGISTER); + LL_TIM_EnableDMAReq_CC3(IC_TIMER_REGISTER); #endif #ifdef USE_TIMER_16 - LL_TIM_EnableDMAReq_CC1(IC_TIMER_REGISTER); + LL_TIM_EnableDMAReq_CC1(IC_TIMER_REGISTER); #endif #ifdef USE_TIMER_2_CHANNEL_1 - LL_TIM_EnableDMAReq_CC1(IC_TIMER_REGISTER); + LL_TIM_EnableDMAReq_CC1(IC_TIMER_REGISTER); #endif - LL_TIM_CC_EnableChannel(IC_TIMER_REGISTER, IC_TIMER_CHANNEL); - LL_TIM_EnableCounter(IC_TIMER_REGISTER); - // TIM16->PSC = 1; + LL_TIM_CC_EnableChannel(IC_TIMER_REGISTER, IC_TIMER_CHANNEL); + LL_TIM_EnableCounter(IC_TIMER_REGISTER); + // TIM16->PSC = 1; } // void detectInput(){ @@ -263,28 +263,34 @@ void receiveDshotDma() // } -uint8_t getInputPinState() { return (INPUT_PIN_PORT->IDR & INPUT_PIN); } +uint8_t getInputPinState() +{ + return (INPUT_PIN_PORT->IDR & INPUT_PIN); +} void setInputPolarityRising() { - LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, - LL_TIM_IC_POLARITY_RISING); + LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, + LL_TIM_IC_POLARITY_RISING); } void setInputPullDown() { - LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_DOWN); + LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_DOWN); } void setInputPullUp() { - LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_UP); + LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_UP); } -void enableHalfTransferInt() { LL_DMA_EnableIT_HT(DMA1, INPUT_DMA_CHANNEL); } +void enableHalfTransferInt() +{ + LL_DMA_EnableIT_HT(DMA1, INPUT_DMA_CHANNEL); +} void setInputPullNone() { - LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_NO); + LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_NO); } // void detectInput(){ diff --git a/Mcu/f031/Src/comparator.c b/Mcu/f031/Src/comparator.c index 42ea5b06..4bb823d7 100644 --- a/Mcu/f031/Src/comparator.c +++ b/Mcu/f031/Src/comparator.c @@ -16,36 +16,39 @@ uint32_t current_EXTI_LINE = 0; void maskPhaseInterrupts() { - EXTI->IMR &= ~(1 << current_EXTI_LINE); - EXTI->PR |= 1 << PHASE_A_EXTI_LINE; - EXTI->PR |= 1 << PHASE_B_EXTI_LINE; - EXTI->PR |= 1 << PHASE_C_EXTI_LINE; + EXTI->IMR &= ~(1 << current_EXTI_LINE); + EXTI->PR |= 1 << PHASE_A_EXTI_LINE; + EXTI->PR |= 1 << PHASE_B_EXTI_LINE; + EXTI->PR |= 1 << PHASE_C_EXTI_LINE; } -void enableCompInterrupts() { EXTI->IMR |= (1 << current_EXTI_LINE); } +void enableCompInterrupts() +{ + EXTI->IMR |= (1 << current_EXTI_LINE); +} void changeCompInput() { - if (step == 1 || step == 4) { - current_GPIO_PIN = PHASE_C_EXTI_PIN; - current_GPIO_PORT = PHASE_C_EXTI_PORT; - current_EXTI_LINE = PHASE_C_EXTI_LINE; - } - if (step == 2 || step == 5) { // in phase two or 5 read from phase A PF - current_GPIO_PIN = PHASE_A_EXTI_PIN; - current_GPIO_PORT = PHASE_A_EXTI_PORT; - current_EXTI_LINE = PHASE_A_EXTI_LINE; - } - if (step == 3 || step == 6) { - current_GPIO_PIN = PHASE_B_EXTI_PIN; - current_GPIO_PORT = PHASE_B_EXTI_PORT; - current_EXTI_LINE = PHASE_B_EXTI_LINE; - } - if (rising) { - EXTI->RTSR |= (1 << current_EXTI_LINE); - EXTI->FTSR &= ~(1 << current_EXTI_LINE); - } else { - EXTI->FTSR |= (1 << current_EXTI_LINE); - EXTI->RTSR &= ~(1 << current_EXTI_LINE); - } + if (step == 1 || step == 4) { + current_GPIO_PIN = PHASE_C_EXTI_PIN; + current_GPIO_PORT = PHASE_C_EXTI_PORT; + current_EXTI_LINE = PHASE_C_EXTI_LINE; + } + if (step == 2 || step == 5) { // in phase two or 5 read from phase A PF + current_GPIO_PIN = PHASE_A_EXTI_PIN; + current_GPIO_PORT = PHASE_A_EXTI_PORT; + current_EXTI_LINE = PHASE_A_EXTI_LINE; + } + if (step == 3 || step == 6) { + current_GPIO_PIN = PHASE_B_EXTI_PIN; + current_GPIO_PORT = PHASE_B_EXTI_PORT; + current_EXTI_LINE = PHASE_B_EXTI_LINE; + } + if (rising) { + EXTI->RTSR |= (1 << current_EXTI_LINE); + EXTI->FTSR &= ~(1 << current_EXTI_LINE); + } else { + EXTI->FTSR |= (1 << current_EXTI_LINE); + EXTI->RTSR &= ~(1 << current_EXTI_LINE); + } } diff --git a/Mcu/f031/Src/eeprom.c b/Mcu/f031/Src/eeprom.c index ec27b20c..a022fa0d 100644 --- a/Mcu/f031/Src/eeprom.c +++ b/Mcu/f031/Src/eeprom.c @@ -18,61 +18,61 @@ uint32_t FLASH_FKEY2 = 0xCDEF89AB; void save_flash_nolib(uint8_t* data, int length, uint32_t add) { - uint16_t data_to_FLASH[length / 2]; - memset(data_to_FLASH, 0, length / 2); - for (int i = 0; i < length / 2; i++) { - data_to_FLASH[i] = data[i * 2 + 1] << 8 | data[i * 2]; // make 16 bit - } - volatile uint32_t data_length = length / 2; + uint16_t data_to_FLASH[length / 2]; + memset(data_to_FLASH, 0, length / 2); + for (int i = 0; i < length / 2; i++) { + data_to_FLASH[i] = data[i * 2 + 1] << 8 | data[i * 2]; // make 16 bit + } + volatile uint32_t data_length = length / 2; + + // unlock flash - // unlock flash + while ((FLASH->SR & FLASH_SR_BSY) != 0) { + /* add time-out*/ + } + if ((FLASH->CR & FLASH_CR_LOCK) != 0) { + FLASH->KEYR = FLASH_FKEY1; + FLASH->KEYR = FLASH_FKEY2; + } + // erase page if address even divisable by 1024 + if ((add % 1024) == 0) { + FLASH->CR |= FLASH_CR_PER; + FLASH->AR = add; + FLASH->CR |= FLASH_CR_STRT; while ((FLASH->SR & FLASH_SR_BSY) != 0) { - /* add time-out*/ + /* add time-out */ } - if ((FLASH->CR & FLASH_CR_LOCK) != 0) { - FLASH->KEYR = FLASH_FKEY1; - FLASH->KEYR = FLASH_FKEY2; + if ((FLASH->SR & FLASH_SR_EOP) != 0) { + FLASH->SR = FLASH_SR_EOP; + } else { + /* error */ } + FLASH->CR &= ~FLASH_CR_PER; + } - // erase page if address even divisable by 1024 - if ((add % 1024) == 0) { - FLASH->CR |= FLASH_CR_PER; - FLASH->AR = add; - FLASH->CR |= FLASH_CR_STRT; - while ((FLASH->SR & FLASH_SR_BSY) != 0) { - /* add time-out */ - } - if ((FLASH->SR & FLASH_SR_EOP) != 0) { - FLASH->SR = FLASH_SR_EOP; - } else { - /* error */ - } - FLASH->CR &= ~FLASH_CR_PER; + volatile uint32_t write_cnt = 0, index = 0; + while (index < data_length) { + FLASH->CR |= FLASH_CR_PG; /* (1) */ + *(__IO uint16_t*)(add + write_cnt) = data_to_FLASH[index]; + while ((FLASH->SR & FLASH_SR_BSY) != 0) { /* add time-out */ } - - volatile uint32_t write_cnt = 0, index = 0; - while (index < data_length) { - FLASH->CR |= FLASH_CR_PG; /* (1) */ - *(__IO uint16_t*)(add + write_cnt) = data_to_FLASH[index]; - while ((FLASH->SR & FLASH_SR_BSY) != 0) { /* add time-out */ - } - if ((FLASH->SR & FLASH_SR_EOP) != 0) { - FLASH->SR = FLASH_SR_EOP; - } else { - /* error */ - } - FLASH->CR &= ~FLASH_CR_PG; - write_cnt += 2; - index++; + if ((FLASH->SR & FLASH_SR_EOP) != 0) { + FLASH->SR = FLASH_SR_EOP; + } else { + /* error */ } - SET_BIT(FLASH->CR, FLASH_CR_LOCK); + FLASH->CR &= ~FLASH_CR_PG; + write_cnt += 2; + index++; + } + SET_BIT(FLASH->CR, FLASH_CR_LOCK); } void read_flash_bin(uint8_t* data, uint32_t add, int out_buff_len) { - // volatile uint32_t read_data; - for (int i = 0; i < out_buff_len; i++) { - data[i] = *(uint8_t*)(add + i); - } + // volatile uint32_t read_data; + for (int i = 0; i < out_buff_len; i++) { + data[i] = *(uint8_t*)(add + i); + } } diff --git a/Mcu/f031/Src/peripherals.c b/Mcu/f031/Src/peripherals.c index 76cd2614..1b2f3f20 100644 --- a/Mcu/f031/Src/peripherals.c +++ b/Mcu/f031/Src/peripherals.c @@ -14,273 +14,273 @@ void initCorePeripherals(void) { - LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_SYSCFG); - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_PWR); - SystemClock_Config(); - - FLASH->ACR |= FLASH_ACR_PRFTBE; //// prefetch buffer enable - UN_GPIO_Init(); - MX_DMA_Init(); - MX_TIM14_Init(); - MX_TIM1_Init(); - MX_TIM3_Init(); - MX_TIM17_Init(); - TEN_KHZ_Timer_Init(); - UN_TIM_Init(); + LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_SYSCFG); + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_PWR); + SystemClock_Config(); + + FLASH->ACR |= FLASH_ACR_PRFTBE; //// prefetch buffer enable + UN_GPIO_Init(); + MX_DMA_Init(); + MX_TIM14_Init(); + MX_TIM1_Init(); + MX_TIM3_Init(); + MX_TIM17_Init(); + TEN_KHZ_Timer_Init(); + UN_TIM_Init(); } void initAfterJump() { - volatile uint32_t* VectorTable = (volatile uint32_t*)0x20000000; - uint32_t vector_index = 0; - for (vector_index = 0; vector_index < 48; vector_index++) { - VectorTable[vector_index] = *(__IO uint32_t*)(APPLICATION_ADDRESS + (vector_index << 2)); // no VTOR on cortex-MO so - // need to copy vector table + volatile uint32_t* VectorTable = (volatile uint32_t*)0x20000000; + uint32_t vector_index = 0; + for (vector_index = 0; vector_index < 48; vector_index++) { + VectorTable[vector_index] = *(__IO uint32_t*)(APPLICATION_ADDRESS + (vector_index << 2)); // no VTOR on cortex-MO so + // need to copy vector table + } + // /* Enable the SYSCFG peripheral clock*/ + do { + volatile uint32_t tmpreg; + ((((RCC_TypeDef*)((((uint32_t)0x40000000U) + 0x00020000) + 0x00001000)) + ->APB2ENR) + |= ((0x1U << (0U)))); + /* Delay after an RCC peripheral clock enabling */ + tmpreg = ((((RCC_TypeDef*)((((uint32_t)0x40000000U) + 0x00020000) + 0x00001000)) + ->APB2ENR) + & ((0x1U << (0U)))); + ((void)(tmpreg)); + } while (0U); + // /* Remap SRAM at 0x00000000 */ + do { + ((SYSCFG_TypeDef*)(((uint32_t)0x40000000U) + 0x00010000))->CFGR1 &= ~((0x3U << (0U))); + ((SYSCFG_TypeDef*)(((uint32_t)0x40000000U) + 0x00010000))->CFGR1 |= ((0x1U << (0U)) | (0x2U << (0U))); + } while (0); + + if (SysTick_Config(SystemCoreClock / 1000)) { + /* Capture error */ + while (1) { } - // /* Enable the SYSCFG peripheral clock*/ - do { - volatile uint32_t tmpreg; - ((((RCC_TypeDef*)((((uint32_t)0x40000000U) + 0x00020000) + 0x00001000)) - ->APB2ENR) - |= ((0x1U << (0U)))); - /* Delay after an RCC peripheral clock enabling */ - tmpreg = ((((RCC_TypeDef*)((((uint32_t)0x40000000U) + 0x00020000) + 0x00001000)) - ->APB2ENR) - & ((0x1U << (0U)))); - ((void)(tmpreg)); - } while (0U); - // /* Remap SRAM at 0x00000000 */ - do { - ((SYSCFG_TypeDef*)(((uint32_t)0x40000000U) + 0x00010000))->CFGR1 &= ~((0x3U << (0U))); - ((SYSCFG_TypeDef*)(((uint32_t)0x40000000U) + 0x00010000))->CFGR1 |= ((0x1U << (0U)) | (0x2U << (0U))); - } while (0); - - if (SysTick_Config(SystemCoreClock / 1000)) { - /* Capture error */ - while (1) { - } - } - __enable_irq(); + } + __enable_irq(); } void SystemClock_Config(void) { - LL_FLASH_SetLatency(LL_FLASH_LATENCY_1); - - if (LL_FLASH_GetLatency() != LL_FLASH_LATENCY_1) { - // Error_Handler(); - } - LL_RCC_HSI_Enable(); - - /* Wait till HSI is ready */ - while (LL_RCC_HSI_IsReady() != 1) { - } - LL_RCC_HSI_SetCalibTrimming(16); - LL_RCC_HSI14_Enable(); - - /* Wait till HSI14 is ready */ - while (LL_RCC_HSI14_IsReady() != 1) { - } - LL_RCC_HSI14_SetCalibTrimming(16); - LL_RCC_LSI_Enable(); - - /* Wait till LSI is ready */ - while (LL_RCC_LSI_IsReady() != 1) { - } - LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSI_DIV_2, LL_RCC_PLL_MUL_12); - LL_RCC_PLL_Enable(); - - /* Wait till PLL is ready */ - while (LL_RCC_PLL_IsReady() != 1) { - } - LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1); - LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1); - LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL); - - /* Wait till System clock is ready */ - while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) { - } - LL_Init1msTick(48000000); - LL_SetSystemCoreClock(48000000); - LL_RCC_HSI14_EnableADCControl(); + LL_FLASH_SetLatency(LL_FLASH_LATENCY_1); + + if (LL_FLASH_GetLatency() != LL_FLASH_LATENCY_1) { + // Error_Handler(); + } + LL_RCC_HSI_Enable(); + + /* Wait till HSI is ready */ + while (LL_RCC_HSI_IsReady() != 1) { + } + LL_RCC_HSI_SetCalibTrimming(16); + LL_RCC_HSI14_Enable(); + + /* Wait till HSI14 is ready */ + while (LL_RCC_HSI14_IsReady() != 1) { + } + LL_RCC_HSI14_SetCalibTrimming(16); + LL_RCC_LSI_Enable(); + + /* Wait till LSI is ready */ + while (LL_RCC_LSI_IsReady() != 1) { + } + LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSI_DIV_2, LL_RCC_PLL_MUL_12); + LL_RCC_PLL_Enable(); + + /* Wait till PLL is ready */ + while (LL_RCC_PLL_IsReady() != 1) { + } + LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1); + LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1); + LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL); + + /* Wait till System clock is ready */ + while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) { + } + LL_Init1msTick(48000000); + LL_SetSystemCoreClock(48000000); + LL_RCC_HSI14_EnableADCControl(); } void MX_IWDG_Init(void) { - LL_IWDG_Enable(IWDG); - LL_IWDG_EnableWriteAccess(IWDG); - LL_IWDG_SetPrescaler(IWDG, LL_IWDG_PRESCALER_8); - LL_IWDG_SetReloadCounter(IWDG, 4095); - while (LL_IWDG_IsReady(IWDG) != 1) { - } - - LL_IWDG_SetWindow(IWDG, 4095); - LL_IWDG_ReloadCounter(IWDG); + LL_IWDG_Enable(IWDG); + LL_IWDG_EnableWriteAccess(IWDG); + LL_IWDG_SetPrescaler(IWDG, LL_IWDG_PRESCALER_8); + LL_IWDG_SetReloadCounter(IWDG, 4095); + while (LL_IWDG_IsReady(IWDG) != 1) { + } + + LL_IWDG_SetWindow(IWDG, 4095); + LL_IWDG_ReloadCounter(IWDG); } void MX_TIM1_Init(void) { - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - LL_TIM_OC_InitTypeDef TIM_OC_InitStruct = { 0 }; - LL_TIM_BDTR_InitTypeDef TIM_BDTRInitStruct = { 0 }; - - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - - /* Peripheral clock enable */ - LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_TIM1); - - TIM_InitStruct.Prescaler = 0; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = TIM1_AUTORELOAD; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - TIM_InitStruct.RepetitionCounter = 0; - LL_TIM_Init(TIM1, &TIM_InitStruct); - LL_TIM_EnableARRPreload(TIM1); - LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH1); - TIM_OC_InitStruct.OCMode = LL_TIM_OCMODE_PWM1; - TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; - TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; - TIM_OC_InitStruct.CompareValue = 0; - TIM_OC_InitStruct.OCPolarity = LL_TIM_OCPOLARITY_HIGH; - TIM_OC_InitStruct.OCNPolarity = LL_TIM_OCPOLARITY_HIGH; - TIM_OC_InitStruct.OCIdleState = LL_TIM_OCIDLESTATE_LOW; - TIM_OC_InitStruct.OCNIdleState = LL_TIM_OCIDLESTATE_LOW; - LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH1, &TIM_OC_InitStruct); - LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH1); - LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH2); - TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; - TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; - LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH2, &TIM_OC_InitStruct); - LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH2); - LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH3); - TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; - TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; - LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH3, &TIM_OC_InitStruct); - LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH3); - LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH4); - TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; - TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; - LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH4, &TIM_OC_InitStruct); - LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH4); - LL_TIM_SetTriggerOutput(TIM1, LL_TIM_TRGO_RESET); - LL_TIM_DisableMasterSlaveMode(TIM1); - TIM_BDTRInitStruct.OSSRState = LL_TIM_OSSR_DISABLE; - TIM_BDTRInitStruct.OSSIState = LL_TIM_OSSI_DISABLE; - TIM_BDTRInitStruct.LockLevel = LL_TIM_LOCKLEVEL_OFF; - TIM_BDTRInitStruct.DeadTime = DEAD_TIME; - TIM_BDTRInitStruct.BreakState = LL_TIM_BREAK_DISABLE; - TIM_BDTRInitStruct.BreakPolarity = LL_TIM_BREAK_POLARITY_HIGH; - TIM_BDTRInitStruct.AutomaticOutput = LL_TIM_AUTOMATICOUTPUT_DISABLE; - LL_TIM_BDTR_Init(TIM1, &TIM_BDTRInitStruct); - /* USER CODE BEGIN TIM1_Init 2 */ - - /* USER CODE END TIM1_Init 2 */ - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); - /**TIM1 GPIO Configuration - PB13 ------> TIM1_CH1N - PB14 ------> TIM1_CH2N - PB15 ------> TIM1_CH3N - PA8 ------> TIM1_CH1 - PA9 ------> TIM1_CH2 - PA10 ------> TIM1_CH3 - */ - GPIO_InitStruct.Pin = LL_GPIO_PIN_13; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_2; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_14; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_2; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_15; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_2; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_8; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_2; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_9; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_2; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_10; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_2; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + LL_TIM_OC_InitTypeDef TIM_OC_InitStruct = { 0 }; + LL_TIM_BDTR_InitTypeDef TIM_BDTRInitStruct = { 0 }; + + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + + /* Peripheral clock enable */ + LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_TIM1); + + TIM_InitStruct.Prescaler = 0; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = TIM1_AUTORELOAD; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + TIM_InitStruct.RepetitionCounter = 0; + LL_TIM_Init(TIM1, &TIM_InitStruct); + LL_TIM_EnableARRPreload(TIM1); + LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH1); + TIM_OC_InitStruct.OCMode = LL_TIM_OCMODE_PWM1; + TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; + TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; + TIM_OC_InitStruct.CompareValue = 0; + TIM_OC_InitStruct.OCPolarity = LL_TIM_OCPOLARITY_HIGH; + TIM_OC_InitStruct.OCNPolarity = LL_TIM_OCPOLARITY_HIGH; + TIM_OC_InitStruct.OCIdleState = LL_TIM_OCIDLESTATE_LOW; + TIM_OC_InitStruct.OCNIdleState = LL_TIM_OCIDLESTATE_LOW; + LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH1, &TIM_OC_InitStruct); + LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH1); + LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH2); + TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; + TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; + LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH2, &TIM_OC_InitStruct); + LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH2); + LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH3); + TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; + TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; + LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH3, &TIM_OC_InitStruct); + LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH3); + LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH4); + TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; + TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; + LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH4, &TIM_OC_InitStruct); + LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH4); + LL_TIM_SetTriggerOutput(TIM1, LL_TIM_TRGO_RESET); + LL_TIM_DisableMasterSlaveMode(TIM1); + TIM_BDTRInitStruct.OSSRState = LL_TIM_OSSR_DISABLE; + TIM_BDTRInitStruct.OSSIState = LL_TIM_OSSI_DISABLE; + TIM_BDTRInitStruct.LockLevel = LL_TIM_LOCKLEVEL_OFF; + TIM_BDTRInitStruct.DeadTime = DEAD_TIME; + TIM_BDTRInitStruct.BreakState = LL_TIM_BREAK_DISABLE; + TIM_BDTRInitStruct.BreakPolarity = LL_TIM_BREAK_POLARITY_HIGH; + TIM_BDTRInitStruct.AutomaticOutput = LL_TIM_AUTOMATICOUTPUT_DISABLE; + LL_TIM_BDTR_Init(TIM1, &TIM_BDTRInitStruct); + /* USER CODE BEGIN TIM1_Init 2 */ + + /* USER CODE END TIM1_Init 2 */ + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); + /**TIM1 GPIO Configuration + PB13 ------> TIM1_CH1N + PB14 ------> TIM1_CH2N + PB15 ------> TIM1_CH3N + PA8 ------> TIM1_CH1 + PA9 ------> TIM1_CH2 + PA10 ------> TIM1_CH3 + */ + GPIO_InitStruct.Pin = LL_GPIO_PIN_13; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_2; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_14; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_2; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_15; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_2; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_8; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_2; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_9; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_2; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_10; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_2; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); } void MX_TIM3_Init(void) { - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM3); - - TIM_InitStruct.Prescaler = 23; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 65535; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - LL_TIM_Init(TIM3, &TIM_InitStruct); - LL_TIM_DisableARRPreload(TIM3); - LL_TIM_SetClockSource(TIM3, LL_TIM_CLOCKSOURCE_INTERNAL); - LL_TIM_SetTriggerOutput(TIM3, LL_TIM_TRGO_RESET); - LL_TIM_DisableMasterSlaveMode(TIM3); + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM3); + + TIM_InitStruct.Prescaler = 23; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 65535; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + LL_TIM_Init(TIM3, &TIM_InitStruct); + LL_TIM_DisableARRPreload(TIM3); + LL_TIM_SetClockSource(TIM3, LL_TIM_CLOCKSOURCE_INTERNAL); + LL_TIM_SetTriggerOutput(TIM3, LL_TIM_TRGO_RESET); + LL_TIM_DisableMasterSlaveMode(TIM3); } void MX_TIM14_Init(void) { - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - - /* Peripheral clock enable */ - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM14); - - /* TIM14 interrupt Init */ - NVIC_SetPriority(TIM14_IRQn, 0); - NVIC_EnableIRQ(TIM14_IRQn); - TIM_InitStruct.Prescaler = 23; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 0xFFFF; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - LL_TIM_Init(TIM14, &TIM_InitStruct); - LL_TIM_DisableARRPreload(TIM14); + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + + /* Peripheral clock enable */ + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM14); + + /* TIM14 interrupt Init */ + NVIC_SetPriority(TIM14_IRQn, 0); + NVIC_EnableIRQ(TIM14_IRQn); + TIM_InitStruct.Prescaler = 23; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 0xFFFF; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + LL_TIM_Init(TIM14, &TIM_InitStruct); + LL_TIM_DisableARRPreload(TIM14); } void MX_TIM17_Init(void) { - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - - /* Peripheral clock enable */ - LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_TIM17); - TIM_InitStruct.Prescaler = 47; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 0xFFFF; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - TIM_InitStruct.RepetitionCounter = 0; - LL_TIM_Init(TIM17, &TIM_InitStruct); - LL_TIM_EnableARRPreload(TIM17); + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + + /* Peripheral clock enable */ + LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_TIM17); + TIM_InitStruct.Prescaler = 47; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 0xFFFF; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + TIM_InitStruct.RepetitionCounter = 0; + LL_TIM_Init(TIM17, &TIM_InitStruct); + LL_TIM_EnableARRPreload(TIM17); } /** @@ -288,346 +288,361 @@ void MX_TIM17_Init(void) */ void MX_DMA_Init(void) { - /* Init with LL driver */ - /* DMA controller clock enable */ - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_DMA1); - - /* DMA interrupt init */ - /* DMA1_Channel2_3_IRQn interrupt configuration */ - NVIC_SetPriority(DMA1_Channel2_3_IRQn, 1); - NVIC_EnableIRQ(DMA1_Channel2_3_IRQn); - /* DMA1_Channel4_5_IRQn interrupt configuration */ - NVIC_SetPriority(IC_DMA_IRQ_NAME, 1); - NVIC_EnableIRQ(IC_DMA_IRQ_NAME); + /* Init with LL driver */ + /* DMA controller clock enable */ + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_DMA1); + + /* DMA interrupt init */ + /* DMA1_Channel2_3_IRQn interrupt configuration */ + NVIC_SetPriority(DMA1_Channel2_3_IRQn, 1); + NVIC_EnableIRQ(DMA1_Channel2_3_IRQn); + /* DMA1_Channel4_5_IRQn interrupt configuration */ + NVIC_SetPriority(IC_DMA_IRQ_NAME, 1); + NVIC_EnableIRQ(IC_DMA_IRQ_NAME); } void TEN_KHZ_Timer_Init() { #ifdef USE_TIMER_16 - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2); - /* Peripheral clock enable */ - TIM_InitStruct.Prescaler = 47; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 1000000 / LOOP_FREQUENCY_HZ; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - TIM_InitStruct.RepetitionCounter = 0; - LL_TIM_Init(TIM2, &TIM_InitStruct); - LL_TIM_EnableARRPreload(TIM2); - NVIC_SetPriority(TIM2_IRQn, 2); - NVIC_EnableIRQ(TIM2_IRQn); + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2); + /* Peripheral clock enable */ + TIM_InitStruct.Prescaler = 47; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 1000000 / LOOP_FREQUENCY_HZ; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + TIM_InitStruct.RepetitionCounter = 0; + LL_TIM_Init(TIM2, &TIM_InitStruct); + LL_TIM_EnableARRPreload(TIM2); + NVIC_SetPriority(TIM2_IRQn, 2); + NVIC_EnableIRQ(TIM2_IRQn); #else - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - /* Peripheral clock enable */ - LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_TIM16); - TIM_InitStruct.Prescaler = 47; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 1000000 / LOOP_FREQUENCY_HZ; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - TIM_InitStruct.RepetitionCounter = 0; - LL_TIM_Init(TIM16, &TIM_InitStruct); - LL_TIM_EnableARRPreload(TIM16); - NVIC_SetPriority(TIM16_IRQn, 2); - NVIC_EnableIRQ(TIM16_IRQn); + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + /* Peripheral clock enable */ + LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_TIM16); + TIM_InitStruct.Prescaler = 47; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 1000000 / LOOP_FREQUENCY_HZ; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + TIM_InitStruct.RepetitionCounter = 0; + LL_TIM_Init(TIM16, &TIM_InitStruct); + LL_TIM_EnableARRPreload(TIM16); + NVIC_SetPriority(TIM16_IRQn, 2); + NVIC_EnableIRQ(TIM16_IRQn); #endif } void UN_TIM_Init(void) { - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - /* Peripheral clock enable */ + /* Peripheral clock enable */ #ifdef USE_TIMER_16 - LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_TIM16); - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); - /**TIM16 GPIO Configuration - PA6 ------> TIM16_CH1 - */ - GPIO_InitStruct.Pin = LL_GPIO_PIN_6; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; - GPIO_InitStruct.Alternate = LL_GPIO_AF_5; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_TIM16); + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); + /**TIM16 GPIO Configuration + PA6 ------> TIM16_CH1 + */ + GPIO_InitStruct.Pin = LL_GPIO_PIN_6; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; + GPIO_InitStruct.Alternate = LL_GPIO_AF_5; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); #else - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2); - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); - /**TIM16 GPIO Configuration - PA6 ------> TIM16_CH1 - */ - GPIO_InitStruct.Pin = INPUT_PIN; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; - GPIO_InitStruct.Alternate = LL_GPIO_AF_2; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2); + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); + /**TIM16 GPIO Configuration + PA6 ------> TIM16_CH1 + */ + GPIO_InitStruct.Pin = INPUT_PIN; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; + GPIO_InitStruct.Alternate = LL_GPIO_AF_2; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); #endif - /* TIM16 DMA Init */ + /* TIM16 DMA Init */ - /* TIM16_CH1_UP Init */ - LL_DMA_SetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL, - LL_DMA_DIRECTION_PERIPH_TO_MEMORY); + /* TIM16_CH1_UP Init */ + LL_DMA_SetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL, + LL_DMA_DIRECTION_PERIPH_TO_MEMORY); - LL_DMA_SetChannelPriorityLevel(DMA1, INPUT_DMA_CHANNEL, LL_DMA_PRIORITY_LOW); + LL_DMA_SetChannelPriorityLevel(DMA1, INPUT_DMA_CHANNEL, LL_DMA_PRIORITY_LOW); - LL_DMA_SetMode(DMA1, INPUT_DMA_CHANNEL, LL_DMA_MODE_NORMAL); + LL_DMA_SetMode(DMA1, INPUT_DMA_CHANNEL, LL_DMA_MODE_NORMAL); - LL_DMA_SetPeriphIncMode(DMA1, INPUT_DMA_CHANNEL, LL_DMA_PERIPH_NOINCREMENT); + LL_DMA_SetPeriphIncMode(DMA1, INPUT_DMA_CHANNEL, LL_DMA_PERIPH_NOINCREMENT); - LL_DMA_SetMemoryIncMode(DMA1, INPUT_DMA_CHANNEL, LL_DMA_MEMORY_INCREMENT); + LL_DMA_SetMemoryIncMode(DMA1, INPUT_DMA_CHANNEL, LL_DMA_MEMORY_INCREMENT); - LL_DMA_SetPeriphSize(DMA1, INPUT_DMA_CHANNEL, LL_DMA_PDATAALIGN_HALFWORD); + LL_DMA_SetPeriphSize(DMA1, INPUT_DMA_CHANNEL, LL_DMA_PDATAALIGN_HALFWORD); - LL_DMA_SetMemorySize(DMA1, INPUT_DMA_CHANNEL, LL_DMA_MDATAALIGN_WORD); + LL_DMA_SetMemorySize(DMA1, INPUT_DMA_CHANNEL, LL_DMA_MDATAALIGN_WORD); - /* TIM16 interrupt Init */ + /* TIM16 interrupt Init */ #ifdef USE_TIMER_16 - // NVIC_SetPriority(TIM16_IRQn, 0); - // NVIC_EnableIRQ(TIM16_IRQn); + // NVIC_SetPriority(TIM16_IRQn, 0); + // NVIC_EnableIRQ(TIM16_IRQn); #else - // NVIC_SetPriority(TIM2_IRQn, 0); - // NVIC_EnableIRQ(TIM2_IRQn); + // NVIC_SetPriority(TIM2_IRQn, 0); + // NVIC_EnableIRQ(TIM2_IRQn); #endif - TIM_InitStruct.Prescaler = 0; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 63; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - TIM_InitStruct.RepetitionCounter = 0; - LL_TIM_Init(IC_TIMER_REGISTER, &TIM_InitStruct); - LL_TIM_DisableARRPreload(IC_TIMER_REGISTER); - // LL_TIM_SetTriggerOutput(TIM2, LL_TIM_TRGO_RESET); - // LL_TIM_DisableMasterSlaveMode(TIM2); - - LL_TIM_IC_SetActiveInput(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, - LL_TIM_ACTIVEINPUT_DIRECTTI); - LL_TIM_IC_SetPrescaler(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, - LL_TIM_ICPSC_DIV1); - LL_TIM_IC_SetFilter(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, - LL_TIM_IC_FILTER_FDIV1); - LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, - LL_TIM_IC_POLARITY_BOTHEDGE); + TIM_InitStruct.Prescaler = 0; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 63; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + TIM_InitStruct.RepetitionCounter = 0; + LL_TIM_Init(IC_TIMER_REGISTER, &TIM_InitStruct); + LL_TIM_DisableARRPreload(IC_TIMER_REGISTER); + // LL_TIM_SetTriggerOutput(TIM2, LL_TIM_TRGO_RESET); + // LL_TIM_DisableMasterSlaveMode(TIM2); + + LL_TIM_IC_SetActiveInput(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, + LL_TIM_ACTIVEINPUT_DIRECTTI); + LL_TIM_IC_SetPrescaler(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, + LL_TIM_ICPSC_DIV1); + LL_TIM_IC_SetFilter(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, + LL_TIM_IC_FILTER_FDIV1); + LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, + LL_TIM_IC_POLARITY_BOTHEDGE); } void UN_GPIO_Init(void) { - LL_EXTI_InitTypeDef EXTI_InitStruct = { 0 }; - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - - /* GPIO Ports Clock Enable */ - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOF); - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); - - /**/ - LL_GPIO_ResetOutputPin(GPIOA, LL_GPIO_PIN_11); - - /**/ - LL_GPIO_ResetOutputPin(GPIOF, LL_GPIO_PIN_6); - - /**/ - LL_GPIO_ResetOutputPin(GPIOF, LL_GPIO_PIN_7); - - /**/ - LL_SYSCFG_SetEXTISource(SYSCFG_EXTI_PORTB, SYSCFG_EXTI_LINEB); - - /**/ - LL_SYSCFG_SetEXTISource(SYSCFG_EXTI_PORTA, SYSCFG_EXTI_LINEA); - - /**/ - LL_SYSCFG_SetEXTISource(SYSCFG_EXTI_PORTC, SYSCFG_EXTI_LINEC); - - LL_GPIO_SetPinPull(PHASE_B_EXTI_PORT, PHASE_B_EXTI_PIN, LL_GPIO_PULL_NO); - - /**/ - LL_GPIO_SetPinPull(PHASE_A_EXTI_PORT, PHASE_A_EXTI_PIN, LL_GPIO_PULL_NO); - - /**/ - LL_GPIO_SetPinPull(PHASE_C_EXTI_PORT, PHASE_C_EXTI_PIN, LL_GPIO_PULL_NO); - - /**/ - LL_GPIO_SetPinMode(PHASE_B_EXTI_PORT, PHASE_B_EXTI_PIN, LL_GPIO_MODE_INPUT); - - /**/ - LL_GPIO_SetPinMode(PHASE_A_EXTI_PORT, PHASE_A_EXTI_PIN, LL_GPIO_MODE_INPUT); - - /**/ - LL_GPIO_SetPinMode(PHASE_C_EXTI_PORT, PHASE_C_EXTI_PIN, LL_GPIO_MODE_INPUT); - - /**/ - EXTI_InitStruct.Line_0_31 = PHASE_B_LL_EXTI_LINE; - EXTI_InitStruct.LineCommand = ENABLE; - EXTI_InitStruct.Mode = LL_EXTI_MODE_IT; - EXTI_InitStruct.Trigger = LL_EXTI_TRIGGER_RISING; - LL_EXTI_Init(&EXTI_InitStruct); - - /**/ - EXTI_InitStruct.Line_0_31 = PHASE_A_LL_EXTI_LINE; - EXTI_InitStruct.LineCommand = ENABLE; - EXTI_InitStruct.Mode = LL_EXTI_MODE_IT; - EXTI_InitStruct.Trigger = LL_EXTI_TRIGGER_RISING; - LL_EXTI_Init(&EXTI_InitStruct); - - /**/ - EXTI_InitStruct.Line_0_31 = PHASE_C_LL_EXTI_LINE; - EXTI_InitStruct.LineCommand = ENABLE; - EXTI_InitStruct.Mode = LL_EXTI_MODE_IT; - EXTI_InitStruct.Trigger = LL_EXTI_TRIGGER_RISING; - LL_EXTI_Init(&EXTI_InitStruct); - - /**/ - GPIO_InitStruct.Pin = LL_GPIO_PIN_11; - GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - - /**/ - GPIO_InitStruct.Pin = LL_GPIO_PIN_6; - GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOF, &GPIO_InitStruct); - - /**/ - GPIO_InitStruct.Pin = LL_GPIO_PIN_7; - GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOF, &GPIO_InitStruct); + LL_EXTI_InitTypeDef EXTI_InitStruct = { 0 }; + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + + /* GPIO Ports Clock Enable */ + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOF); + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); + + /**/ + LL_GPIO_ResetOutputPin(GPIOA, LL_GPIO_PIN_11); + + /**/ + LL_GPIO_ResetOutputPin(GPIOF, LL_GPIO_PIN_6); + + /**/ + LL_GPIO_ResetOutputPin(GPIOF, LL_GPIO_PIN_7); + + /**/ + LL_SYSCFG_SetEXTISource(SYSCFG_EXTI_PORTB, SYSCFG_EXTI_LINEB); + + /**/ + LL_SYSCFG_SetEXTISource(SYSCFG_EXTI_PORTA, SYSCFG_EXTI_LINEA); + + /**/ + LL_SYSCFG_SetEXTISource(SYSCFG_EXTI_PORTC, SYSCFG_EXTI_LINEC); + + LL_GPIO_SetPinPull(PHASE_B_EXTI_PORT, PHASE_B_EXTI_PIN, LL_GPIO_PULL_NO); + + /**/ + LL_GPIO_SetPinPull(PHASE_A_EXTI_PORT, PHASE_A_EXTI_PIN, LL_GPIO_PULL_NO); + + /**/ + LL_GPIO_SetPinPull(PHASE_C_EXTI_PORT, PHASE_C_EXTI_PIN, LL_GPIO_PULL_NO); + + /**/ + LL_GPIO_SetPinMode(PHASE_B_EXTI_PORT, PHASE_B_EXTI_PIN, LL_GPIO_MODE_INPUT); + + /**/ + LL_GPIO_SetPinMode(PHASE_A_EXTI_PORT, PHASE_A_EXTI_PIN, LL_GPIO_MODE_INPUT); + + /**/ + LL_GPIO_SetPinMode(PHASE_C_EXTI_PORT, PHASE_C_EXTI_PIN, LL_GPIO_MODE_INPUT); + + /**/ + EXTI_InitStruct.Line_0_31 = PHASE_B_LL_EXTI_LINE; + EXTI_InitStruct.LineCommand = ENABLE; + EXTI_InitStruct.Mode = LL_EXTI_MODE_IT; + EXTI_InitStruct.Trigger = LL_EXTI_TRIGGER_RISING; + LL_EXTI_Init(&EXTI_InitStruct); + + /**/ + EXTI_InitStruct.Line_0_31 = PHASE_A_LL_EXTI_LINE; + EXTI_InitStruct.LineCommand = ENABLE; + EXTI_InitStruct.Mode = LL_EXTI_MODE_IT; + EXTI_InitStruct.Trigger = LL_EXTI_TRIGGER_RISING; + LL_EXTI_Init(&EXTI_InitStruct); + + /**/ + EXTI_InitStruct.Line_0_31 = PHASE_C_LL_EXTI_LINE; + EXTI_InitStruct.LineCommand = ENABLE; + EXTI_InitStruct.Mode = LL_EXTI_MODE_IT; + EXTI_InitStruct.Trigger = LL_EXTI_TRIGGER_RISING; + LL_EXTI_Init(&EXTI_InitStruct); + + /**/ + GPIO_InitStruct.Pin = LL_GPIO_PIN_11; + GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + /**/ + GPIO_InitStruct.Pin = LL_GPIO_PIN_6; + GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOF, &GPIO_InitStruct); + + /**/ + GPIO_InitStruct.Pin = LL_GPIO_PIN_7; + GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOF, &GPIO_InitStruct); #ifdef USE_HALL_SENSOR - GPIO_InitStruct.Pin = HALL_A_PIN; - GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT; - GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; - LL_GPIO_Init(HALL_A_PORT, &GPIO_InitStruct); + GPIO_InitStruct.Pin = HALL_A_PIN; + GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; + LL_GPIO_Init(HALL_A_PORT, &GPIO_InitStruct); - GPIO_InitStruct.Pin = HALL_B_PIN; - GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT; - GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; - LL_GPIO_Init(HALL_B_PORT, &GPIO_InitStruct); + GPIO_InitStruct.Pin = HALL_B_PIN; + GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; + LL_GPIO_Init(HALL_B_PORT, &GPIO_InitStruct); - GPIO_InitStruct.Pin = HALL_C_PIN; - GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT; - GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; - LL_GPIO_Init(HALL_C_PORT, &GPIO_InitStruct); + GPIO_InitStruct.Pin = HALL_C_PIN; + GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; + LL_GPIO_Init(HALL_C_PORT, &GPIO_InitStruct); #endif - /* EXTI interrupt init*/ - NVIC_SetPriority(EXTI_IRQ1_NAME, 0); - NVIC_EnableIRQ(EXTI_IRQ1_NAME); - NVIC_SetPriority(EXTI_IRQ2_NAME, 0); - NVIC_EnableIRQ(EXTI_IRQ2_NAME); + /* EXTI interrupt init*/ + NVIC_SetPriority(EXTI_IRQ1_NAME, 0); + NVIC_EnableIRQ(EXTI_IRQ1_NAME); + NVIC_SetPriority(EXTI_IRQ2_NAME, 0); + NVIC_EnableIRQ(EXTI_IRQ2_NAME); } -void reloadWatchDogCounter() { LL_IWDG_ReloadCounter(IWDG); } +void reloadWatchDogCounter() +{ + LL_IWDG_ReloadCounter(IWDG); +} -void setPWMCompare1(uint16_t compareone) { TIM1->CCR1 = compareone; } -void setPWMCompare2(uint16_t comparetwo) { TIM1->CCR2 = comparetwo; } -void setPWMCompare3(uint16_t comparethree) { TIM1->CCR3 = comparethree; } +void setPWMCompare1(uint16_t compareone) +{ + TIM1->CCR1 = compareone; +} +void setPWMCompare2(uint16_t comparetwo) +{ + TIM1->CCR2 = comparetwo; +} +void setPWMCompare3(uint16_t comparethree) +{ + TIM1->CCR3 = comparethree; +} -void generatePwmTimerEvent() { LL_TIM_GenerateEvent_UPDATE(TIM1); } +void generatePwmTimerEvent() +{ + LL_TIM_GenerateEvent_UPDATE(TIM1); +} void resetInputCaptureTimer() { - IC_TIMER_REGISTER->PSC = 0; - IC_TIMER_REGISTER->CNT = 0; + IC_TIMER_REGISTER->PSC = 0; + IC_TIMER_REGISTER->CNT = 0; } void enableCorePeripherals() { - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH1); - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH2); - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH3); - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH1N); - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH2N); - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH3N); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH1); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH2); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH3); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH1N); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH2N); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH3N); #ifdef MCU_G071 - LL_TIM_CC_EnableChannel( - TIM1, LL_TIM_CHANNEL_CH5); // timer used for comparator blanking + LL_TIM_CC_EnableChannel( + TIM1, LL_TIM_CHANNEL_CH5); // timer used for comparator blanking #endif - LL_TIM_CC_EnableChannel(TIM1, - LL_TIM_CHANNEL_CH4); // timer used for timing adc read - TIM1->CCR4 = 100; // set in 10khz loop to match pwm cycle timed to end of pwm on + LL_TIM_CC_EnableChannel(TIM1, + LL_TIM_CHANNEL_CH4); // timer used for timing adc read + TIM1->CCR4 = 100; // set in 10khz loop to match pwm cycle timed to end of pwm on - /* Enable counter */ - LL_TIM_EnableCounter(TIM1); - LL_TIM_EnableAllOutputs(TIM1); - /* Force update generation */ - LL_TIM_GenerateEvent_UPDATE(TIM1); + /* Enable counter */ + LL_TIM_EnableCounter(TIM1); + LL_TIM_EnableAllOutputs(TIM1); + /* Force update generation */ + LL_TIM_GenerateEvent_UPDATE(TIM1); #ifdef USE_ADC_INPUT #else - LL_TIM_CC_EnableChannel(IC_TIMER_REGISTER, - IC_TIMER_CHANNEL); // input capture and output compare - LL_TIM_EnableCounter(IC_TIMER_REGISTER); + LL_TIM_CC_EnableChannel(IC_TIMER_REGISTER, + IC_TIMER_CHANNEL); // input capture and output compare + LL_TIM_EnableCounter(IC_TIMER_REGISTER); #endif #ifdef USE_LED_STRIP - send_LED_RGB(255, 0, 0); + send_LED_RGB(255, 0, 0); #endif #ifdef USE_RGB_LED - LED_GPIO_init(); - GPIOB->BRR = LL_GPIO_PIN_8; // turn on red - GPIOB->BSRR = LL_GPIO_PIN_5; - GPIOB->BSRR = LL_GPIO_PIN_3; // + LED_GPIO_init(); + GPIOB->BRR = LL_GPIO_PIN_8; // turn on red + GPIOB->BSRR = LL_GPIO_PIN_5; + GPIOB->BSRR = LL_GPIO_PIN_3; // #endif #ifndef BRUSHED_MODE - LL_TIM_EnableCounter(COM_TIMER); // commutation_timer priority 0 - LL_TIM_GenerateEvent_UPDATE(COM_TIMER); - LL_TIM_EnableIT_UPDATE(COM_TIMER); - COM_TIMER->DIER &= ~((0x1UL << (0U))); // disable for now. + LL_TIM_EnableCounter(COM_TIMER); // commutation_timer priority 0 + LL_TIM_GenerateEvent_UPDATE(COM_TIMER); + LL_TIM_EnableIT_UPDATE(COM_TIMER); + COM_TIMER->DIER &= ~((0x1UL << (0U))); // disable for now. #endif - LL_TIM_EnableCounter(UTILITY_TIMER); - LL_TIM_GenerateEvent_UPDATE(UTILITY_TIMER); - // - LL_TIM_EnableCounter(INTERVAL_TIMER); - LL_TIM_GenerateEvent_UPDATE(INTERVAL_TIMER); - - LL_TIM_EnableCounter(TEN_KHZ_TIMER); // 10khz timer - LL_TIM_GenerateEvent_UPDATE(TEN_KHZ_TIMER); - TEN_KHZ_TIMER->DIER |= (0x1UL << (0U)); // enable interrupt - // RCC->APB2ENR &= ~(1 << 22); // turn debug off + LL_TIM_EnableCounter(UTILITY_TIMER); + LL_TIM_GenerateEvent_UPDATE(UTILITY_TIMER); + // + LL_TIM_EnableCounter(INTERVAL_TIMER); + LL_TIM_GenerateEvent_UPDATE(INTERVAL_TIMER); + + LL_TIM_EnableCounter(TEN_KHZ_TIMER); // 10khz timer + LL_TIM_GenerateEvent_UPDATE(TEN_KHZ_TIMER); + TEN_KHZ_TIMER->DIER |= (0x1UL << (0U)); // enable interrupt + // RCC->APB2ENR &= ~(1 << 22); // turn debug off #ifdef USE_ADC - ADC_Init(); - enableADC_DMA(); - activateADC(); + ADC_Init(); + enableADC_DMA(); + activateADC(); #endif #ifndef MCU_F031 - __IO uint32_t wait_loop_index = 0; - /* Enable comparator */ - LL_COMP_Enable(MAIN_COMP); + __IO uint32_t wait_loop_index = 0; + /* Enable comparator */ + LL_COMP_Enable(MAIN_COMP); #ifdef N_VARIANT // needs comp 1 and 2 - LL_COMP_Enable(COMP1); + LL_COMP_Enable(COMP1); #endif - wait_loop_index = ((LL_COMP_DELAY_STARTUP_US * (SystemCoreClock / (100000 * 2))) / 10); - while (wait_loop_index != 0) { - wait_loop_index--; - } + wait_loop_index = ((LL_COMP_DELAY_STARTUP_US * (SystemCoreClock / (100000 * 2))) / 10); + while (wait_loop_index != 0) { + wait_loop_index--; + } #endif } void Error_Handler(void) { - // do absolutely nothing + // do absolutely nothing } diff --git a/Mcu/f031/Src/phaseouts.c b/Mcu/f031/Src/phaseouts.c index fdcf49e0..1bc452d2 100644 --- a/Mcu/f031/Src/phaseouts.c +++ b/Mcu/f031/Src/phaseouts.c @@ -31,66 +31,67 @@ extern char prop_brake_active; #endif void proportionalBrake() -{ // alternate all channels between braking (ABC LOW) - // and coasting (ABC float) put lower channel into - // alternate mode and turn upper OFF for each - // channel - // turn all HIGH channels off for ABC - - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; - - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; - - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; - - // set low channel to PWM, duty cycle will now control braking - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); +{ + // alternate all channels between braking (ABC LOW) + // and coasting (ABC float) put lower channel into + // alternate mode and turn upper OFF for each + // channel + // turn all HIGH channels off for ABC + + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; + + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; + + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; + + // set low channel to PWM, duty cycle will now control braking + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); } void phaseBPWM() { - if (!eepromBuffer.comp_pwm) { // for future - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); // low - } - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, - LL_GPIO_MODE_ALTERNATE); // high + if (!eepromBuffer.comp_pwm) { // for future + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); // low + } + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, + LL_GPIO_MODE_ALTERNATE); // high } void phaseBFLOAT() { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; } void phaseBLOW() { - // low mosfet on - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_B_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; + // low mosfet on + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_B_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; } //////////////////////////////PHASE @@ -98,37 +99,37 @@ void phaseBLOW() void phaseCPWM() { - if (!eepromBuffer.comp_pwm) { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); - } - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); + } + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, + LL_GPIO_MODE_ALTERNATE); } void phaseCFLOAT() { - // floating - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; + // floating + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; } void phaseCLOW() { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_C_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_C_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; } ///////////////////////////////////////////////PHASE 3 @@ -136,36 +137,36 @@ void phaseCLOW() void phaseAPWM() { - if (!eepromBuffer.comp_pwm) { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); - } - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); + } + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, + LL_GPIO_MODE_ALTERNATE); } void phaseAFLOAT() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; } void phaseALOW() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_A_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_A_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; } #else @@ -173,38 +174,38 @@ void phaseALOW() //////////////////////////////////PHASE 1////////////////////// void phaseBPWM() { - if (!eepromBuffer.comp_pwm) { // for future - // LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, - // PHASE_B_GPIO_LOW, LL_GPIO_MODE_OUTPUT); - // PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); // high pwm + if (!eepromBuffer.comp_pwm) { // for future + // LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, + // PHASE_B_GPIO_LOW, LL_GPIO_MODE_OUTPUT); + // PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); // high pwm } void phaseBFLOAT() { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable off - PHASE_B_GPIO_PORT_ENABLE->BRR = PHASE_B_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); // pwm off - PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable off + PHASE_B_GPIO_PORT_ENABLE->BRR = PHASE_B_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); // pwm off + PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; } void phaseBLOW() { - // low mosfet on - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); // pwm off - PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; + // low mosfet on + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); // pwm off + PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; } //////////////////////////////PHASE @@ -212,38 +213,38 @@ void phaseBLOW() void phaseCPWM() { - if (!eepromBuffer.comp_pwm) { - // LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - // LL_GPIO_MODE_OUTPUT); PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = - // PHASE_C_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + // LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + // LL_GPIO_MODE_OUTPUT); PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = + // PHASE_C_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); } void phaseCFLOAT() { - // floating - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable off - PHASE_C_GPIO_PORT_ENABLE->BRR = PHASE_C_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; + // floating + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable off + PHASE_C_GPIO_PORT_ENABLE->BRR = PHASE_C_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; } void phaseCLOW() { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; } ///////////////////////////////////////////////PHASE 3 @@ -251,113 +252,115 @@ void phaseCLOW() void phaseAPWM() { - if (!eepromBuffer.comp_pwm) { - // LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - // LL_GPIO_MODE_OUTPUT); PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = - // PHASE_A_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + // LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + // LL_GPIO_MODE_OUTPUT); PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = + // PHASE_A_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); } void phaseAFLOAT() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BRR = PHASE_A_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BRR = PHASE_A_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; } void phaseALOW() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; } #endif void allOff() { - phaseAFLOAT(); - phaseBFLOAT(); - phaseCFLOAT(); + phaseAFLOAT(); + phaseBFLOAT(); + phaseCFLOAT(); } void comStep(int newStep) { - switch (newStep) { - case 1: // A-B - phaseCFLOAT(); - phaseBLOW(); - phaseAPWM(); - break; - - case 2: // C-B - phaseAFLOAT(); - phaseBLOW(); - phaseCPWM(); - break; - - case 3: // C-A - phaseBFLOAT(); - phaseALOW(); - phaseCPWM(); - break; - - case 4: // B-A - phaseCFLOAT(); - phaseALOW(); - phaseBPWM(); - break; - - case 5: // B-C - phaseAFLOAT(); - phaseCLOW(); - phaseBPWM(); - break; - - case 6: // A-C - phaseBFLOAT(); - phaseCLOW(); - phaseAPWM(); - break; - } -} + switch (newStep) { + case 1: // A-B + phaseCFLOAT(); + phaseBLOW(); + phaseAPWM(); + break; -void fullBrake() -{ // full braking shorting all low sides - phaseALOW(); + case 2: // C-B + phaseAFLOAT(); phaseBLOW(); + phaseCPWM(); + break; + + case 3: // C-A + phaseBFLOAT(); + phaseALOW(); + phaseCPWM(); + break; + + case 4: // B-A + phaseCFLOAT(); + phaseALOW(); + phaseBPWM(); + break; + + case 5: // B-C + phaseAFLOAT(); + phaseCLOW(); + phaseBPWM(); + break; + + case 6: // A-C + phaseBFLOAT(); phaseCLOW(); + phaseAPWM(); + break; + } +} + +void fullBrake() +{ + // full braking shorting all low sides + phaseALOW(); + phaseBLOW(); + phaseCLOW(); } void allpwm() -{ // for stepper_sine - phaseAPWM(); - phaseBPWM(); - phaseCPWM(); +{ + // for stepper_sine + phaseAPWM(); + phaseBPWM(); + phaseCPWM(); } void twoChannelForward() { - phaseAPWM(); - phaseBLOW(); - phaseCPWM(); + phaseAPWM(); + phaseBLOW(); + phaseCPWM(); } void twoChannelReverse() { - phaseALOW(); - phaseBPWM(); - phaseCLOW(); + phaseALOW(); + phaseBPWM(); + phaseCLOW(); } diff --git a/Mcu/f031/Src/serial_telemetry.c b/Mcu/f031/Src/serial_telemetry.c index a1ecfcde..4179d3a5 100644 --- a/Mcu/f031/Src/serial_telemetry.c +++ b/Mcu/f031/Src/serial_telemetry.c @@ -12,116 +12,119 @@ uint8_t nbDataToTransmit = sizeof(aTxBuffer); void telem_UART_Init(void) { - LL_USART_InitTypeDef USART_InitStruct = { 0 }; - - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - - /* Peripheral clock enable */ - LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_USART1); - - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); - /**USART1 GPIO Configuration - - PB6 ------> USART1_TX - */ - - GPIO_InitStruct.Pin = LL_GPIO_PIN_6; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; - GPIO_InitStruct.Alternate = LL_GPIO_AF_0; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - /* USART1 DMA Init */ - - /* USART1_TX Init */ - LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_4, - LL_DMA_DIRECTION_MEMORY_TO_PERIPH); - LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_4, LL_DMA_PRIORITY_LOW); - LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_4, LL_DMA_MODE_NORMAL); - LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_4, LL_DMA_PERIPH_NOINCREMENT); - LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_4, LL_DMA_MEMORY_INCREMENT); - LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_4, LL_DMA_PDATAALIGN_BYTE); - LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_4, LL_DMA_MDATAALIGN_BYTE); - LL_SYSCFG_SetRemapDMA_USART(LL_SYSCFG_USART1TX_RMP_DMA1CH4); - - /* USART1 interrupt Init */ - NVIC_SetPriority(USART1_IRQn, 3); - NVIC_EnableIRQ(USART1_IRQn); - - USART_InitStruct.BaudRate = 115200; - USART_InitStruct.DataWidth = LL_USART_DATAWIDTH_8B; - USART_InitStruct.StopBits = LL_USART_STOPBITS_1; - USART_InitStruct.Parity = LL_USART_PARITY_NONE; - USART_InitStruct.TransferDirection = LL_USART_DIRECTION_TX; - USART_InitStruct.HardwareFlowControl = LL_USART_HWCONTROL_NONE; - USART_InitStruct.OverSampling = LL_USART_OVERSAMPLING_16; - LL_USART_Init(USART1, &USART_InitStruct); - LL_USART_DisableIT_CTS(USART1); - LL_USART_ConfigAsyncMode(USART1); - LL_USART_Enable(USART1); - - // set dma address - LL_DMA_ConfigAddresses( - DMA1, LL_DMA_CHANNEL_4, (uint32_t)aTxBuffer, - LL_USART_DMA_GetRegAddr(USART1, LL_USART_DMA_REG_DATA_TRANSMIT), - LL_DMA_GetDataTransferDirection(DMA1, LL_DMA_CHANNEL_4)); - LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_4, nbDataToTransmit); - - /* (5) Enable DMA transfer complete/error interrupts */ - // LL_DMA_EnableIT_TC(DMA1, LL_DMA_CHANNEL_4); - // LL_DMA_EnableIT_TE(DMA1, LL_DMA_CHANNEL_4); - - // NVIC_SetPriority(DMA1_Channel4_5_IRQn, 2); - // NVIC_EnableIRQ(DMA1_Channel4_5_IRQn); + LL_USART_InitTypeDef USART_InitStruct = { 0 }; + + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + + /* Peripheral clock enable */ + LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_USART1); + + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); + /**USART1 GPIO Configuration + + PB6 ------> USART1_TX + */ + + GPIO_InitStruct.Pin = LL_GPIO_PIN_6; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; + GPIO_InitStruct.Alternate = LL_GPIO_AF_0; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + /* USART1 DMA Init */ + + /* USART1_TX Init */ + LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_4, + LL_DMA_DIRECTION_MEMORY_TO_PERIPH); + LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_4, LL_DMA_PRIORITY_LOW); + LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_4, LL_DMA_MODE_NORMAL); + LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_4, LL_DMA_PERIPH_NOINCREMENT); + LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_4, LL_DMA_MEMORY_INCREMENT); + LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_4, LL_DMA_PDATAALIGN_BYTE); + LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_4, LL_DMA_MDATAALIGN_BYTE); + LL_SYSCFG_SetRemapDMA_USART(LL_SYSCFG_USART1TX_RMP_DMA1CH4); + + /* USART1 interrupt Init */ + NVIC_SetPriority(USART1_IRQn, 3); + NVIC_EnableIRQ(USART1_IRQn); + + USART_InitStruct.BaudRate = 115200; + USART_InitStruct.DataWidth = LL_USART_DATAWIDTH_8B; + USART_InitStruct.StopBits = LL_USART_STOPBITS_1; + USART_InitStruct.Parity = LL_USART_PARITY_NONE; + USART_InitStruct.TransferDirection = LL_USART_DIRECTION_TX; + USART_InitStruct.HardwareFlowControl = LL_USART_HWCONTROL_NONE; + USART_InitStruct.OverSampling = LL_USART_OVERSAMPLING_16; + LL_USART_Init(USART1, &USART_InitStruct); + LL_USART_DisableIT_CTS(USART1); + LL_USART_ConfigAsyncMode(USART1); + LL_USART_Enable(USART1); + + // set dma address + LL_DMA_ConfigAddresses( + DMA1, LL_DMA_CHANNEL_4, (uint32_t)aTxBuffer, + LL_USART_DMA_GetRegAddr(USART1, LL_USART_DMA_REG_DATA_TRANSMIT), + LL_DMA_GetDataTransferDirection(DMA1, LL_DMA_CHANNEL_4)); + LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_4, nbDataToTransmit); + + /* (5) Enable DMA transfer complete/error interrupts */ + // LL_DMA_EnableIT_TC(DMA1, LL_DMA_CHANNEL_4); + // LL_DMA_EnableIT_TE(DMA1, LL_DMA_CHANNEL_4); + + // NVIC_SetPriority(DMA1_Channel4_5_IRQn, 2); + // NVIC_EnableIRQ(DMA1_Channel4_5_IRQn); } void send_telem_DMA() -{ // set data length and enable channel to start transfer - LL_USART_SetTransferDirection(USART1, LL_USART_DIRECTION_TX); - // GPIOB->OTYPER &= 0 << 6; - LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_4, nbDataToTransmit); - LL_USART_EnableDMAReq_TX(USART1); - - LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_4); - LL_USART_SetTransferDirection(USART1, LL_USART_DIRECTION_RX); +{ + // set data length and enable channel to start transfer + LL_USART_SetTransferDirection(USART1, LL_USART_DIRECTION_TX); + // GPIOB->OTYPER &= 0 << 6; + LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_4, nbDataToTransmit); + LL_USART_EnableDMAReq_TX(USART1); + + LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_4); + LL_USART_SetTransferDirection(USART1, LL_USART_DIRECTION_RX); } uint8_t update_crc8(uint8_t crc, uint8_t crc_seed) { - uint8_t crc_u, i; - crc_u = crc; - crc_u ^= crc_seed; - for (i = 0; i < 8; i++) - crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); - return (crc_u); + uint8_t crc_u, i; + crc_u = crc; + crc_u ^= crc_seed; + for (i = 0; i < 8; i++) { + crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); + } + return (crc_u); } uint8_t get_crc8(uint8_t* Buf, uint8_t BufLen) { - uint8_t crc = 0, i; - for (i = 0; i < BufLen; i++) - crc = update_crc8(Buf[i], crc); - return (crc); + uint8_t crc = 0, i; + for (i = 0; i < BufLen; i++) { + crc = update_crc8(Buf[i], crc); + } + return (crc); } void makeTelemPackage(uint8_t temp, uint16_t voltage, uint16_t current, - uint16_t consumption, uint16_t e_rpm) + uint16_t consumption, uint16_t e_rpm) { - aTxBuffer[0] = temp; // temperature + aTxBuffer[0] = temp; // temperature - aTxBuffer[1] = (voltage >> 8) & 0xFF; // voltage hB - aTxBuffer[2] = voltage & 0xFF; // voltage lowB + aTxBuffer[1] = (voltage >> 8) & 0xFF; // voltage hB + aTxBuffer[2] = voltage & 0xFF; // voltage lowB - aTxBuffer[3] = (current >> 8) & 0xFF; // current - aTxBuffer[4] = current & 0xFF; // divide by 10 for Amps + aTxBuffer[3] = (current >> 8) & 0xFF; // current + aTxBuffer[4] = current & 0xFF; // divide by 10 for Amps - aTxBuffer[5] = (consumption >> 8) & 0xFF; // consumption - aTxBuffer[6] = consumption & 0xFF; // in mah + aTxBuffer[5] = (consumption >> 8) & 0xFF; // consumption + aTxBuffer[6] = consumption & 0xFF; // in mah - aTxBuffer[7] = (e_rpm >> 8) & 0xFF; // - aTxBuffer[8] = e_rpm & 0xFF; // eRpM *100 + aTxBuffer[7] = (e_rpm >> 8) & 0xFF; // + aTxBuffer[8] = e_rpm & 0xFF; // eRpM *100 - aTxBuffer[9] = get_crc8(aTxBuffer, 9); + aTxBuffer[9] = get_crc8(aTxBuffer, 9); } diff --git a/Mcu/f031/Src/stm32f0xx_it.c b/Mcu/f031/Src/stm32f0xx_it.c index 5e0c5eed..4624b516 100644 --- a/Mcu/f031/Src/stm32f0xx_it.c +++ b/Mcu/f031/Src/stm32f0xx_it.c @@ -95,12 +95,12 @@ uint16_t interrupt_time = 0; */ void NMI_Handler(void) { - /* USER CODE BEGIN NonMaskableInt_IRQn 0 */ + /* USER CODE BEGIN NonMaskableInt_IRQn 0 */ - /* USER CODE END NonMaskableInt_IRQn 0 */ - /* USER CODE BEGIN NonMaskableInt_IRQn 1 */ + /* USER CODE END NonMaskableInt_IRQn 0 */ + /* USER CODE BEGIN NonMaskableInt_IRQn 1 */ - /* USER CODE END NonMaskableInt_IRQn 1 */ + /* USER CODE END NonMaskableInt_IRQn 1 */ } /** @@ -108,13 +108,13 @@ void NMI_Handler(void) */ void HardFault_Handler(void) { - /* USER CODE BEGIN HardFault_IRQn 0 */ + /* USER CODE BEGIN HardFault_IRQn 0 */ - /* USER CODE END HardFault_IRQn 0 */ - while (1) { - /* USER CODE BEGIN W1_HardFault_IRQn 0 */ - /* USER CODE END W1_HardFault_IRQn 0 */ - } + /* USER CODE END HardFault_IRQn 0 */ + while (1) { + /* USER CODE BEGIN W1_HardFault_IRQn 0 */ + /* USER CODE END W1_HardFault_IRQn 0 */ + } } /** @@ -122,12 +122,12 @@ void HardFault_Handler(void) */ void SVC_Handler(void) { - /* USER CODE BEGIN SVC_IRQn 0 */ + /* USER CODE BEGIN SVC_IRQn 0 */ - /* USER CODE END SVC_IRQn 0 */ - /* USER CODE BEGIN SVC_IRQn 1 */ + /* USER CODE END SVC_IRQn 0 */ + /* USER CODE BEGIN SVC_IRQn 1 */ - /* USER CODE END SVC_IRQn 1 */ + /* USER CODE END SVC_IRQn 1 */ } /** @@ -135,12 +135,12 @@ void SVC_Handler(void) */ void PendSV_Handler(void) { - /* USER CODE BEGIN PendSV_IRQn 0 */ + /* USER CODE BEGIN PendSV_IRQn 0 */ - /* USER CODE END PendSV_IRQn 0 */ - /* USER CODE BEGIN PendSV_IRQn 1 */ + /* USER CODE END PendSV_IRQn 0 */ + /* USER CODE BEGIN PendSV_IRQn 1 */ - /* USER CODE END PendSV_IRQn 1 */ + /* USER CODE END PendSV_IRQn 1 */ } /** @@ -148,13 +148,13 @@ void PendSV_Handler(void) */ void SysTick_Handler(void) { - /* USER CODE BEGIN SysTick_IRQn 0 */ + /* USER CODE BEGIN SysTick_IRQn 0 */ - /* USER CODE END SysTick_IRQn 0 */ + /* USER CODE END SysTick_IRQn 0 */ - /* USER CODE BEGIN SysTick_IRQn 1 */ + /* USER CODE BEGIN SysTick_IRQn 1 */ - /* USER CODE END SysTick_IRQn 1 */ + /* USER CODE END SysTick_IRQn 1 */ } /******************************************************************************/ @@ -169,44 +169,44 @@ void SysTick_Handler(void) */ void DMA1_Channel2_3_IRQHandler(void) { - /* USER CODE BEGIN DMA1_Channel2_3_IRQn 0 */ - /* USER CODE BEGIN DMA1_Channel5_IRQn 0 */ - if (LL_DMA_IsActiveFlag_HT3(DMA1)) { - if (servoPwm) { - LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, - LL_TIM_IC_POLARITY_FALLING); - LL_DMA_ClearFlag_HT3(DMA1); - } - } - if (LL_DMA_IsActiveFlag_TC3(DMA1) == 1) { - LL_DMA_ClearFlag_GI3(DMA1); - - LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_3); - transfercomplete(); - input_ready = 1; - } else if (LL_DMA_IsActiveFlag_TE3(DMA1) == 1) { - LL_DMA_ClearFlag_GI3(DMA1); + /* USER CODE BEGIN DMA1_Channel2_3_IRQn 0 */ + /* USER CODE BEGIN DMA1_Channel5_IRQn 0 */ + if (LL_DMA_IsActiveFlag_HT3(DMA1)) { + if (servoPwm) { + LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, + LL_TIM_IC_POLARITY_FALLING); + LL_DMA_ClearFlag_HT3(DMA1); } - /* USER CODE END DMA1_Channel2_3_IRQn 0 */ - if (LL_DMA_IsActiveFlag_TC2(DMA1) == 1) { - /* Clear flag DMA global interrupt */ - /* (global interrupt flag: half transfer and transfer complete flags) */ - LL_DMA_ClearFlag_GI2(DMA1); - ADC_DMA_Callback(); - /* Call interruption treatment function */ - // AdcDmaTransferComplete_Callback(); - } - - /* Check whether DMA transfer error caused the DMA interruption */ - if (LL_DMA_IsActiveFlag_TE2(DMA1) == 1) { - /* Clear flag DMA transfer error */ - LL_DMA_ClearFlag_TE2(DMA1); - - /* Call interruption treatment function */ - } - /* USER CODE BEGIN DMA1_Channel2_3_IRQn 1 */ - - /* USER CODE END DMA1_Channel2_3_IRQn 1 */ + } + if (LL_DMA_IsActiveFlag_TC3(DMA1) == 1) { + LL_DMA_ClearFlag_GI3(DMA1); + + LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_3); + transfercomplete(); + input_ready = 1; + } else if (LL_DMA_IsActiveFlag_TE3(DMA1) == 1) { + LL_DMA_ClearFlag_GI3(DMA1); + } + /* USER CODE END DMA1_Channel2_3_IRQn 0 */ + if (LL_DMA_IsActiveFlag_TC2(DMA1) == 1) { + /* Clear flag DMA global interrupt */ + /* (global interrupt flag: half transfer and transfer complete flags) */ + LL_DMA_ClearFlag_GI2(DMA1); + ADC_DMA_Callback(); + /* Call interruption treatment function */ + // AdcDmaTransferComplete_Callback(); + } + + /* Check whether DMA transfer error caused the DMA interruption */ + if (LL_DMA_IsActiveFlag_TE2(DMA1) == 1) { + /* Clear flag DMA transfer error */ + LL_DMA_ClearFlag_TE2(DMA1); + + /* Call interruption treatment function */ + } + /* USER CODE BEGIN DMA1_Channel2_3_IRQn 1 */ + + /* USER CODE END DMA1_Channel2_3_IRQn 1 */ } /** @@ -215,56 +215,56 @@ void DMA1_Channel2_3_IRQHandler(void) void DMA1_Channel4_5_IRQHandler(void) { #ifdef USE_SERIAL_TELEMETRY - if (LL_DMA_IsActiveFlag_TC4(DMA1)) { - LL_DMA_ClearFlag_GI4(DMA1); - LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_4); - /* Call function Transmission complete Callback */ - } else if (LL_DMA_IsActiveFlag_TE4(DMA1)) { - LL_DMA_ClearFlag_GI4(DMA1); - LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_4); - /* Call Error function */ - // USART_TransferError_Callback(); - } + if (LL_DMA_IsActiveFlag_TC4(DMA1)) { + LL_DMA_ClearFlag_GI4(DMA1); + LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_4); + /* Call function Transmission complete Callback */ + } else if (LL_DMA_IsActiveFlag_TE4(DMA1)) { + LL_DMA_ClearFlag_GI4(DMA1); + LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_4); + /* Call Error function */ + // USART_TransferError_Callback(); + } #endif #ifdef USE_TIMER_2_CHANNEL_4 - if (LL_DMA_IsActiveFlag_HT4(DMA1)) { - if (servoPwm) { - LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, - LL_TIM_IC_POLARITY_FALLING); - LL_DMA_ClearFlag_HT4(DMA1); - } + if (LL_DMA_IsActiveFlag_HT4(DMA1)) { + if (servoPwm) { + LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, + LL_TIM_IC_POLARITY_FALLING); + LL_DMA_ClearFlag_HT4(DMA1); } - if (LL_DMA_IsActiveFlag_TC4(DMA1) == 1) { - LL_DMA_ClearFlag_GI4(DMA1); + } + if (LL_DMA_IsActiveFlag_TC4(DMA1) == 1) { + LL_DMA_ClearFlag_GI4(DMA1); - LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_4); + LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_4); - transfercomplete(); - } else if (LL_DMA_IsActiveFlag_TE4(DMA1) == 1) { - LL_DMA_ClearFlag_GI4(DMA1); - } + transfercomplete(); + } else if (LL_DMA_IsActiveFlag_TE4(DMA1) == 1) { + LL_DMA_ClearFlag_GI4(DMA1); + } #endif #ifdef USE_TIMER_2_CHANNEL_1 - if (LL_DMA_IsActiveFlag_HT5(DMA1)) { - if (servoPwm) { - LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, - LL_TIM_IC_POLARITY_FALLING); - LL_DMA_ClearFlag_HT5(DMA1); - } + if (LL_DMA_IsActiveFlag_HT5(DMA1)) { + if (servoPwm) { + LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, + LL_TIM_IC_POLARITY_FALLING); + LL_DMA_ClearFlag_HT5(DMA1); } - if (LL_DMA_IsActiveFlag_TC5(DMA1) == 1) { - LL_DMA_ClearFlag_GI5(DMA1); + } + if (LL_DMA_IsActiveFlag_TC5(DMA1) == 1) { + LL_DMA_ClearFlag_GI5(DMA1); - LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_5); + LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_5); - transfercomplete(); - } else if (LL_DMA_IsActiveFlag_TE5(DMA1) == 1) { - LL_DMA_ClearFlag_GI5(DMA1); - } + transfercomplete(); + } else if (LL_DMA_IsActiveFlag_TE5(DMA1) == 1) { + LL_DMA_ClearFlag_GI5(DMA1); + } #endif - /* USER CODE BEGIN DMA1_Channel4_5_IRQn 1 */ + /* USER CODE BEGIN DMA1_Channel4_5_IRQn 1 */ - /* USER CODE END DMA1_Channel4_5_IRQn 1 */ + /* USER CODE END DMA1_Channel4_5_IRQn 1 */ } /** @@ -272,18 +272,18 @@ void DMA1_Channel4_5_IRQHandler(void) */ void TIM2_IRQHandler(void) { - if (LL_TIM_IsActiveFlag_CC4(TIM2) == 1) { - LL_TIM_ClearFlag_CC4(TIM2); - } - - if (LL_TIM_IsActiveFlag_UPDATE(TIM2) == 1) { - LL_TIM_ClearFlag_UPDATE(TIM2); - update_interupt++; - } - - if (LL_TIM_IsActiveFlag_CC1(TIM2) == 1) { - LL_TIM_ClearFlag_CC1(TIM2); - } + if (LL_TIM_IsActiveFlag_CC4(TIM2) == 1) { + LL_TIM_ClearFlag_CC4(TIM2); + } + + if (LL_TIM_IsActiveFlag_UPDATE(TIM2) == 1) { + LL_TIM_ClearFlag_UPDATE(TIM2); + update_interupt++; + } + + if (LL_TIM_IsActiveFlag_CC1(TIM2) == 1) { + LL_TIM_ClearFlag_CC1(TIM2); + } } /** @@ -291,17 +291,17 @@ void TIM2_IRQHandler(void) */ void TIM14_IRQHandler(void) { - /* USER CODE BEGIN TIM14_IRQn 0 */ - if (LL_TIM_IsActiveFlag_UPDATE(TIM14) == 1) { - interrupt_time = UTILITY_TIMER->CNT; - PeriodElapsedCallback(); - LL_TIM_ClearFlag_UPDATE(TIM14); - interrupt_time = ((uint16_t)UTILITY_TIMER->CNT) - interrupt_time; - } - /* USER CODE END TIM14_IRQn 0 */ - /* USER CODE BEGIN TIM14_IRQn 1 */ - - /* USER CODE END TIM14_IRQn 1 */ + /* USER CODE BEGIN TIM14_IRQn 0 */ + if (LL_TIM_IsActiveFlag_UPDATE(TIM14) == 1) { + interrupt_time = UTILITY_TIMER->CNT; + PeriodElapsedCallback(); + LL_TIM_ClearFlag_UPDATE(TIM14); + interrupt_time = ((uint16_t)UTILITY_TIMER->CNT) - interrupt_time; + } + /* USER CODE END TIM14_IRQn 0 */ + /* USER CODE BEGIN TIM14_IRQn 1 */ + + /* USER CODE END TIM14_IRQn 1 */ } /** @@ -310,19 +310,19 @@ void TIM14_IRQHandler(void) void TIM16_IRQHandler(void) { #ifdef USE_TIM_16 - if (LL_TIM_IsActiveFlag_CC1(TIM16) == 1) { - LL_TIM_ClearFlag_CC1(TIM16); - } - - if (LL_TIM_IsActiveFlag_UPDATE(TIM16) == 1) { - LL_TIM_ClearFlag_UPDATE(TIM16); - update_interupt++; - } + if (LL_TIM_IsActiveFlag_CC1(TIM16) == 1) { + LL_TIM_ClearFlag_CC1(TIM16); + } + + if (LL_TIM_IsActiveFlag_UPDATE(TIM16) == 1) { + LL_TIM_ClearFlag_UPDATE(TIM16); + update_interupt++; + } #else - if (LL_TIM_IsActiveFlag_UPDATE(TIM16) == 1) { - LL_TIM_ClearFlag_UPDATE(TIM16); - tenKhzRoutine(); - } + if (LL_TIM_IsActiveFlag_UPDATE(TIM16) == 1) { + LL_TIM_ClearFlag_UPDATE(TIM16); + tenKhzRoutine(); + } #endif } @@ -344,55 +344,56 @@ void TIM16_IRQHandler(void) void EXTI4_15_IRQHandler(void) { - if (LL_EXTI_IsActiveFlag_0_31(PHASE_C_LL_EXTI_LINE) != RESET) { - LL_EXTI_ClearFlag_0_31(PHASE_C_LL_EXTI_LINE); + if (LL_EXTI_IsActiveFlag_0_31(PHASE_C_LL_EXTI_LINE) != RESET) { + LL_EXTI_ClearFlag_0_31(PHASE_C_LL_EXTI_LINE); - interruptRoutine(); - } + interruptRoutine(); + } } // #ifdef BLUE_BOARD void EXTI0_1_IRQHandler(void) -{ // interrupt_time = UTILITY_TIMER->CNT; - if (LL_EXTI_IsActiveFlag_0_31(LL_EXTI_LINE_0) != RESET) { - LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_0); - interruptRoutine(); - } - if (LL_EXTI_IsActiveFlag_0_31(LL_EXTI_LINE_1) != RESET) { - LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_1); - interruptRoutine(); - } - // +{ + // interrupt_time = UTILITY_TIMER->CNT; + if (LL_EXTI_IsActiveFlag_0_31(LL_EXTI_LINE_0) != RESET) { + LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_0); + interruptRoutine(); + } + if (LL_EXTI_IsActiveFlag_0_31(LL_EXTI_LINE_1) != RESET) { + LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_1); + interruptRoutine(); + } + // } void EXTI2_3_IRQHandler(void) { - if (LL_EXTI_IsActiveFlag_0_31(LL_EXTI_LINE_2) != RESET) { - LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_2); - interruptRoutine(); - } + if (LL_EXTI_IsActiveFlag_0_31(LL_EXTI_LINE_2) != RESET) { + LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_2); + interruptRoutine(); + } } // #endif void DMA1_Channel1_IRQHandler(void) { - if (LL_DMA_IsActiveFlag_HT1(DMA1)) { - if (servoPwm) { - LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, - LL_TIM_IC_POLARITY_FALLING); - LL_DMA_ClearFlag_HT1(DMA1); - } + if (LL_DMA_IsActiveFlag_HT1(DMA1)) { + if (servoPwm) { + LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, + LL_TIM_IC_POLARITY_FALLING); + LL_DMA_ClearFlag_HT1(DMA1); } - if (LL_DMA_IsActiveFlag_TC1(DMA1) == 1) { - LL_DMA_ClearFlag_GI1(DMA1); + } + if (LL_DMA_IsActiveFlag_TC1(DMA1) == 1) { + LL_DMA_ClearFlag_GI1(DMA1); - LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_1); + LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_1); - transfercomplete(); - input_ready = 1; - } else if (LL_DMA_IsActiveFlag_TE1(DMA1) == 1) { - LL_DMA_ClearFlag_GI1(DMA1); - } + transfercomplete(); + input_ready = 1; + } else if (LL_DMA_IsActiveFlag_TE1(DMA1) == 1) { + LL_DMA_ClearFlag_GI1(DMA1); + } } /* USER CODE END 1 */ diff --git a/Mcu/f031/Src/system_stm32f0xx.c b/Mcu/f031/Src/system_stm32f0xx.c index f7f1811f..8b8afddc 100644 --- a/Mcu/f031/Src/system_stm32f0xx.c +++ b/Mcu/f031/Src/system_stm32f0xx.c @@ -125,7 +125,8 @@ uint32_t SystemCoreClock = 8000000; const uint8_t AHBPrescTable[16] = { 0, 0, 0, 0, 0, 0, 0, 0, - 1, 2, 3, 4, 6, 7, 8, 9 }; + 1, 2, 3, 4, 6, 7, 8, 9 + }; const uint8_t APBPrescTable[8] = { 0, 0, 0, 0, 1, 2, 3, 4 }; /** @@ -151,12 +152,12 @@ const uint8_t APBPrescTable[8] = { 0, 0, 0, 0, 1, 2, 3, 4 }; */ void SystemInit(void) { - /* NOTE :SystemInit(): This function is called at startup just after reset - and before branch to main program. This call is made inside the - "startup_stm32f0xx.s" file. User can setups the default system clock - (System clock source, PLL Multiplier and Divider factors, AHB/APBx - prescalers and Flash settings). - */ + /* NOTE :SystemInit(): This function is called at startup just after reset + and before branch to main program. This call is made inside the + "startup_stm32f0xx.s" file. User can setups the default system clock + (System clock source, PLL Multiplier and Divider factors, AHB/APBx + prescalers and Flash settings). + */ } /** @@ -199,64 +200,64 @@ void SystemInit(void) */ void SystemCoreClockUpdate(void) { - uint32_t tmp = 0, pllmull = 0, pllsource = 0, predivfactor = 0; + uint32_t tmp = 0, pllmull = 0, pllsource = 0, predivfactor = 0; - /* Get SYSCLK source - * -------------------------------------------------------*/ - tmp = RCC->CFGR & RCC_CFGR_SWS; + /* Get SYSCLK source + * -------------------------------------------------------*/ + tmp = RCC->CFGR & RCC_CFGR_SWS; - switch (tmp) { - case RCC_CFGR_SWS_HSI: /* HSI used as system clock */ - SystemCoreClock = HSI_VALUE; - break; - case RCC_CFGR_SWS_HSE: /* HSE used as system clock */ - SystemCoreClock = HSE_VALUE; - break; - case RCC_CFGR_SWS_PLL: /* PLL used as system clock */ - /* Get PLL clock source and multiplication factor - * ----------------------*/ - pllmull = RCC->CFGR & RCC_CFGR_PLLMUL; - pllsource = RCC->CFGR & RCC_CFGR_PLLSRC; - pllmull = (pllmull >> 18) + 2; - predivfactor = (RCC->CFGR2 & RCC_CFGR2_PREDIV) + 1; + switch (tmp) { + case RCC_CFGR_SWS_HSI: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + case RCC_CFGR_SWS_HSE: /* HSE used as system clock */ + SystemCoreClock = HSE_VALUE; + break; + case RCC_CFGR_SWS_PLL: /* PLL used as system clock */ + /* Get PLL clock source and multiplication factor + * ----------------------*/ + pllmull = RCC->CFGR & RCC_CFGR_PLLMUL; + pllsource = RCC->CFGR & RCC_CFGR_PLLSRC; + pllmull = (pllmull >> 18) + 2; + predivfactor = (RCC->CFGR2 & RCC_CFGR2_PREDIV) + 1; - if (pllsource == RCC_CFGR_PLLSRC_HSE_PREDIV) { - /* HSE used as PLL clock source : SystemCoreClock = HSE/PREDIV * - * PLLMUL - */ - SystemCoreClock = (HSE_VALUE / predivfactor) * pllmull; - } + if (pllsource == RCC_CFGR_PLLSRC_HSE_PREDIV) { + /* HSE used as PLL clock source : SystemCoreClock = HSE/PREDIV * + * PLLMUL + */ + SystemCoreClock = (HSE_VALUE / predivfactor) * pllmull; + } #if defined(STM32F042x6) || defined(STM32F048xx) || defined(STM32F072xB) || defined(STM32F078xx) || defined(STM32F091xC) || defined(STM32F098xx) - else if (pllsource == RCC_CFGR_PLLSRC_HSI48_PREDIV) { - /* HSI48 used as PLL clock source : SystemCoreClock = HSI48/PREDIV * - * PLLMUL */ - SystemCoreClock = (HSI48_VALUE / predivfactor) * pllmull; - } + else if (pllsource == RCC_CFGR_PLLSRC_HSI48_PREDIV) { + /* HSI48 used as PLL clock source : SystemCoreClock = HSI48/PREDIV * + * PLLMUL */ + SystemCoreClock = (HSI48_VALUE / predivfactor) * pllmull; + } #endif /* STM32F042x6 || STM32F048xx || STM32F072xB || STM32F078xx || \ STM32F091xC || STM32F098xx */ - else { + else { #if defined(STM32F042x6) || defined(STM32F048xx) || defined(STM32F070x6) || defined(STM32F078xx) || defined(STM32F071xB) || defined(STM32F072xB) || defined(STM32F070xB) || defined(STM32F091xC) || defined(STM32F098xx) || defined(STM32F030xC) - /* HSI used as PLL clock source : SystemCoreClock = HSI/PREDIV * - * PLLMUL - */ - SystemCoreClock = (HSI_VALUE / predivfactor) * pllmull; + /* HSI used as PLL clock source : SystemCoreClock = HSI/PREDIV * + * PLLMUL + */ + SystemCoreClock = (HSI_VALUE / predivfactor) * pllmull; #else - /* HSI used as PLL clock source : SystemCoreClock = HSI/2 * PLLMUL */ - SystemCoreClock = (HSI_VALUE >> 1) * pllmull; + /* HSI used as PLL clock source : SystemCoreClock = HSI/2 * PLLMUL */ + SystemCoreClock = (HSI_VALUE >> 1) * pllmull; #endif /* STM32F042x6 || STM32F048xx || STM32F070x6 || \ STM32F071xB || STM32F072xB || STM32F078xx || STM32F070xB || \ STM32F091xC || STM32F098xx || STM32F030xC */ - } - break; - default: /* HSI used as system clock */ - SystemCoreClock = HSI_VALUE; - break; } - /* Compute HCLK clock frequency ----------------*/ - /* Get HCLK prescaler */ - tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; - /* HCLK clock frequency */ - SystemCoreClock >>= tmp; + break; + default: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + } + /* Compute HCLK clock frequency ----------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; + /* HCLK clock frequency */ + SystemCoreClock >>= tmp; } /** diff --git a/Mcu/f051/Inc/IO.h b/Mcu/f051/Inc/IO.h index 8b1d7459..9be98616 100644 --- a/Mcu/f051/Inc/IO.h +++ b/Mcu/f051/Inc/IO.h @@ -30,6 +30,6 @@ extern uint8_t degrees_celsius; extern uint16_t ADC_raw_volts; extern uint16_t servo_low_threshold; // anything below this point considered 0 extern uint16_t - servo_high_threshold; // anything above this point considered 2000 (max) +servo_high_threshold; // anything above this point considered 2000 (max) extern uint16_t servo_neutral; extern uint8_t servo_dead_band; diff --git a/Mcu/f051/Inc/serial_telemetry.h b/Mcu/f051/Inc/serial_telemetry.h index b4f1c5cf..fb8cd3b9 100644 --- a/Mcu/f051/Inc/serial_telemetry.h +++ b/Mcu/f051/Inc/serial_telemetry.h @@ -11,7 +11,7 @@ #define SERIAL_TELEMETRY_H_ void makeTelemPackage(uint8_t temp, uint16_t voltage, uint16_t current, - uint16_t consumption, uint16_t e_rpm); + uint16_t consumption, uint16_t e_rpm); void telem_UART_Init(void); void send_telem_DMA(); diff --git a/Mcu/f051/Src/ADC.c b/Mcu/f051/Src/ADC.c index be8d668a..97b471cd 100644 --- a/Mcu/f051/Src/ADC.c +++ b/Mcu/f051/Src/ADC.c @@ -18,155 +18,158 @@ extern uint16_t ADC_raw_current; extern uint16_t ADC_raw_input; void ADC_DMA_Callback() -{ // read dma buffer and set extern variables +{ + // read dma buffer and set extern variables #ifdef USE_ADC_INPUT - ADC_raw_temp = ADCDataDMA[3]; - ADC_raw_volts = ADCDataDMA[1] / 2; - ADC_raw_current = ADCDataDMA[2]; - ADC_raw_input = ADCDataDMA[0]; + ADC_raw_temp = ADCDataDMA[3]; + ADC_raw_volts = ADCDataDMA[1] / 2; + ADC_raw_current = ADCDataDMA[2]; + ADC_raw_input = ADCDataDMA[0]; #else - ADC_raw_temp = ADCDataDMA[2]; - if (VOLTAGE_ADC_PIN > CURRENT_ADC_PIN) { - ADC_raw_volts = ADCDataDMA[1]; - ADC_raw_current = ADCDataDMA[0]; - } else { - ADC_raw_volts = ADCDataDMA[0]; - ADC_raw_current = ADCDataDMA[1]; - } + ADC_raw_temp = ADCDataDMA[2]; + if (VOLTAGE_ADC_PIN > CURRENT_ADC_PIN) { + ADC_raw_volts = ADCDataDMA[1]; + ADC_raw_current = ADCDataDMA[0]; + } else { + ADC_raw_volts = ADCDataDMA[0]; + ADC_raw_current = ADCDataDMA[1]; + } #endif } void enableADC_DMA() -{ // enables channel +{ + // enables channel - // NVIC_SetPriority(DMA1_Channel1_IRQn, 3); - // NVIC_EnableIRQ(DMA1_Channel1_IRQn); + // NVIC_SetPriority(DMA1_Channel1_IRQn, 3); + // NVIC_EnableIRQ(DMA1_Channel1_IRQn); - LL_DMA_ConfigAddresses( - DMA1, LL_DMA_CHANNEL_1, - LL_ADC_DMA_GetRegAddr(ADC1, LL_ADC_DMA_REG_REGULAR_DATA), - (uint32_t)&ADCDataDMA, LL_DMA_DIRECTION_PERIPH_TO_MEMORY); + LL_DMA_ConfigAddresses( + DMA1, LL_DMA_CHANNEL_1, + LL_ADC_DMA_GetRegAddr(ADC1, LL_ADC_DMA_REG_REGULAR_DATA), + (uint32_t)&ADCDataDMA, LL_DMA_DIRECTION_PERIPH_TO_MEMORY); - /* Set DMA transfer size */ + /* Set DMA transfer size */ #ifdef USE_ADC_INPUT - LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_1, 4); + LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_1, 4); #else - LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_1, 3); + LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_1, 3); #endif - // LL_DMA_EnableIT_TC(DMA1,LL_DMA_CHANNEL_1); + // LL_DMA_EnableIT_TC(DMA1,LL_DMA_CHANNEL_1); - // LL_DMA_EnableIT_TE(DMA1,LL_DMA_CHANNEL_1); + // LL_DMA_EnableIT_TE(DMA1,LL_DMA_CHANNEL_1); - /*## Activation of DMA - * #####################################################*/ - /* Enable the DMA transfer */ - LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_1); + /*## Activation of DMA + * #####################################################*/ + /* Enable the DMA transfer */ + LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_1); } void activateADC() -{ // called right after enable regular conversions are - // started by software and DMA interrupt happens at end of - // transfer +{ + // called right after enable regular conversions are + // started by software and DMA interrupt happens at end of + // transfer - __IO uint32_t wait_loop_index = 0; + __IO uint32_t wait_loop_index = 0; - LL_ADC_StartCalibration(ADC1); + LL_ADC_StartCalibration(ADC1); - /* Poll for ADC effectively calibrated */ + /* Poll for ADC effectively calibrated */ - while (LL_ADC_IsCalibrationOnGoing(ADC1) != 0) { - } - wait_loop_index = (LL_ADC_DELAY_CALIB_ENABLE_ADC_CYCLES >> 1); - while (wait_loop_index != 0) { - wait_loop_index--; - } - LL_ADC_Enable(ADC1); - while (LL_ADC_IsActiveFlag_ADRDY(ADC1) == 0) { - } + while (LL_ADC_IsCalibrationOnGoing(ADC1) != 0) { + } + wait_loop_index = (LL_ADC_DELAY_CALIB_ENABLE_ADC_CYCLES >> 1); + while (wait_loop_index != 0) { + wait_loop_index--; + } + LL_ADC_Enable(ADC1); + while (LL_ADC_IsActiveFlag_ADRDY(ADC1) == 0) { + } } void ADC_Init(void) { - LL_ADC_InitTypeDef ADC_InitStruct = { 0 }; - LL_ADC_REG_InitTypeDef ADC_REG_InitStruct = { 0 }; + LL_ADC_InitTypeDef ADC_InitStruct = { 0 }; + LL_ADC_REG_InitTypeDef ADC_REG_InitStruct = { 0 }; - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - /* Peripheral clock enable */ - LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_ADC1); + /* Peripheral clock enable */ + LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_ADC1); - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); - /**ADC GPIO Configuration - PA3 ------> ADC_IN3 - PA6 ------> ADC_IN6 - */ + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); + /**ADC GPIO Configuration + PA3 ------> ADC_IN3 + PA6 ------> ADC_IN6 + */ #ifdef USE_ADC_INPUT - GPIO_InitStruct.Pin = LL_GPIO_PIN_2; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + GPIO_InitStruct.Pin = LL_GPIO_PIN_2; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); #endif - GPIO_InitStruct.Pin = CURRENT_ADC_PIN; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + GPIO_InitStruct.Pin = CURRENT_ADC_PIN; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - GPIO_InitStruct.Pin = VOLTAGE_ADC_PIN; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + GPIO_InitStruct.Pin = VOLTAGE_ADC_PIN; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_1, - LL_DMA_DIRECTION_PERIPH_TO_MEMORY); + LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_1, + LL_DMA_DIRECTION_PERIPH_TO_MEMORY); - LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PRIORITY_LOW); + LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PRIORITY_LOW); - LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MODE_CIRCULAR); + LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MODE_CIRCULAR); - LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PERIPH_NOINCREMENT); + LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PERIPH_NOINCREMENT); - LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MEMORY_INCREMENT); + LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MEMORY_INCREMENT); - LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PDATAALIGN_HALFWORD); + LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PDATAALIGN_HALFWORD); - LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MDATAALIGN_HALFWORD); + LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MDATAALIGN_HALFWORD); #ifdef USE_ADC_INPUT - LL_ADC_REG_SetSequencerChAdd(ADC1, LL_ADC_CHANNEL_2); + LL_ADC_REG_SetSequencerChAdd(ADC1, LL_ADC_CHANNEL_2); #endif - LL_ADC_REG_SetSequencerChAdd(ADC1, VOLTAGE_ADC_CHANNEL); - /** Configure Regular Channel - */ - LL_ADC_REG_SetSequencerChAdd(ADC1, CURRENT_ADC_CHANNEL); - /** Configure Regular Channel - */ - LL_ADC_REG_SetSequencerChAdd(ADC1, LL_ADC_CHANNEL_TEMPSENSOR); - /** Configure Internal Channel - */ - LL_ADC_SetCommonPathInternalCh(__LL_ADC_COMMON_INSTANCE(ADC1), - LL_ADC_PATH_INTERNAL_TEMPSENSOR); - /** Configure the global features of the ADC (Clock, Resolution, Data - * Alignment and number of conversion) - */ - ADC_InitStruct.Clock = LL_ADC_CLOCK_SYNC_PCLK_DIV4; - ADC_InitStruct.Resolution = LL_ADC_RESOLUTION_12B; - ADC_InitStruct.DataAlignment = LL_ADC_DATA_ALIGN_RIGHT; - ADC_InitStruct.LowPowerMode = LL_ADC_LP_MODE_NONE; - LL_ADC_Init(ADC1, &ADC_InitStruct); - ADC_REG_InitStruct.TriggerSource = LL_ADC_REG_TRIG_SOFTWARE; - ADC_REG_InitStruct.SequencerDiscont = LL_ADC_REG_SEQ_DISCONT_DISABLE; - ADC_REG_InitStruct.ContinuousMode = LL_ADC_REG_CONV_SINGLE; - ADC_REG_InitStruct.DMATransfer = LL_ADC_REG_DMA_TRANSFER_LIMITED; - ADC_REG_InitStruct.Overrun = LL_ADC_REG_OVR_DATA_PRESERVED; - LL_ADC_REG_Init(ADC1, &ADC_REG_InitStruct); - // LL_ADC_REG_SetTriggerEdge(ADC1, LL_ADC_REG_TRIG_EXT_FALLING); - LL_ADC_REG_SetSequencerScanDirection(ADC1, LL_ADC_REG_SEQ_SCAN_DIR_FORWARD); - LL_ADC_SetSamplingTimeCommonChannels(ADC1, LL_ADC_SAMPLINGTIME_71CYCLES_5); - LL_ADC_DisableIT_EOC(ADC1); - LL_ADC_DisableIT_EOS(ADC1); + LL_ADC_REG_SetSequencerChAdd(ADC1, VOLTAGE_ADC_CHANNEL); + /** Configure Regular Channel + */ + LL_ADC_REG_SetSequencerChAdd(ADC1, CURRENT_ADC_CHANNEL); + /** Configure Regular Channel + */ + LL_ADC_REG_SetSequencerChAdd(ADC1, LL_ADC_CHANNEL_TEMPSENSOR); + /** Configure Internal Channel + */ + LL_ADC_SetCommonPathInternalCh(__LL_ADC_COMMON_INSTANCE(ADC1), + LL_ADC_PATH_INTERNAL_TEMPSENSOR); + /** Configure the global features of the ADC (Clock, Resolution, Data + * Alignment and number of conversion) + */ + ADC_InitStruct.Clock = LL_ADC_CLOCK_SYNC_PCLK_DIV4; + ADC_InitStruct.Resolution = LL_ADC_RESOLUTION_12B; + ADC_InitStruct.DataAlignment = LL_ADC_DATA_ALIGN_RIGHT; + ADC_InitStruct.LowPowerMode = LL_ADC_LP_MODE_NONE; + LL_ADC_Init(ADC1, &ADC_InitStruct); + ADC_REG_InitStruct.TriggerSource = LL_ADC_REG_TRIG_SOFTWARE; + ADC_REG_InitStruct.SequencerDiscont = LL_ADC_REG_SEQ_DISCONT_DISABLE; + ADC_REG_InitStruct.ContinuousMode = LL_ADC_REG_CONV_SINGLE; + ADC_REG_InitStruct.DMATransfer = LL_ADC_REG_DMA_TRANSFER_LIMITED; + ADC_REG_InitStruct.Overrun = LL_ADC_REG_OVR_DATA_PRESERVED; + LL_ADC_REG_Init(ADC1, &ADC_REG_InitStruct); + // LL_ADC_REG_SetTriggerEdge(ADC1, LL_ADC_REG_TRIG_EXT_FALLING); + LL_ADC_REG_SetSequencerScanDirection(ADC1, LL_ADC_REG_SEQ_SCAN_DIR_FORWARD); + LL_ADC_SetSamplingTimeCommonChannels(ADC1, LL_ADC_SAMPLINGTIME_71CYCLES_5); + LL_ADC_DisableIT_EOC(ADC1); + LL_ADC_DisableIT_EOS(ADC1); } diff --git a/Mcu/f051/Src/IO.c b/Mcu/f051/Src/IO.c index b60af71c..10f79a05 100644 --- a/Mcu/f051/Src/IO.c +++ b/Mcu/f051/Src/IO.c @@ -20,99 +20,105 @@ uint8_t buffer_padding = 0; void receiveDshotDma() { - out_put = 0; + out_put = 0; #ifdef USE_TIMER_3_CHANNEL_1 - RCC->APB1RSTR |= LL_APB1_GRP1_PERIPH_TIM3; - RCC->APB1RSTR &= ~LL_APB1_GRP1_PERIPH_TIM3; + RCC->APB1RSTR |= LL_APB1_GRP1_PERIPH_TIM3; + RCC->APB1RSTR &= ~LL_APB1_GRP1_PERIPH_TIM3; #endif #ifdef USE_TIMER_15_CHANNEL_1 - RCC->APB2RSTR |= LL_APB1_GRP2_PERIPH_TIM15; - RCC->APB2RSTR &= ~LL_APB1_GRP2_PERIPH_TIM15; + RCC->APB2RSTR |= LL_APB1_GRP2_PERIPH_TIM15; + RCC->APB2RSTR &= ~LL_APB1_GRP2_PERIPH_TIM15; #endif - IC_TIMER_REGISTER->CCMR1 = 0x41; - IC_TIMER_REGISTER->CCER = 0xa; - IC_TIMER_REGISTER->PSC = ic_timer_prescaler; - IC_TIMER_REGISTER->ARR = 0xFFFF; - IC_TIMER_REGISTER->EGR |= TIM_EGR_UG; + IC_TIMER_REGISTER->CCMR1 = 0x41; + IC_TIMER_REGISTER->CCER = 0xa; + IC_TIMER_REGISTER->PSC = ic_timer_prescaler; + IC_TIMER_REGISTER->ARR = 0xFFFF; + IC_TIMER_REGISTER->EGR |= TIM_EGR_UG; - IC_TIMER_REGISTER->CNT = 0; + IC_TIMER_REGISTER->CNT = 0; #ifdef USE_TIMER_3_CHANNEL_1 - DMA1_Channel4->CMAR = (uint32_t)&dma_buffer; - DMA1_Channel4->CPAR = (uint32_t)&IC_TIMER_REGISTER->CCR1; - DMA1_Channel4->CNDTR = buffersize; - DMA1_Channel4->CCR = 0x98b; + DMA1_Channel4->CMAR = (uint32_t)&dma_buffer; + DMA1_Channel4->CPAR = (uint32_t)&IC_TIMER_REGISTER->CCR1; + DMA1_Channel4->CNDTR = buffersize; + DMA1_Channel4->CCR = 0x98b; #endif #ifdef USE_TIMER_15_CHANNEL_1 - DMA1_Channel5->CMAR = (uint32_t)&dma_buffer; - DMA1_Channel5->CPAR = (uint32_t)&IC_TIMER_REGISTER->CCR1; - DMA1_Channel5->CNDTR = buffersize; - DMA1_Channel5->CCR = 0x98b; + DMA1_Channel5->CMAR = (uint32_t)&dma_buffer; + DMA1_Channel5->CPAR = (uint32_t)&IC_TIMER_REGISTER->CCR1; + DMA1_Channel5->CNDTR = buffersize; + DMA1_Channel5->CCR = 0x98b; #endif - IC_TIMER_REGISTER->DIER |= TIM_DIER_CC1DE; - IC_TIMER_REGISTER->CCER |= IC_TIMER_CHANNEL; - IC_TIMER_REGISTER->CR1 |= TIM_CR1_CEN; + IC_TIMER_REGISTER->DIER |= TIM_DIER_CC1DE; + IC_TIMER_REGISTER->CCER |= IC_TIMER_CHANNEL; + IC_TIMER_REGISTER->CR1 |= TIM_CR1_CEN; } void sendDshotDma() { - out_put = 1; + out_put = 1; #ifdef USE_TIMER_3_CHANNEL_1 - // // de-init timer 2 - RCC->APB1RSTR |= LL_APB1_GRP1_PERIPH_TIM3; - RCC->APB1RSTR &= ~LL_APB1_GRP1_PERIPH_TIM3; + // // de-init timer 2 + RCC->APB1RSTR |= LL_APB1_GRP1_PERIPH_TIM3; + RCC->APB1RSTR &= ~LL_APB1_GRP1_PERIPH_TIM3; #endif #ifdef USE_TIMER_15_CHANNEL_1 - RCC->APB2RSTR |= LL_APB1_GRP2_PERIPH_TIM15; - RCC->APB2RSTR &= ~LL_APB1_GRP2_PERIPH_TIM15; + RCC->APB2RSTR |= LL_APB1_GRP2_PERIPH_TIM15; + RCC->APB2RSTR &= ~LL_APB1_GRP2_PERIPH_TIM15; #endif - IC_TIMER_REGISTER->CCMR1 = 0x60; - IC_TIMER_REGISTER->CCER = 0x3; - IC_TIMER_REGISTER->PSC = output_timer_prescaler; - IC_TIMER_REGISTER->ARR = 61; + IC_TIMER_REGISTER->CCMR1 = 0x60; + IC_TIMER_REGISTER->CCER = 0x3; + IC_TIMER_REGISTER->PSC = output_timer_prescaler; + IC_TIMER_REGISTER->ARR = 61; - IC_TIMER_REGISTER->EGR |= TIM_EGR_UG; + IC_TIMER_REGISTER->EGR |= TIM_EGR_UG; #ifdef USE_TIMER_3_CHANNEL_1 - DMA1_Channel4->CMAR = (uint32_t)&gcr; - DMA1_Channel4->CPAR = (uint32_t)&IC_TIMER_REGISTER->CCR1; - DMA1_Channel4->CNDTR = 23 + buffer_padding; - DMA1_Channel4->CCR = 0x99b; + DMA1_Channel4->CMAR = (uint32_t)&gcr; + DMA1_Channel4->CPAR = (uint32_t)&IC_TIMER_REGISTER->CCR1; + DMA1_Channel4->CNDTR = 23 + buffer_padding; + DMA1_Channel4->CCR = 0x99b; #endif #ifdef USE_TIMER_15_CHANNEL_1 - // LL_DMA_ConfigAddresses(DMA1, INPUT_DMA_CHANNEL, - //(uint32_t)&gcr, (uint32_t)&IC_TIMER_REGISTER->CCR1, - // LL_DMA_GetDataTransferDirection(DMA1, - // INPUT_DMA_CHANNEL)); - DMA1_Channel5->CMAR = (uint32_t)&gcr; - DMA1_Channel5->CPAR = (uint32_t)&IC_TIMER_REGISTER->CCR1; - DMA1_Channel5->CNDTR = 23 + buffer_padding; - DMA1_Channel5->CCR = 0x99b; + // LL_DMA_ConfigAddresses(DMA1, INPUT_DMA_CHANNEL, + //(uint32_t)&gcr, (uint32_t)&IC_TIMER_REGISTER->CCR1, + // LL_DMA_GetDataTransferDirection(DMA1, + // INPUT_DMA_CHANNEL)); + DMA1_Channel5->CMAR = (uint32_t)&gcr; + DMA1_Channel5->CPAR = (uint32_t)&IC_TIMER_REGISTER->CCR1; + DMA1_Channel5->CNDTR = 23 + buffer_padding; + DMA1_Channel5->CCR = 0x99b; #endif - IC_TIMER_REGISTER->DIER |= TIM_DIER_CC1DE; - IC_TIMER_REGISTER->CCER |= IC_TIMER_CHANNEL; - IC_TIMER_REGISTER->BDTR |= TIM_BDTR_MOE; - IC_TIMER_REGISTER->CR1 |= TIM_CR1_CEN; + IC_TIMER_REGISTER->DIER |= TIM_DIER_CC1DE; + IC_TIMER_REGISTER->CCER |= IC_TIMER_CHANNEL; + IC_TIMER_REGISTER->BDTR |= TIM_BDTR_MOE; + IC_TIMER_REGISTER->CR1 |= TIM_CR1_CEN; } -uint8_t getInputPinState() { return (INPUT_PIN_PORT->IDR & INPUT_PIN); } +uint8_t getInputPinState() +{ + return (INPUT_PIN_PORT->IDR & INPUT_PIN); +} void setInputPolarityRising() { - LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, - LL_TIM_IC_POLARITY_RISING); + LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, + LL_TIM_IC_POLARITY_RISING); } void setInputPullDown() { - LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_DOWN); + LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_DOWN); } void setInputPullUp() { - LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_UP); + LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_UP); } -void enableHalfTransferInt() { LL_DMA_EnableIT_HT(DMA1, INPUT_DMA_CHANNEL); } +void enableHalfTransferInt() +{ + LL_DMA_EnableIT_HT(DMA1, INPUT_DMA_CHANNEL); +} void setInputPullNone() { - LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_NO); + LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_NO); } diff --git a/Mcu/f051/Src/comparator.c b/Mcu/f051/Src/comparator.c index 32850e2b..db106819 100644 --- a/Mcu/f051/Src/comparator.c +++ b/Mcu/f051/Src/comparator.c @@ -11,42 +11,48 @@ COMP_TypeDef* active_COMP = COMP1; -uint8_t getCompOutputLevel() { return LL_COMP_ReadOutputLevel(active_COMP); } +uint8_t getCompOutputLevel() +{ + return LL_COMP_ReadOutputLevel(active_COMP); +} void maskPhaseInterrupts() { - EXTI->IMR &= ~(1 << 21); - LL_EXTI_ClearFlag_0_31(EXTI_LINE); + EXTI->IMR &= ~(1 << 21); + LL_EXTI_ClearFlag_0_31(EXTI_LINE); } -void enableCompInterrupts() { EXTI->IMR |= (1 << 21); } +void enableCompInterrupts() +{ + EXTI->IMR |= (1 << 21); +} void changeCompInput() { - // TIM3->CNT = 0; - // HAL_COMP_Stop_IT(&hcomp1); // done in comparator interrupt - // routine - - if (step == 1 || step == 4) { // c floating - COMP->CSR = PHASE_C_COMP; - } - if (step == 2 || step == 5) { // a floating - COMP->CSR = PHASE_A_COMP; - } - if (step == 3 || step == 6) { // b floating - COMP->CSR = PHASE_B_COMP; - } - if (rising) { - EXTI->RTSR = 0x0; - EXTI->FTSR = 0x200000; - - // hcomp1.Init.TriggerMode = COMP_TRIGGERMODE_IT_FALLING; // - // polarity of - // comp output reversed - } else { - // falling bemf - EXTI->FTSR = 0x0; - EXTI->RTSR = 0x200000; - // hcomp1.Init.TriggerMode = COMP_TRIGGERMODE_IT_RISING; - } + // TIM3->CNT = 0; + // HAL_COMP_Stop_IT(&hcomp1); // done in comparator interrupt + // routine + + if (step == 1 || step == 4) { // c floating + COMP->CSR = PHASE_C_COMP; + } + if (step == 2 || step == 5) { // a floating + COMP->CSR = PHASE_A_COMP; + } + if (step == 3 || step == 6) { // b floating + COMP->CSR = PHASE_B_COMP; + } + if (rising) { + EXTI->RTSR = 0x0; + EXTI->FTSR = 0x200000; + + // hcomp1.Init.TriggerMode = COMP_TRIGGERMODE_IT_FALLING; // + // polarity of + // comp output reversed + } else { + // falling bemf + EXTI->FTSR = 0x0; + EXTI->RTSR = 0x200000; + // hcomp1.Init.TriggerMode = COMP_TRIGGERMODE_IT_RISING; + } } diff --git a/Mcu/f051/Src/eeprom.c b/Mcu/f051/Src/eeprom.c index 0c8ed3ee..7d883dd2 100644 --- a/Mcu/f051/Src/eeprom.c +++ b/Mcu/f051/Src/eeprom.c @@ -19,61 +19,61 @@ uint32_t FLASH_FKEY2 = 0xCDEF89AB; void save_flash_nolib(uint8_t* data, int length, uint32_t add) { - uint16_t data_to_FLASH[length / 2]; - memset(data_to_FLASH, 0, length / 2); - for (int i = 0; i < length / 2; i++) { - data_to_FLASH[i] = data[i * 2 + 1] << 8 | data[i * 2]; // make 16 bit - } - volatile uint32_t data_length = length / 2; + uint16_t data_to_FLASH[length / 2]; + memset(data_to_FLASH, 0, length / 2); + for (int i = 0; i < length / 2; i++) { + data_to_FLASH[i] = data[i * 2 + 1] << 8 | data[i * 2]; // make 16 bit + } + volatile uint32_t data_length = length / 2; + + // unlock flash - // unlock flash + while ((FLASH->SR & FLASH_SR_BSY) != 0) { + /* add time-out*/ + } + if ((FLASH->CR & FLASH_CR_LOCK) != 0) { + FLASH->KEYR = FLASH_FKEY1; + FLASH->KEYR = FLASH_FKEY2; + } + // erase page if address even divisable by 1024 + if ((add % 1024) == 0) { + FLASH->CR |= FLASH_CR_PER; + FLASH->AR = add; + FLASH->CR |= FLASH_CR_STRT; while ((FLASH->SR & FLASH_SR_BSY) != 0) { - /* add time-out*/ + /* add time-out */ } - if ((FLASH->CR & FLASH_CR_LOCK) != 0) { - FLASH->KEYR = FLASH_FKEY1; - FLASH->KEYR = FLASH_FKEY2; + if ((FLASH->SR & FLASH_SR_EOP) != 0) { + FLASH->SR = FLASH_SR_EOP; + } else { + /* error */ } + FLASH->CR &= ~FLASH_CR_PER; + } - // erase page if address even divisable by 1024 - if ((add % 1024) == 0) { - FLASH->CR |= FLASH_CR_PER; - FLASH->AR = add; - FLASH->CR |= FLASH_CR_STRT; - while ((FLASH->SR & FLASH_SR_BSY) != 0) { - /* add time-out */ - } - if ((FLASH->SR & FLASH_SR_EOP) != 0) { - FLASH->SR = FLASH_SR_EOP; - } else { - /* error */ - } - FLASH->CR &= ~FLASH_CR_PER; + volatile uint32_t write_cnt = 0, index = 0; + while (index < data_length) { + FLASH->CR |= FLASH_CR_PG; /* (1) */ + *(__IO uint16_t*)(add + write_cnt) = data_to_FLASH[index]; + while ((FLASH->SR & FLASH_SR_BSY) != 0) { /* add time-out */ } - - volatile uint32_t write_cnt = 0, index = 0; - while (index < data_length) { - FLASH->CR |= FLASH_CR_PG; /* (1) */ - *(__IO uint16_t*)(add + write_cnt) = data_to_FLASH[index]; - while ((FLASH->SR & FLASH_SR_BSY) != 0) { /* add time-out */ - } - if ((FLASH->SR & FLASH_SR_EOP) != 0) { - FLASH->SR = FLASH_SR_EOP; - } else { - /* error */ - } - FLASH->CR &= ~FLASH_CR_PG; - write_cnt += 2; - index++; + if ((FLASH->SR & FLASH_SR_EOP) != 0) { + FLASH->SR = FLASH_SR_EOP; + } else { + /* error */ } - SET_BIT(FLASH->CR, FLASH_CR_LOCK); + FLASH->CR &= ~FLASH_CR_PG; + write_cnt += 2; + index++; + } + SET_BIT(FLASH->CR, FLASH_CR_LOCK); } void read_flash_bin(uint8_t* data, uint32_t add, int out_buff_len) { - // volatile uint32_t read_data; - for (int i = 0; i < out_buff_len; i++) { - data[i] = *(uint8_t*)(add + i); - } + // volatile uint32_t read_data; + for (int i = 0; i < out_buff_len; i++) { + data[i] = *(uint8_t*)(add + i); + } } diff --git a/Mcu/f051/Src/peripherals.c b/Mcu/f051/Src/peripherals.c index ee96a62d..c1f40463 100644 --- a/Mcu/f051/Src/peripherals.c +++ b/Mcu/f051/Src/peripherals.c @@ -15,210 +15,210 @@ void initCorePeripherals(void) { - SystemClock_Config(); - LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_SYSCFG); - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_PWR); - FLASH->ACR |= FLASH_ACR_PRFTBE; //// prefetch buffer enable - MX_GPIO_Init(); - MX_DMA_Init(); - MX_TIM1_Init(); - MX_TIM2_Init(); - MX_COMP1_Init(); - MX_TIM14_Init(); - MX_TIM6_Init(); - MX_TIM17_Init(); - UN_TIM_Init(); + SystemClock_Config(); + LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_SYSCFG); + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_PWR); + FLASH->ACR |= FLASH_ACR_PRFTBE; //// prefetch buffer enable + MX_GPIO_Init(); + MX_DMA_Init(); + MX_TIM1_Init(); + MX_TIM2_Init(); + MX_COMP1_Init(); + MX_TIM14_Init(); + MX_TIM6_Init(); + MX_TIM17_Init(); + UN_TIM_Init(); #ifdef USE_SERIAL_TELEMETRY - telem_UART_Init(); + telem_UART_Init(); #endif } void initAfterJump(void) { - volatile uint32_t* VectorTable = (volatile uint32_t*)0x20000000; - uint32_t vector_index = 0; - for (vector_index = 0; vector_index < 48; vector_index++) { - VectorTable[vector_index] = *(__IO uint32_t*)(APPLICATION_ADDRESS + (vector_index << 2)); // no VTOR on cortex-MO so - // need to copy vector table + volatile uint32_t* VectorTable = (volatile uint32_t*)0x20000000; + uint32_t vector_index = 0; + for (vector_index = 0; vector_index < 48; vector_index++) { + VectorTable[vector_index] = *(__IO uint32_t*)(APPLICATION_ADDRESS + (vector_index << 2)); // no VTOR on cortex-MO so + // need to copy vector table + } + /* Enable the SYSCFG peripheral clock*/ + do { + volatile uint32_t tmpreg; + ((((RCC_TypeDef*)((((uint32_t)0x40000000U) + 0x00020000) + 0x00001000)) + ->APB2ENR) + |= ((0x1U << (0U)))); + /* Delay after an RCC peripheral clock enabling */ + tmpreg = ((((RCC_TypeDef*)((((uint32_t)0x40000000U) + 0x00020000) + 0x00001000)) + ->APB2ENR) + & ((0x1U << (0U)))); + ((void)(tmpreg)); + } while (0U); + // /* Remap SRAM at 0x00000000 */ + do { + ((SYSCFG_TypeDef*)(((uint32_t)0x40000000U) + 0x00010000))->CFGR1 &= ~((0x3U << (0U))); + ((SYSCFG_TypeDef*)(((uint32_t)0x40000000U) + 0x00010000))->CFGR1 |= ((0x1U << (0U)) | (0x2U << (0U))); + } while (0); + + if (SysTick_Config(SystemCoreClock / 1000)) { + /* Capture error */ + while (1) { } - /* Enable the SYSCFG peripheral clock*/ - do { - volatile uint32_t tmpreg; - ((((RCC_TypeDef*)((((uint32_t)0x40000000U) + 0x00020000) + 0x00001000)) - ->APB2ENR) - |= ((0x1U << (0U)))); - /* Delay after an RCC peripheral clock enabling */ - tmpreg = ((((RCC_TypeDef*)((((uint32_t)0x40000000U) + 0x00020000) + 0x00001000)) - ->APB2ENR) - & ((0x1U << (0U)))); - ((void)(tmpreg)); - } while (0U); - // /* Remap SRAM at 0x00000000 */ - do { - ((SYSCFG_TypeDef*)(((uint32_t)0x40000000U) + 0x00010000))->CFGR1 &= ~((0x3U << (0U))); - ((SYSCFG_TypeDef*)(((uint32_t)0x40000000U) + 0x00010000))->CFGR1 |= ((0x1U << (0U)) | (0x2U << (0U))); - } while (0); - - if (SysTick_Config(SystemCoreClock / 1000)) { - /* Capture error */ - while (1) { - } - } - __enable_irq(); + } + __enable_irq(); } void SystemClock_Config(void) { - LL_FLASH_SetLatency(LL_FLASH_LATENCY_1); - - if (LL_FLASH_GetLatency() != LL_FLASH_LATENCY_1) { - // Error_Handler(); - } - LL_RCC_HSI_Enable(); - - /* Wait till HSI is ready */ - while (LL_RCC_HSI_IsReady() != 1) { - } - LL_RCC_HSI_SetCalibTrimming(16); - LL_RCC_HSI14_Enable(); - - /* Wait till HSI14 is ready */ - while (LL_RCC_HSI14_IsReady() != 1) { - } - LL_RCC_HSI14_SetCalibTrimming(16); - LL_RCC_LSI_Enable(); - - /* Wait till LSI is ready */ - while (LL_RCC_LSI_IsReady() != 1) { - } - LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSI_DIV_2, LL_RCC_PLL_MUL_12); - LL_RCC_PLL_Enable(); - - /* Wait till PLL is ready */ - while (LL_RCC_PLL_IsReady() != 1) { - } - LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1); - LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1); - LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL); - - /* Wait till System clock is ready */ - while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) { - } - LL_Init1msTick(48000000); - LL_SetSystemCoreClock(48000000); - LL_RCC_HSI14_EnableADCControl(); - LL_RCC_SetUSARTClockSource(LL_RCC_USART1_CLKSOURCE_PCLK1); + LL_FLASH_SetLatency(LL_FLASH_LATENCY_1); + + if (LL_FLASH_GetLatency() != LL_FLASH_LATENCY_1) { + // Error_Handler(); + } + LL_RCC_HSI_Enable(); + + /* Wait till HSI is ready */ + while (LL_RCC_HSI_IsReady() != 1) { + } + LL_RCC_HSI_SetCalibTrimming(16); + LL_RCC_HSI14_Enable(); + + /* Wait till HSI14 is ready */ + while (LL_RCC_HSI14_IsReady() != 1) { + } + LL_RCC_HSI14_SetCalibTrimming(16); + LL_RCC_LSI_Enable(); + + /* Wait till LSI is ready */ + while (LL_RCC_LSI_IsReady() != 1) { + } + LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSI_DIV_2, LL_RCC_PLL_MUL_12); + LL_RCC_PLL_Enable(); + + /* Wait till PLL is ready */ + while (LL_RCC_PLL_IsReady() != 1) { + } + LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1); + LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1); + LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL); + + /* Wait till System clock is ready */ + while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) { + } + LL_Init1msTick(48000000); + LL_SetSystemCoreClock(48000000); + LL_RCC_HSI14_EnableADCControl(); + LL_RCC_SetUSARTClockSource(LL_RCC_USART1_CLKSOURCE_PCLK1); } void MX_COMP1_Init(void) { - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); - /**COMP1 GPIO Configuration - PA1 ------> COMP1_INP - PA5 ------> COMP1_INM - */ - GPIO_InitStruct.Pin = LL_GPIO_PIN_1; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_5; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - - NVIC_SetPriority(ADC1_COMP_IRQn, 0); - NVIC_EnableIRQ(ADC1_COMP_IRQn); + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); + /**COMP1 GPIO Configuration + PA1 ------> COMP1_INP + PA5 ------> COMP1_INM + */ + GPIO_InitStruct.Pin = LL_GPIO_PIN_1; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_5; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + NVIC_SetPriority(ADC1_COMP_IRQn, 0); + NVIC_EnableIRQ(ADC1_COMP_IRQn); } void MX_IWDG_Init(void) { - IWDG->KR = 0x0000CCCCU; - IWDG->KR = 0x00005555U; - IWDG->PR = LL_IWDG_PRESCALER_16; - IWDG->RLR = 4000; - LL_IWDG_ReloadCounter(IWDG); + IWDG->KR = 0x0000CCCCU; + IWDG->KR = 0x00005555U; + IWDG->PR = LL_IWDG_PRESCALER_16; + IWDG->RLR = 4000; + LL_IWDG_ReloadCounter(IWDG); } void MX_TIM1_Init(void) { - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - LL_TIM_OC_InitTypeDef TIM_OC_InitStruct = { 0 }; - LL_TIM_BDTR_InitTypeDef TIM_BDTRInitStruct = { 0 }; - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_TIM1); - - TIM_InitStruct.Prescaler = 0; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = TIM1_AUTORELOAD; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - TIM_InitStruct.RepetitionCounter = 0; - LL_TIM_Init(TIM1, &TIM_InitStruct); - LL_TIM_EnableARRPreload(TIM1); - LL_TIM_SetClockSource(TIM1, LL_TIM_CLOCKSOURCE_INTERNAL); - LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH1); + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + LL_TIM_OC_InitTypeDef TIM_OC_InitStruct = { 0 }; + LL_TIM_BDTR_InitTypeDef TIM_BDTRInitStruct = { 0 }; + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_TIM1); + + TIM_InitStruct.Prescaler = 0; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = TIM1_AUTORELOAD; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + TIM_InitStruct.RepetitionCounter = 0; + LL_TIM_Init(TIM1, &TIM_InitStruct); + LL_TIM_EnableARRPreload(TIM1); + LL_TIM_SetClockSource(TIM1, LL_TIM_CLOCKSOURCE_INTERNAL); + LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH1); #ifdef USE_SWAPPED_OUPUT - TIM_OC_InitStruct.OCMode = LL_TIM_OCMODE_PWM2; + TIM_OC_InitStruct.OCMode = LL_TIM_OCMODE_PWM2; #else - TIM_OC_InitStruct.OCMode = LL_TIM_OCMODE_PWM1; + TIM_OC_InitStruct.OCMode = LL_TIM_OCMODE_PWM1; #endif - TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; - TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; - TIM_OC_InitStruct.CompareValue = 0; + TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; + TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; + TIM_OC_InitStruct.CompareValue = 0; #ifdef USE_INVERTED_HIGH - TIM_OC_InitStruct.OCPolarity = LL_TIM_OCPOLARITY_LOW; - TIM_OC_InitStruct.OCIdleState = LL_TIM_OCIDLESTATE_HIGH; + TIM_OC_InitStruct.OCPolarity = LL_TIM_OCPOLARITY_LOW; + TIM_OC_InitStruct.OCIdleState = LL_TIM_OCIDLESTATE_HIGH; #else - TIM_OC_InitStruct.OCPolarity = LL_TIM_OCPOLARITY_HIGH; - TIM_OC_InitStruct.OCIdleState = LL_TIM_OCIDLESTATE_LOW; + TIM_OC_InitStruct.OCPolarity = LL_TIM_OCPOLARITY_HIGH; + TIM_OC_InitStruct.OCIdleState = LL_TIM_OCIDLESTATE_LOW; #endif #ifdef USE_INVERTED_LOW - TIM_OC_InitStruct.OCNPolarity = LL_TIM_OCPOLARITY_LOW; - TIM_OC_InitStruct.OCNIdleState = LL_TIM_OCIDLESTATE_HIGH; + TIM_OC_InitStruct.OCNPolarity = LL_TIM_OCPOLARITY_LOW; + TIM_OC_InitStruct.OCNIdleState = LL_TIM_OCIDLESTATE_HIGH; #else - TIM_OC_InitStruct.OCNPolarity = LL_TIM_OCPOLARITY_HIGH; - TIM_OC_InitStruct.OCNIdleState = LL_TIM_OCIDLESTATE_LOW; + TIM_OC_InitStruct.OCNPolarity = LL_TIM_OCPOLARITY_HIGH; + TIM_OC_InitStruct.OCNIdleState = LL_TIM_OCIDLESTATE_LOW; #endif - LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH1, &TIM_OC_InitStruct); - LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH1); - LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH2); - TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; - TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; - LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH2, &TIM_OC_InitStruct); - LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH2); - LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH3); - TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; - TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; - LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH3, &TIM_OC_InitStruct); - LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH3); - LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH4); - TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; - TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; - LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH4, &TIM_OC_InitStruct); - LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH4); - LL_TIM_SetTriggerOutput(TIM1, LL_TIM_TRGO_RESET); - LL_TIM_DisableMasterSlaveMode(TIM1); - TIM_BDTRInitStruct.OSSRState = LL_TIM_OSSR_DISABLE; - TIM_BDTRInitStruct.OSSIState = LL_TIM_OSSI_DISABLE; - TIM_BDTRInitStruct.LockLevel = LL_TIM_LOCKLEVEL_OFF; - TIM_BDTRInitStruct.DeadTime = DEAD_TIME; - TIM_BDTRInitStruct.BreakState = LL_TIM_BREAK_DISABLE; - TIM_BDTRInitStruct.BreakPolarity = LL_TIM_BREAK_POLARITY_HIGH; - TIM_BDTRInitStruct.AutomaticOutput = LL_TIM_AUTOMATICOUTPUT_DISABLE; - LL_TIM_BDTR_Init(TIM1, &TIM_BDTRInitStruct); - - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); -/**TIM1 GPIO Configuration -PA7 ------> TIM1_CH1N -PB0 ------> TIM1_CH2N -PB1 ------> TIM1_CH3N -PA8 ------> TIM1_CH1 -PA9 ------> TIM1_CH2 -PA10 ------> TIM1_CH3 -*/ + LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH1, &TIM_OC_InitStruct); + LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH1); + LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH2); + TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; + TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; + LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH2, &TIM_OC_InitStruct); + LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH2); + LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH3); + TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; + TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; + LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH3, &TIM_OC_InitStruct); + LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH3); + LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH4); + TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; + TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; + LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH4, &TIM_OC_InitStruct); + LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH4); + LL_TIM_SetTriggerOutput(TIM1, LL_TIM_TRGO_RESET); + LL_TIM_DisableMasterSlaveMode(TIM1); + TIM_BDTRInitStruct.OSSRState = LL_TIM_OSSR_DISABLE; + TIM_BDTRInitStruct.OSSIState = LL_TIM_OSSI_DISABLE; + TIM_BDTRInitStruct.LockLevel = LL_TIM_LOCKLEVEL_OFF; + TIM_BDTRInitStruct.DeadTime = DEAD_TIME; + TIM_BDTRInitStruct.BreakState = LL_TIM_BREAK_DISABLE; + TIM_BDTRInitStruct.BreakPolarity = LL_TIM_BREAK_POLARITY_HIGH; + TIM_BDTRInitStruct.AutomaticOutput = LL_TIM_AUTOMATICOUTPUT_DISABLE; + LL_TIM_BDTR_Init(TIM1, &TIM_BDTRInitStruct); + + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); + /**TIM1 GPIO Configuration + PA7 ------> TIM1_CH1N + PB0 ------> TIM1_CH2N + PB1 ------> TIM1_CH3N + PA8 ------> TIM1_CH1 + PA9 ------> TIM1_CH2 + PA10 ------> TIM1_CH3 + */ #ifdef USE_OPEN_DRAIN_LOW #pragma message("using open drain low side") #define LOW_OUTPUT_TYPE LL_GPIO_OUTPUT_OPENDRAIN @@ -231,253 +231,253 @@ PA10 ------> TIM1_CH3 #else #define HIGH_OUTPUT_TYPE LL_GPIO_OUTPUT_PUSHPULL #endif - GPIO_InitStruct.Pin = PHASE_A_GPIO_LOW; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LOW_OUTPUT_TYPE; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_2; - LL_GPIO_Init(PHASE_A_GPIO_PORT_LOW, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = PHASE_B_GPIO_LOW; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LOW_OUTPUT_TYPE; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_2; - LL_GPIO_Init(PHASE_B_GPIO_PORT_LOW, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = PHASE_C_GPIO_LOW; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LOW_OUTPUT_TYPE; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_2; - LL_GPIO_Init(PHASE_C_GPIO_PORT_LOW, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = PHASE_A_GPIO_HIGH; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = HIGH_OUTPUT_TYPE; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_2; - LL_GPIO_Init(PHASE_A_GPIO_PORT_HIGH, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = PHASE_B_GPIO_HIGH; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = HIGH_OUTPUT_TYPE; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_2; - LL_GPIO_Init(PHASE_B_GPIO_PORT_HIGH, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = PHASE_C_GPIO_HIGH; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = HIGH_OUTPUT_TYPE; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_2; - LL_GPIO_Init(PHASE_C_GPIO_PORT_HIGH, &GPIO_InitStruct); - - // NVIC_SetPriority(TIM1_BRK_UP_TRG_COM_IRQn, 2); - // NVIC_EnableIRQ(TIM1_BRK_UP_TRG_COM_IRQn); + GPIO_InitStruct.Pin = PHASE_A_GPIO_LOW; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LOW_OUTPUT_TYPE; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_2; + LL_GPIO_Init(PHASE_A_GPIO_PORT_LOW, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = PHASE_B_GPIO_LOW; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LOW_OUTPUT_TYPE; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_2; + LL_GPIO_Init(PHASE_B_GPIO_PORT_LOW, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = PHASE_C_GPIO_LOW; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LOW_OUTPUT_TYPE; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_2; + LL_GPIO_Init(PHASE_C_GPIO_PORT_LOW, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = PHASE_A_GPIO_HIGH; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = HIGH_OUTPUT_TYPE; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_2; + LL_GPIO_Init(PHASE_A_GPIO_PORT_HIGH, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = PHASE_B_GPIO_HIGH; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = HIGH_OUTPUT_TYPE; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_2; + LL_GPIO_Init(PHASE_B_GPIO_PORT_HIGH, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = PHASE_C_GPIO_HIGH; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = HIGH_OUTPUT_TYPE; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_2; + LL_GPIO_Init(PHASE_C_GPIO_PORT_HIGH, &GPIO_InitStruct); + + // NVIC_SetPriority(TIM1_BRK_UP_TRG_COM_IRQn, 2); + // NVIC_EnableIRQ(TIM1_BRK_UP_TRG_COM_IRQn); } void MX_TIM2_Init(void) { - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2); - TIM2->PSC = 23; - TIM2->ARR = 0xFFFF; + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2); + TIM2->PSC = 23; + TIM2->ARR = 0xFFFF; } void MX_TIM6_Init(void) { - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM6); + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM6); - NVIC_SetPriority(TIM6_DAC_IRQn, 3); - NVIC_EnableIRQ(TIM6_DAC_IRQn); - TIM6->PSC = 47; - TIM6->ARR = 1000000 / LOOP_FREQUENCY_HZ; + NVIC_SetPriority(TIM6_DAC_IRQn, 3); + NVIC_EnableIRQ(TIM6_DAC_IRQn); + TIM6->PSC = 47; + TIM6->ARR = 1000000 / LOOP_FREQUENCY_HZ; } void MX_TIM14_Init(void) { - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM14); - TIM14->PSC = 23; - TIM14->ARR = 4000; - NVIC_SetPriority(TIM14_IRQn, 0); - NVIC_EnableIRQ(TIM14_IRQn); - LL_TIM_EnableARRPreload(TIM14); + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM14); + TIM14->PSC = 23; + TIM14->ARR = 4000; + NVIC_SetPriority(TIM14_IRQn, 0); + NVIC_EnableIRQ(TIM14_IRQn); + LL_TIM_EnableARRPreload(TIM14); } void MX_TIM16_Init(void) { - LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_TIM16); - // NVIC_SetPriority(TIM16_IRQn, 2); - // NVIC_EnableIRQ(TIM16_IRQn); - TIM16->PSC = 0; - TIM16->ARR = 9000; - LL_TIM_DisableARRPreload(TIM16); + LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_TIM16); + // NVIC_SetPriority(TIM16_IRQn, 2); + // NVIC_EnableIRQ(TIM16_IRQn); + TIM16->PSC = 0; + TIM16->ARR = 9000; + LL_TIM_DisableARRPreload(TIM16); } void MX_TIM17_Init(void) { - LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_TIM17); - TIM17->PSC = 47; - TIM17->ARR = 0XFFFF; - LL_TIM_DisableARRPreload(TIM17); + LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_TIM17); + TIM17->PSC = 47; + TIM17->ARR = 0XFFFF; + LL_TIM_DisableARRPreload(TIM17); } void MX_DMA_Init(void) { - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_DMA1); - NVIC_SetPriority(DMA1_Channel2_3_IRQn, 1); - NVIC_EnableIRQ(DMA1_Channel2_3_IRQn); - NVIC_SetPriority(DMA1_Channel4_5_IRQn, 1); - NVIC_EnableIRQ(DMA1_Channel4_5_IRQn); + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_DMA1); + NVIC_SetPriority(DMA1_Channel2_3_IRQn, 1); + NVIC_EnableIRQ(DMA1_Channel2_3_IRQn); + NVIC_SetPriority(DMA1_Channel4_5_IRQn, 1); + NVIC_EnableIRQ(DMA1_Channel4_5_IRQn); } void MX_GPIO_Init(void) { - /* GPIO Ports Clock Enable */ - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); + /* GPIO Ports Clock Enable */ + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); - /**/ - LL_GPIO_ResetOutputPin(GPIOA, LL_GPIO_PIN_15); + /**/ + LL_GPIO_ResetOutputPin(GPIOA, LL_GPIO_PIN_15); -/**/ + /**/ #ifdef USE_ALKAS_DEBUG_LED - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - - GPIO_InitStruct.Pin = LL_GPIO_PIN_15; - GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + + GPIO_InitStruct.Pin = LL_GPIO_PIN_15; + GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); #endif } void UN_TIM_Init(void) { - // LL_TIM_InitTypeDef TIM_InitStruct = {0}; + // LL_TIM_InitTypeDef TIM_InitStruct = {0}; - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - /* Peripheral clock enable */ + /* Peripheral clock enable */ #ifdef USE_TIMER_15_CHANNEL_1 - LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_TIM15); - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); - /**TIM16 GPIO Configuration - PA6 ------> TIM16_CH1 - */ - GPIO_InitStruct.Pin = LL_GPIO_PIN_2; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_0; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_TIM15); + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); + /**TIM16 GPIO Configuration + PA6 ------> TIM16_CH1 + */ + GPIO_InitStruct.Pin = LL_GPIO_PIN_2; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_0; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); #endif #ifdef USE_TIMER_3_CHANNEL_1 - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM3); - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); - /**TIM16 GPIO Configuration - PA6 ------> TIM16_CH1 - */ - GPIO_InitStruct.Pin = INPUT_PIN; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_1; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM3); + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); + /**TIM16 GPIO Configuration + PA6 ------> TIM16_CH1 + */ + GPIO_InitStruct.Pin = INPUT_PIN; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_1; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); #endif - /* TIM16 DMA Init */ + /* TIM16 DMA Init */ - /* TIM16_CH1_UP Init */ - // LL_DMA_SetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL, - // LL_DMA_DIRECTION_PERIPH_TO_MEMORY); + /* TIM16_CH1_UP Init */ + // LL_DMA_SetDataTransferDirection(DMA1, INPUT_DMA_CHANNEL, + // LL_DMA_DIRECTION_PERIPH_TO_MEMORY); - // LL_DMA_SetChannelPriorityLevel(DMA1, INPUT_DMA_CHANNEL, - // LL_DMA_PRIORITY_LOW); + // LL_DMA_SetChannelPriorityLevel(DMA1, INPUT_DMA_CHANNEL, + // LL_DMA_PRIORITY_LOW); - // LL_DMA_SetMode(DMA1, INPUT_DMA_CHANNEL, LL_DMA_MODE_NORMAL); + // LL_DMA_SetMode(DMA1, INPUT_DMA_CHANNEL, LL_DMA_MODE_NORMAL); - // LL_DMA_SetPeriphIncMode(DMA1, INPUT_DMA_CHANNEL, - // LL_DMA_PERIPH_NOINCREMENT); + // LL_DMA_SetPeriphIncMode(DMA1, INPUT_DMA_CHANNEL, + // LL_DMA_PERIPH_NOINCREMENT); - // LL_DMA_SetMemoryIncMode(DMA1, INPUT_DMA_CHANNEL, LL_DMA_MEMORY_INCREMENT); + // LL_DMA_SetMemoryIncMode(DMA1, INPUT_DMA_CHANNEL, LL_DMA_MEMORY_INCREMENT); - // LL_DMA_SetPeriphSize(DMA1, INPUT_DMA_CHANNEL, LL_DMA_PDATAALIGN_HALFWORD); + // LL_DMA_SetPeriphSize(DMA1, INPUT_DMA_CHANNEL, LL_DMA_PDATAALIGN_HALFWORD); - // LL_DMA_SetMemorySize(DMA1, INPUT_DMA_CHANNEL, LL_DMA_MDATAALIGN_WORD); + // LL_DMA_SetMemorySize(DMA1, INPUT_DMA_CHANNEL, LL_DMA_MDATAALIGN_WORD); - /* TIM16 interrupt Init */ + /* TIM16 interrupt Init */ #ifdef USE_TIMER_15_CHANNEL_1 - NVIC_SetPriority(IC_DMA_IRQ_NAME, 1); - NVIC_EnableIRQ(IC_DMA_IRQ_NAME); + NVIC_SetPriority(IC_DMA_IRQ_NAME, 1); + NVIC_EnableIRQ(IC_DMA_IRQ_NAME); #endif #ifdef USE_TIMER_3_CHANNEL_1 - NVIC_SetPriority(IC_DMA_IRQ_NAME, 1); - NVIC_EnableIRQ(IC_DMA_IRQ_NAME); + NVIC_SetPriority(IC_DMA_IRQ_NAME, 1); + NVIC_EnableIRQ(IC_DMA_IRQ_NAME); #endif - // TIM_InitStruct.Prescaler = 0; - // TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - // TIM_InitStruct.Autoreload = 63; - // TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - // TIM_InitStruct.RepetitionCounter = 0; - // LL_TIM_Init(IC_TIMER_REGISTER, &TIM_InitStruct); - IC_TIMER_REGISTER->PSC = 0; - IC_TIMER_REGISTER->ARR = 63; - - // LL_TIM_DisableARRPreload(IC_TIMER_REGISTER); - // LL_TIM_IC_SetActiveInput(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, - // LL_TIM_ACTIVEINPUT_DIRECTTI); LL_TIM_IC_SetPrescaler(IC_TIMER_REGISTER, - // IC_TIMER_CHANNEL, LL_TIM_ICPSC_DIV1); - // LL_TIM_IC_SetFilter(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, - // LL_TIM_IC_FILTER_FDIV1); LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, - // IC_TIMER_CHANNEL, LL_TIM_IC_POLARITY_BOTHEDGE); + // TIM_InitStruct.Prescaler = 0; + // TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + // TIM_InitStruct.Autoreload = 63; + // TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + // TIM_InitStruct.RepetitionCounter = 0; + // LL_TIM_Init(IC_TIMER_REGISTER, &TIM_InitStruct); + IC_TIMER_REGISTER->PSC = 0; + IC_TIMER_REGISTER->ARR = 63; + + // LL_TIM_DisableARRPreload(IC_TIMER_REGISTER); + // LL_TIM_IC_SetActiveInput(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, + // LL_TIM_ACTIVEINPUT_DIRECTTI); LL_TIM_IC_SetPrescaler(IC_TIMER_REGISTER, + // IC_TIMER_CHANNEL, LL_TIM_ICPSC_DIV1); + // LL_TIM_IC_SetFilter(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, + // LL_TIM_IC_FILTER_FDIV1); LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, + // IC_TIMER_CHANNEL, LL_TIM_IC_POLARITY_BOTHEDGE); } #ifdef USE_RGB_LED // has 3 color led void LED_GPIO_init() { - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - - /* GPIO Ports Clock Enable */ - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); - - LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_8); - LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_5); - LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_3); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_8; - GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_5; - GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_3; - GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + + /* GPIO Ports Clock Enable */ + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); + + LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_8); + LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_5); + LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_3); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_8; + GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_5; + GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_3; + GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); } #endif @@ -485,151 +485,163 @@ void LED_GPIO_init() #ifdef USE_CUSTOM_LED void initLed() { - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - GPIO_InitStruct.Pin = LL_GPIO_PIN_3; - GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + GPIO_InitStruct.Pin = LL_GPIO_PIN_3; + GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); } #endif #ifdef USE_DRV8328_NSLEEP // Disable gate driver when disarmed void initnSleep() { - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - GPIO_InitStruct.Pin = NSLEEP_PIN; - GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(NSLEEP_PORT, &GPIO_InitStruct); - NSLEEP_PORT->BSRR = NSLEEP_PIN; + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + GPIO_InitStruct.Pin = NSLEEP_PIN; + GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(NSLEEP_PORT, &GPIO_InitStruct); + NSLEEP_PORT->BSRR = NSLEEP_PIN; } #endif #ifdef USE_DRV8328_NFAULT void initnFault() { - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - GPIO_InitStruct.Pin = NFAULT_PIN; - GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; - LL_GPIO_Init(NFAULT_PORT, &GPIO_InitStruct); + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + GPIO_InitStruct.Pin = NFAULT_PIN; + GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; + LL_GPIO_Init(NFAULT_PORT, &GPIO_InitStruct); } #endif void reloadWatchDogCounter() { - LL_IWDG_ReloadCounter(IWDG); + LL_IWDG_ReloadCounter(IWDG); } -inline void setPWMCompare1(uint16_t compareone) { TIM1->CCR1 = compareone; } -inline void setPWMCompare2(uint16_t comparetwo) { TIM1->CCR2 = comparetwo; } -inline void setPWMCompare3(uint16_t comparethree) { TIM1->CCR3 = comparethree; } +inline void setPWMCompare1(uint16_t compareone) +{ + TIM1->CCR1 = compareone; +} +inline void setPWMCompare2(uint16_t comparetwo) +{ + TIM1->CCR2 = comparetwo; +} +inline void setPWMCompare3(uint16_t comparethree) +{ + TIM1->CCR3 = comparethree; +} -inline void generatePwmTimerEvent() { LL_TIM_GenerateEvent_UPDATE(TIM1); } +inline void generatePwmTimerEvent() +{ + LL_TIM_GenerateEvent_UPDATE(TIM1); +} inline void resetInputCaptureTimer() { - IC_TIMER_REGISTER->PSC = 0; - IC_TIMER_REGISTER->CNT = 0; + IC_TIMER_REGISTER->PSC = 0; + IC_TIMER_REGISTER->CNT = 0; } void enableCorePeripherals() { - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH1); - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH2); - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH3); - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH1N); - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH2N); - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH3N); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH1); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH2); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH3); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH1N); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH2N); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH3N); #ifdef MCU_G071 - LL_TIM_CC_EnableChannel( - TIM1, LL_TIM_CHANNEL_CH5); // timer used for comparator blanking + LL_TIM_CC_EnableChannel( + TIM1, LL_TIM_CHANNEL_CH5); // timer used for comparator blanking #endif - LL_TIM_CC_EnableChannel(TIM1, - LL_TIM_CHANNEL_CH4); // timer used for timing adc read - TIM1->CCR4 = 100; // set in 10khz loop to match pwm cycle timed to end of pwm on + LL_TIM_CC_EnableChannel(TIM1, + LL_TIM_CHANNEL_CH4); // timer used for timing adc read + TIM1->CCR4 = 100; // set in 10khz loop to match pwm cycle timed to end of pwm on - /* Enable counter */ - LL_TIM_EnableCounter(TIM1); - LL_TIM_EnableAllOutputs(TIM1); - /* Force update generation */ - LL_TIM_GenerateEvent_UPDATE(TIM1); + /* Enable counter */ + LL_TIM_EnableCounter(TIM1); + LL_TIM_EnableAllOutputs(TIM1); + /* Force update generation */ + LL_TIM_GenerateEvent_UPDATE(TIM1); #ifdef USE_ADC_INPUT #else - LL_TIM_CC_EnableChannel(IC_TIMER_REGISTER, - IC_TIMER_CHANNEL); // input capture and output compare - LL_TIM_EnableCounter(IC_TIMER_REGISTER); + LL_TIM_CC_EnableChannel(IC_TIMER_REGISTER, + IC_TIMER_CHANNEL); // input capture and output compare + LL_TIM_EnableCounter(IC_TIMER_REGISTER); #endif #ifdef USE_LED_STRIP - send_LED_RGB(255, 0, 0); + send_LED_RGB(255, 0, 0); #endif #ifdef USE_RGB_LED - LED_GPIO_init(); - GPIOB->BRR = LL_GPIO_PIN_8; // turn on red - GPIOB->BSRR = LL_GPIO_PIN_5; - GPIOB->BSRR = LL_GPIO_PIN_3; // + LED_GPIO_init(); + GPIOB->BRR = LL_GPIO_PIN_8; // turn on red + GPIOB->BSRR = LL_GPIO_PIN_5; + GPIOB->BSRR = LL_GPIO_PIN_3; // #endif #ifdef USE_CUSTOM_LED - initLed(); + initLed(); #endif #ifdef USE_DRV8328_NSLEEP - initnSleep(); + initnSleep(); #endif #ifdef USE_DRV8328_NFAULT - initnFault(); + initnFault(); #endif #ifdef USE_DRVOFF - initDrvoff(); + initDrvoff(); #endif #ifndef BRUSHED_MODE - LL_TIM_EnableCounter(COM_TIMER); // commutation_timer priority 0 - LL_TIM_GenerateEvent_UPDATE(COM_TIMER); - LL_TIM_EnableIT_UPDATE(COM_TIMER); - COM_TIMER->DIER &= ~((0x1UL << (0U))); // disable for now. + LL_TIM_EnableCounter(COM_TIMER); // commutation_timer priority 0 + LL_TIM_GenerateEvent_UPDATE(COM_TIMER); + LL_TIM_EnableIT_UPDATE(COM_TIMER); + COM_TIMER->DIER &= ~((0x1UL << (0U))); // disable for now. #endif - LL_TIM_EnableCounter(UTILITY_TIMER); - LL_TIM_GenerateEvent_UPDATE(UTILITY_TIMER); - // - LL_TIM_EnableCounter(INTERVAL_TIMER); - LL_TIM_GenerateEvent_UPDATE(INTERVAL_TIMER); - - LL_TIM_EnableCounter(TEN_KHZ_TIMER); // 10khz timer - LL_TIM_GenerateEvent_UPDATE(TEN_KHZ_TIMER); - TEN_KHZ_TIMER->DIER |= (0x1UL << (0U)); // enable interrupt - // RCC->APB2ENR &= ~(1 << 22); // turn debug off + LL_TIM_EnableCounter(UTILITY_TIMER); + LL_TIM_GenerateEvent_UPDATE(UTILITY_TIMER); + // + LL_TIM_EnableCounter(INTERVAL_TIMER); + LL_TIM_GenerateEvent_UPDATE(INTERVAL_TIMER); + + LL_TIM_EnableCounter(TEN_KHZ_TIMER); // 10khz timer + LL_TIM_GenerateEvent_UPDATE(TEN_KHZ_TIMER); + TEN_KHZ_TIMER->DIER |= (0x1UL << (0U)); // enable interrupt + // RCC->APB2ENR &= ~(1 << 22); // turn debug off #ifdef USE_ADC - ADC_Init(); - enableADC_DMA(); - activateADC(); + ADC_Init(); + enableADC_DMA(); + activateADC(); #endif #ifndef MCU_F031 - __IO uint32_t wait_loop_index = 0; - /* Enable comparator */ - LL_COMP_Enable(MAIN_COMP); + __IO uint32_t wait_loop_index = 0; + /* Enable comparator */ + LL_COMP_Enable(MAIN_COMP); #ifdef N_VARIANT // needs comp 1 and 2 - LL_COMP_Enable(COMP1); + LL_COMP_Enable(COMP1); #endif - wait_loop_index = ((LL_COMP_DELAY_STARTUP_US * (SystemCoreClock / (100000 * 2))) / 10); - while (wait_loop_index != 0) { - wait_loop_index--; - } + wait_loop_index = ((LL_COMP_DELAY_STARTUP_US * (SystemCoreClock / (100000 * 2))) / 10); + while (wait_loop_index != 0) { + wait_loop_index--; + } #endif - NVIC_SetPriority(EXTI4_15_IRQn, 2); - NVIC_EnableIRQ(EXTI4_15_IRQn); - EXTI->IMR |= (1 << 15); + NVIC_SetPriority(EXTI4_15_IRQn, 2); + NVIC_EnableIRQ(EXTI4_15_IRQn); + EXTI->IMR |= (1 << 15); } diff --git a/Mcu/f051/Src/phaseouts.c b/Mcu/f051/Src/phaseouts.c index eedd2d8e..9e1f993d 100644 --- a/Mcu/f051/Src/phaseouts.c +++ b/Mcu/f051/Src/phaseouts.c @@ -31,65 +31,66 @@ extern char prop_brake_active; #endif void proportionalBrake() -{ // alternate all channels between braking (ABC LOW) - // and coasting (ABC float) put lower channel into - // alternate mode and turn upper OFF for each - // channel - // turn all HIGH channels off for ABC - - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; - - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; - - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; - // set low channel to PWM, duty cycle will now control braking - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); +{ + // alternate all channels between braking (ABC LOW) + // and coasting (ABC float) put lower channel into + // alternate mode and turn upper OFF for each + // channel + // turn all HIGH channels off for ABC + + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; + + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; + + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; + // set low channel to PWM, duty cycle will now control braking + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); } void phaseBPWM() { - if (!eepromBuffer.comp_pwm) { // for future - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); // low - } - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, - LL_GPIO_MODE_ALTERNATE); // high + if (!eepromBuffer.comp_pwm) { // for future + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); // low + } + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, + LL_GPIO_MODE_ALTERNATE); // high } void phaseBFLOAT() { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; } void phaseBLOW() { - // low mosfet on - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_B_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; + // low mosfet on + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_B_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; } //////////////////////////////PHASE @@ -97,37 +98,37 @@ void phaseBLOW() void phaseCPWM() { - if (!eepromBuffer.comp_pwm) { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); - } - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); + } + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, + LL_GPIO_MODE_ALTERNATE); } void phaseCFLOAT() { - // floating - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; + // floating + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; } void phaseCLOW() { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_C_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_C_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; } ///////////////////////////////////////////////PHASE 3 @@ -135,36 +136,36 @@ void phaseCLOW() void phaseAPWM() { - if (!eepromBuffer.comp_pwm) { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); - } - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); + } + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, + LL_GPIO_MODE_ALTERNATE); } void phaseAFLOAT() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; } void phaseALOW() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_A_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_A_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; } #else @@ -172,38 +173,38 @@ void phaseALOW() //////////////////////////////////PHASE 1////////////////////// void phaseBPWM() { - if (!eepromBuffer.comp_pwm) { // for future - // LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, - // PHASE_B_GPIO_LOW, LL_GPIO_MODE_OUTPUT); - // PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); // high pwm + if (!eepromBuffer.comp_pwm) { // for future + // LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, + // PHASE_B_GPIO_LOW, LL_GPIO_MODE_OUTPUT); + // PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); // high pwm } void phaseBFLOAT() { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable off - PHASE_B_GPIO_PORT_ENABLE->BRR = PHASE_B_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); // pwm off - PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable off + PHASE_B_GPIO_PORT_ENABLE->BRR = PHASE_B_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); // pwm off + PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; } void phaseBLOW() { - // low mosfet on - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); // pwm off - PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; + // low mosfet on + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); // pwm off + PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; } //////////////////////////////PHASE @@ -211,38 +212,38 @@ void phaseBLOW() void phaseCPWM() { - if (!eepromBuffer.comp_pwm) { - // LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - // LL_GPIO_MODE_OUTPUT); PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = - // PHASE_C_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + // LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + // LL_GPIO_MODE_OUTPUT); PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = + // PHASE_C_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); } void phaseCFLOAT() { - // floating - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable off - PHASE_C_GPIO_PORT_ENABLE->BRR = PHASE_C_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; + // floating + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable off + PHASE_C_GPIO_PORT_ENABLE->BRR = PHASE_C_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; } void phaseCLOW() { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; } ///////////////////////////////////////////////PHASE 3 @@ -250,113 +251,115 @@ void phaseCLOW() void phaseAPWM() { - if (!eepromBuffer.comp_pwm) { - // LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - // LL_GPIO_MODE_OUTPUT); PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = - // PHASE_A_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + // LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + // LL_GPIO_MODE_OUTPUT); PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = + // PHASE_A_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); } void phaseAFLOAT() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BRR = PHASE_A_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BRR = PHASE_A_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; } void phaseALOW() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; } #endif void allOff() { - phaseAFLOAT(); - phaseBFLOAT(); - phaseCFLOAT(); + phaseAFLOAT(); + phaseBFLOAT(); + phaseCFLOAT(); } void comStep(char newStep) { - switch (newStep) { - case 1: // A-B - phaseCFLOAT(); - phaseBLOW(); - phaseAPWM(); - break; - - case 2: // C-B - phaseAFLOAT(); - phaseBLOW(); - phaseCPWM(); - break; - - case 3: // C-A - phaseBFLOAT(); - phaseALOW(); - phaseCPWM(); - break; - - case 4: // B-A - phaseCFLOAT(); - phaseALOW(); - phaseBPWM(); - break; - - case 5: // B-C - phaseAFLOAT(); - phaseCLOW(); - phaseBPWM(); - break; - - case 6: // A-C - phaseBFLOAT(); - phaseCLOW(); - phaseAPWM(); - break; - } -} + switch (newStep) { + case 1: // A-B + phaseCFLOAT(); + phaseBLOW(); + phaseAPWM(); + break; -void fullBrake() -{ // full braking shorting all low sides - phaseALOW(); + case 2: // C-B + phaseAFLOAT(); phaseBLOW(); + phaseCPWM(); + break; + + case 3: // C-A + phaseBFLOAT(); + phaseALOW(); + phaseCPWM(); + break; + + case 4: // B-A + phaseCFLOAT(); + phaseALOW(); + phaseBPWM(); + break; + + case 5: // B-C + phaseAFLOAT(); phaseCLOW(); + phaseBPWM(); + break; + + case 6: // A-C + phaseBFLOAT(); + phaseCLOW(); + phaseAPWM(); + break; + } +} + +void fullBrake() +{ + // full braking shorting all low sides + phaseALOW(); + phaseBLOW(); + phaseCLOW(); } void allpwm() -{ // for stepper_sine - phaseAPWM(); - phaseBPWM(); - phaseCPWM(); +{ + // for stepper_sine + phaseAPWM(); + phaseBPWM(); + phaseCPWM(); } void twoChannelForward() { - phaseAPWM(); - phaseBLOW(); - phaseCPWM(); + phaseAPWM(); + phaseBLOW(); + phaseCPWM(); } void twoChannelReverse() { - phaseALOW(); - phaseBPWM(); - phaseCLOW(); + phaseALOW(); + phaseBPWM(); + phaseCLOW(); } diff --git a/Mcu/f051/Src/serial_telemetry.c b/Mcu/f051/Src/serial_telemetry.c index aedaf507..adc21d16 100644 --- a/Mcu/f051/Src/serial_telemetry.c +++ b/Mcu/f051/Src/serial_telemetry.c @@ -15,189 +15,193 @@ uint8_t nbDataToTransmit = sizeof(aTxBuffer); #ifdef USE_PA14_TELEMETRY void telem_UART_Init(void) { - LL_USART_InitTypeDef USART_InitStruct = { 0 }; - - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - - /* Peripheral clock enable */ - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_USART2); - - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); - /**USART1 GPIO Configuration - - PB6 ------> USART1_TX - */ - - GPIO_InitStruct.Pin = LL_GPIO_PIN_14; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; - GPIO_InitStruct.Alternate = LL_GPIO_AF_1; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - - /* USART1 DMA Init */ - - /* USART1_TX Init */ - LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_4, - LL_DMA_DIRECTION_MEMORY_TO_PERIPH); - LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_4, LL_DMA_PRIORITY_LOW); - LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_4, LL_DMA_MODE_NORMAL); - LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_4, LL_DMA_PERIPH_NOINCREMENT); - LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_4, LL_DMA_MEMORY_INCREMENT); - LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_4, LL_DMA_PDATAALIGN_BYTE); - LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_4, LL_DMA_MDATAALIGN_BYTE); - - /* USART1 interrupt Init */ - NVIC_SetPriority(USART2_IRQn, 3); - NVIC_EnableIRQ(USART2_IRQn); - - USART_InitStruct.BaudRate = 115200; - USART_InitStruct.DataWidth = LL_USART_DATAWIDTH_8B; - USART_InitStruct.StopBits = LL_USART_STOPBITS_1; - USART_InitStruct.Parity = LL_USART_PARITY_NONE; - USART_InitStruct.TransferDirection = LL_USART_DIRECTION_TX; - USART_InitStruct.HardwareFlowControl = LL_USART_HWCONTROL_NONE; - USART_InitStruct.OverSampling = LL_USART_OVERSAMPLING_16; - LL_USART_Init(USART2, &USART_InitStruct); - LL_USART_DisableIT_CTS(USART2); - LL_USART_ConfigAsyncMode(USART2); - LL_USART_Enable(USART2); - - // set dma address - LL_DMA_ConfigAddresses( - DMA1, LL_DMA_CHANNEL_4, (uint32_t)aTxBuffer, - LL_USART_DMA_GetRegAddr(USART2, LL_USART_DMA_REG_DATA_TRANSMIT), - LL_DMA_GetDataTransferDirection(DMA1, LL_DMA_CHANNEL_4)); - LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_4, nbDataToTransmit); - - /* (5) Enable DMA transfer complete/error interrupts */ - LL_DMA_EnableIT_TC(DMA1, LL_DMA_CHANNEL_4); - LL_DMA_EnableIT_TE(DMA1, LL_DMA_CHANNEL_4); + LL_USART_InitTypeDef USART_InitStruct = { 0 }; + + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + + /* Peripheral clock enable */ + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_USART2); + + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); + /**USART1 GPIO Configuration + + PB6 ------> USART1_TX + */ + + GPIO_InitStruct.Pin = LL_GPIO_PIN_14; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; + GPIO_InitStruct.Alternate = LL_GPIO_AF_1; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + /* USART1 DMA Init */ + + /* USART1_TX Init */ + LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_4, + LL_DMA_DIRECTION_MEMORY_TO_PERIPH); + LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_4, LL_DMA_PRIORITY_LOW); + LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_4, LL_DMA_MODE_NORMAL); + LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_4, LL_DMA_PERIPH_NOINCREMENT); + LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_4, LL_DMA_MEMORY_INCREMENT); + LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_4, LL_DMA_PDATAALIGN_BYTE); + LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_4, LL_DMA_MDATAALIGN_BYTE); + + /* USART1 interrupt Init */ + NVIC_SetPriority(USART2_IRQn, 3); + NVIC_EnableIRQ(USART2_IRQn); + + USART_InitStruct.BaudRate = 115200; + USART_InitStruct.DataWidth = LL_USART_DATAWIDTH_8B; + USART_InitStruct.StopBits = LL_USART_STOPBITS_1; + USART_InitStruct.Parity = LL_USART_PARITY_NONE; + USART_InitStruct.TransferDirection = LL_USART_DIRECTION_TX; + USART_InitStruct.HardwareFlowControl = LL_USART_HWCONTROL_NONE; + USART_InitStruct.OverSampling = LL_USART_OVERSAMPLING_16; + LL_USART_Init(USART2, &USART_InitStruct); + LL_USART_DisableIT_CTS(USART2); + LL_USART_ConfigAsyncMode(USART2); + LL_USART_Enable(USART2); + + // set dma address + LL_DMA_ConfigAddresses( + DMA1, LL_DMA_CHANNEL_4, (uint32_t)aTxBuffer, + LL_USART_DMA_GetRegAddr(USART2, LL_USART_DMA_REG_DATA_TRANSMIT), + LL_DMA_GetDataTransferDirection(DMA1, LL_DMA_CHANNEL_4)); + LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_4, nbDataToTransmit); + + /* (5) Enable DMA transfer complete/error interrupts */ + LL_DMA_EnableIT_TC(DMA1, LL_DMA_CHANNEL_4); + LL_DMA_EnableIT_TE(DMA1, LL_DMA_CHANNEL_4); } void send_telem_DMA() -{ // set data length and enable channel to start transfer - LL_USART_SetTransferDirection(USART2, LL_USART_DIRECTION_TX); - // GPIOB->OTYPER &= 0 << 6; - LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_4, nbDataToTransmit); - LL_USART_EnableDMAReq_TX(USART2); - - LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_4); - LL_USART_SetTransferDirection(USART2, LL_USART_DIRECTION_RX); +{ + // set data length and enable channel to start transfer + LL_USART_SetTransferDirection(USART2, LL_USART_DIRECTION_TX); + // GPIOB->OTYPER &= 0 << 6; + LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_4, nbDataToTransmit); + LL_USART_EnableDMAReq_TX(USART2); + + LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_4); + LL_USART_SetTransferDirection(USART2, LL_USART_DIRECTION_RX); } #else void telem_UART_Init(void) { - LL_USART_InitTypeDef USART_InitStruct = { 0 }; - - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - - /* Peripheral clock enable */ - LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_USART1); - - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); - /**USART1 GPIO Configuration - - PB6 ------> USART1_TX - */ - - GPIO_InitStruct.Pin = LL_GPIO_PIN_6; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; - GPIO_InitStruct.Alternate = LL_GPIO_AF_0; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - /* USART1 DMA Init */ - - /* USART1_TX Init */ - LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_2, - LL_DMA_DIRECTION_MEMORY_TO_PERIPH); - LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_2, LL_DMA_PRIORITY_LOW); - LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_2, LL_DMA_MODE_NORMAL); - LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_2, LL_DMA_PERIPH_NOINCREMENT); - LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_2, LL_DMA_MEMORY_INCREMENT); - LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_2, LL_DMA_PDATAALIGN_BYTE); - LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_2, LL_DMA_MDATAALIGN_BYTE); - - /* USART1 interrupt Init */ - NVIC_SetPriority(USART1_IRQn, 3); - NVIC_EnableIRQ(USART1_IRQn); - - USART_InitStruct.BaudRate = 115200; - USART_InitStruct.DataWidth = LL_USART_DATAWIDTH_8B; - USART_InitStruct.StopBits = LL_USART_STOPBITS_1; - USART_InitStruct.Parity = LL_USART_PARITY_NONE; - USART_InitStruct.TransferDirection = LL_USART_DIRECTION_TX; - USART_InitStruct.HardwareFlowControl = LL_USART_HWCONTROL_NONE; - USART_InitStruct.OverSampling = LL_USART_OVERSAMPLING_16; - LL_USART_Init(USART1, &USART_InitStruct); - LL_USART_DisableIT_CTS(USART1); - LL_USART_ConfigAsyncMode(USART1); - LL_USART_Enable(USART1); - - // set dma address - LL_DMA_ConfigAddresses( - DMA1, LL_DMA_CHANNEL_2, (uint32_t)aTxBuffer, - LL_USART_DMA_GetRegAddr(USART1, LL_USART_DMA_REG_DATA_TRANSMIT), - LL_DMA_GetDataTransferDirection(DMA1, LL_DMA_CHANNEL_2)); - LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_2, nbDataToTransmit); - - /* (5) Enable DMA transfer complete/error interrupts */ - LL_DMA_EnableIT_TC(DMA1, LL_DMA_CHANNEL_2); - LL_DMA_EnableIT_TE(DMA1, LL_DMA_CHANNEL_2); + LL_USART_InitTypeDef USART_InitStruct = { 0 }; + + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + + /* Peripheral clock enable */ + LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_USART1); + + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); + /**USART1 GPIO Configuration + + PB6 ------> USART1_TX + */ + + GPIO_InitStruct.Pin = LL_GPIO_PIN_6; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; + GPIO_InitStruct.Alternate = LL_GPIO_AF_0; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + /* USART1 DMA Init */ + + /* USART1_TX Init */ + LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_2, + LL_DMA_DIRECTION_MEMORY_TO_PERIPH); + LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_2, LL_DMA_PRIORITY_LOW); + LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_2, LL_DMA_MODE_NORMAL); + LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_2, LL_DMA_PERIPH_NOINCREMENT); + LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_2, LL_DMA_MEMORY_INCREMENT); + LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_2, LL_DMA_PDATAALIGN_BYTE); + LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_2, LL_DMA_MDATAALIGN_BYTE); + + /* USART1 interrupt Init */ + NVIC_SetPriority(USART1_IRQn, 3); + NVIC_EnableIRQ(USART1_IRQn); + + USART_InitStruct.BaudRate = 115200; + USART_InitStruct.DataWidth = LL_USART_DATAWIDTH_8B; + USART_InitStruct.StopBits = LL_USART_STOPBITS_1; + USART_InitStruct.Parity = LL_USART_PARITY_NONE; + USART_InitStruct.TransferDirection = LL_USART_DIRECTION_TX; + USART_InitStruct.HardwareFlowControl = LL_USART_HWCONTROL_NONE; + USART_InitStruct.OverSampling = LL_USART_OVERSAMPLING_16; + LL_USART_Init(USART1, &USART_InitStruct); + LL_USART_DisableIT_CTS(USART1); + LL_USART_ConfigAsyncMode(USART1); + LL_USART_Enable(USART1); + + // set dma address + LL_DMA_ConfigAddresses( + DMA1, LL_DMA_CHANNEL_2, (uint32_t)aTxBuffer, + LL_USART_DMA_GetRegAddr(USART1, LL_USART_DMA_REG_DATA_TRANSMIT), + LL_DMA_GetDataTransferDirection(DMA1, LL_DMA_CHANNEL_2)); + LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_2, nbDataToTransmit); + + /* (5) Enable DMA transfer complete/error interrupts */ + LL_DMA_EnableIT_TC(DMA1, LL_DMA_CHANNEL_2); + LL_DMA_EnableIT_TE(DMA1, LL_DMA_CHANNEL_2); } void send_telem_DMA() -{ // set data length and enable channel to start transfer - LL_USART_SetTransferDirection(USART1, LL_USART_DIRECTION_TX); - // GPIOB->OTYPER &= 0 << 6; - LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_2, nbDataToTransmit); - LL_USART_EnableDMAReq_TX(USART1); - - LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_2); - LL_USART_SetTransferDirection(USART1, LL_USART_DIRECTION_RX); +{ + // set data length and enable channel to start transfer + LL_USART_SetTransferDirection(USART1, LL_USART_DIRECTION_TX); + // GPIOB->OTYPER &= 0 << 6; + LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_2, nbDataToTransmit); + LL_USART_EnableDMAReq_TX(USART1); + + LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_2); + LL_USART_SetTransferDirection(USART1, LL_USART_DIRECTION_RX); } #endif uint8_t update_crc8(uint8_t crc, uint8_t crc_seed) { - uint8_t crc_u, i; - crc_u = crc; - crc_u ^= crc_seed; - for (i = 0; i < 8; i++) - crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); - return (crc_u); + uint8_t crc_u, i; + crc_u = crc; + crc_u ^= crc_seed; + for (i = 0; i < 8; i++) { + crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); + } + return (crc_u); } uint8_t get_crc8(uint8_t* Buf, uint8_t BufLen) { - uint8_t crc = 0, i; - for (i = 0; i < BufLen; i++) - crc = update_crc8(Buf[i], crc); - return (crc); + uint8_t crc = 0, i; + for (i = 0; i < BufLen; i++) { + crc = update_crc8(Buf[i], crc); + } + return (crc); } void makeTelemPackage(uint8_t temp, uint16_t voltage, uint16_t current, - uint16_t consumption, uint16_t e_rpm) + uint16_t consumption, uint16_t e_rpm) { - aTxBuffer[0] = temp; // temperature + aTxBuffer[0] = temp; // temperature - aTxBuffer[1] = (voltage >> 8) & 0xFF; // voltage hB - aTxBuffer[2] = voltage & 0xFF; // voltage lowB + aTxBuffer[1] = (voltage >> 8) & 0xFF; // voltage hB + aTxBuffer[2] = voltage & 0xFF; // voltage lowB - aTxBuffer[3] = (current >> 8) & 0xFF; // current - aTxBuffer[4] = current & 0xFF; // divide by 10 for Amps + aTxBuffer[3] = (current >> 8) & 0xFF; // current + aTxBuffer[4] = current & 0xFF; // divide by 10 for Amps - aTxBuffer[5] = (consumption >> 8) & 0xFF; // consumption - aTxBuffer[6] = consumption & 0xFF; // in mah + aTxBuffer[5] = (consumption >> 8) & 0xFF; // consumption + aTxBuffer[6] = consumption & 0xFF; // in mah - aTxBuffer[7] = (e_rpm >> 8) & 0xFF; // - aTxBuffer[8] = e_rpm & 0xFF; // eRpM *100 + aTxBuffer[7] = (e_rpm >> 8) & 0xFF; // + aTxBuffer[8] = e_rpm & 0xFF; // eRpM *100 - aTxBuffer[9] = get_crc8(aTxBuffer, 9); + aTxBuffer[9] = get_crc8(aTxBuffer, 9); } diff --git a/Mcu/f051/Src/stm32f0xx_it.c b/Mcu/f051/Src/stm32f0xx_it.c index 0bdb6b62..47384a51 100644 --- a/Mcu/f051/Src/stm32f0xx_it.c +++ b/Mcu/f051/Src/stm32f0xx_it.c @@ -60,8 +60,8 @@ void NMI_Handler(void) { } */ void HardFault_Handler(void) { - while (1) { - } + while (1) { + } } /** @@ -91,13 +91,13 @@ void SysTick_Handler(void) { } */ void DMA1_Channel2_3_IRQHandler(void) { - if (LL_DMA_IsActiveFlag_TC2(DMA1)) { - LL_DMA_ClearFlag_GI2(DMA1); - LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_2); - } else if (LL_DMA_IsActiveFlag_TE2(DMA1)) { - LL_DMA_ClearFlag_GI2(DMA1); - LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_2); - } + if (LL_DMA_IsActiveFlag_TC2(DMA1)) { + LL_DMA_ClearFlag_GI2(DMA1); + LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_2); + } else if (LL_DMA_IsActiveFlag_TE2(DMA1)) { + LL_DMA_ClearFlag_GI2(DMA1); + LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_2); + } } /** @@ -105,85 +105,85 @@ void DMA1_Channel2_3_IRQHandler(void) */ void DMA1_Channel4_5_IRQHandler(void) { - /* USER CODE BEGIN DMA1_Channel4_5_IRQn 0 */ + /* USER CODE BEGIN DMA1_Channel4_5_IRQn 0 */ #ifdef USE_TIMER_15_CHANNEL_1 - if (armed && dshot_telemetry) { - DMA1->IFCR |= DMA_IFCR_CGIF5; - DMA1_Channel5->CCR = 0x00; - if (out_put) { - receiveDshotDma(); - compute_dshot_flag = 2; - } else { - sendDshotDma(); - compute_dshot_flag = 1; - } - EXTI->SWIER |= LL_EXTI_LINE_15; - return; - } - - if (LL_DMA_IsActiveFlag_HT5(DMA1)) { - if (servoPwm) { - LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, - LL_TIM_IC_POLARITY_FALLING); - LL_DMA_ClearFlag_HT5(DMA1); - } + if (armed && dshot_telemetry) { + DMA1->IFCR |= DMA_IFCR_CGIF5; + DMA1_Channel5->CCR = 0x00; + if (out_put) { + receiveDshotDma(); + compute_dshot_flag = 2; + } else { + sendDshotDma(); + compute_dshot_flag = 1; } - if (LL_DMA_IsActiveFlag_TC5(DMA1) == 1) { - LL_DMA_ClearFlag_GI5(DMA1); - LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_5); - transfercomplete(); - EXTI->SWIER |= LL_EXTI_LINE_15; - return; - } else if (LL_DMA_IsActiveFlag_TE5(DMA1) == 1) { - LL_DMA_ClearFlag_GI5(DMA1); + EXTI->SWIER |= LL_EXTI_LINE_15; + return; + } + + if (LL_DMA_IsActiveFlag_HT5(DMA1)) { + if (servoPwm) { + LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, + LL_TIM_IC_POLARITY_FALLING); + LL_DMA_ClearFlag_HT5(DMA1); } + } + if (LL_DMA_IsActiveFlag_TC5(DMA1) == 1) { + LL_DMA_ClearFlag_GI5(DMA1); + LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_5); + transfercomplete(); + EXTI->SWIER |= LL_EXTI_LINE_15; + return; + } else if (LL_DMA_IsActiveFlag_TE5(DMA1) == 1) { + LL_DMA_ClearFlag_GI5(DMA1); + } #ifdef USE_PA14_TELEMETRY - if (LL_DMA_IsActiveFlag_TC4(DMA1)) { - LL_DMA_ClearFlag_GI4(DMA1); - LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_4); - /* Call function Transmission complete Callback */ - } else if (LL_DMA_IsActiveFlag_TE4(DMA1)) { - LL_DMA_ClearFlag_GI4(DMA1); - LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_4); - /* Call Error function */ - // USART_TransferError_Callback(); - } + if (LL_DMA_IsActiveFlag_TC4(DMA1)) { + LL_DMA_ClearFlag_GI4(DMA1); + LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_4); + /* Call function Transmission complete Callback */ + } else if (LL_DMA_IsActiveFlag_TE4(DMA1)) { + LL_DMA_ClearFlag_GI4(DMA1); + LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_4); + /* Call Error function */ + // USART_TransferError_Callback(); + } #endif #endif - /* USER CODE END DMA1_Channel4_5_IRQn 0 */ + /* USER CODE END DMA1_Channel4_5_IRQn 0 */ - /* USER CODE BEGIN DMA1_Channel4_5_IRQn 1 */ + /* USER CODE BEGIN DMA1_Channel4_5_IRQn 1 */ #ifdef USE_TIMER_3_CHANNEL_1 - if (armed && dshot_telemetry) { - DMA1->IFCR |= DMA_IFCR_CGIF4; - DMA1_Channel4->CCR = 0x00; - if (out_put) { - receiveDshotDma(); - compute_dshot_flag = 2; - } else { - sendDshotDma(); - compute_dshot_flag = 1; - } - EXTI->SWIER |= LL_EXTI_LINE_15; - return; - } - if (LL_DMA_IsActiveFlag_HT4(DMA1)) { - if (servoPwm) { - LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, - LL_TIM_IC_POLARITY_FALLING); - LL_DMA_ClearFlag_HT4(DMA1); - } + if (armed && dshot_telemetry) { + DMA1->IFCR |= DMA_IFCR_CGIF4; + DMA1_Channel4->CCR = 0x00; + if (out_put) { + receiveDshotDma(); + compute_dshot_flag = 2; + } else { + sendDshotDma(); + compute_dshot_flag = 1; } - if (LL_DMA_IsActiveFlag_TC4(DMA1) == 1) { - LL_DMA_ClearFlag_GI4(DMA1); - LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_4); - transfercomplete(); - EXTI->SWIER |= LL_EXTI_LINE_15; - } else if (LL_DMA_IsActiveFlag_TE4(DMA1) == 1) { - LL_DMA_ClearFlag_GI4(DMA1); + EXTI->SWIER |= LL_EXTI_LINE_15; + return; + } + if (LL_DMA_IsActiveFlag_HT4(DMA1)) { + if (servoPwm) { + LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, + LL_TIM_IC_POLARITY_FALLING); + LL_DMA_ClearFlag_HT4(DMA1); } + } + if (LL_DMA_IsActiveFlag_TC4(DMA1) == 1) { + LL_DMA_ClearFlag_GI4(DMA1); + LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_4); + transfercomplete(); + EXTI->SWIER |= LL_EXTI_LINE_15; + } else if (LL_DMA_IsActiveFlag_TE4(DMA1) == 1) { + LL_DMA_ClearFlag_GI4(DMA1); + } #endif } @@ -193,11 +193,11 @@ void DMA1_Channel4_5_IRQHandler(void) */ void ADC1_COMP_IRQHandler(void) { - if (LL_EXTI_IsActiveFlag_0_31(LL_EXTI_LINE_21) != RESET) { - LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_21); - interruptRoutine(); - } - // + if (LL_EXTI_IsActiveFlag_0_31(LL_EXTI_LINE_21) != RESET) { + LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_21); + interruptRoutine(); + } + // } /** @@ -205,10 +205,10 @@ void ADC1_COMP_IRQHandler(void) */ void TIM6_DAC_IRQHandler(void) { - if (LL_TIM_IsActiveFlag_UPDATE(TIM6) == 1) { - LL_TIM_ClearFlag_UPDATE(TIM6); - tenKhzRoutine(); - } + if (LL_TIM_IsActiveFlag_UPDATE(TIM6) == 1) { + LL_TIM_ClearFlag_UPDATE(TIM6); + tenKhzRoutine(); + } } /** @@ -216,8 +216,8 @@ void TIM6_DAC_IRQHandler(void) */ void TIM14_IRQHandler(void) { - LL_TIM_ClearFlag_UPDATE(TIM14); - PeriodElapsedCallback(); + LL_TIM_ClearFlag_UPDATE(TIM14); + PeriodElapsedCallback(); } /** @@ -225,12 +225,12 @@ void TIM14_IRQHandler(void) */ void TIM16_IRQHandler(void) { - /* USER CODE BEGIN TIM16_IRQn 0 */ + /* USER CODE BEGIN TIM16_IRQn 0 */ - /* USER CODE END TIM16_IRQn 0 */ - /* USER CODE BEGIN TIM16_IRQn 1 */ + /* USER CODE END TIM16_IRQn 0 */ + /* USER CODE BEGIN TIM16_IRQn 1 */ - /* USER CODE END TIM16_IRQn 1 */ + /* USER CODE END TIM16_IRQn 1 */ } /** @@ -239,75 +239,75 @@ void TIM16_IRQHandler(void) */ void USART1_IRQHandler(void) { - /* USER CODE BEGIN USART1_IRQn 0 */ + /* USER CODE BEGIN USART1_IRQn 0 */ - /* USER CODE END USART1_IRQn 0 */ - /* USER CODE BEGIN USART1_IRQn 1 */ + /* USER CODE END USART1_IRQn 0 */ + /* USER CODE BEGIN USART1_IRQn 1 */ - /* USER CODE END USART1_IRQn 1 */ + /* USER CODE END USART1_IRQn 1 */ } /* USER CODE BEGIN 1 */ void TIM15_IRQHandler(void) { - if (LL_TIM_IsActiveFlag_CC1(TIM15) == 1) { - LL_TIM_ClearFlag_CC1(TIM15); - } - - if (LL_TIM_IsActiveFlag_UPDATE(TIM15) == 1) { - LL_TIM_ClearFlag_UPDATE(TIM15); - // update_interupt++; - } + if (LL_TIM_IsActiveFlag_CC1(TIM15) == 1) { + LL_TIM_ClearFlag_CC1(TIM15); + } + + if (LL_TIM_IsActiveFlag_UPDATE(TIM15) == 1) { + LL_TIM_ClearFlag_UPDATE(TIM15); + // update_interupt++; + } } void TIM3_IRQHandler(void) { - if (LL_TIM_IsActiveFlag_CC1(TIM3) == 1) { - LL_TIM_ClearFlag_CC1(TIM3); - } - - if (LL_TIM_IsActiveFlag_UPDATE(TIM3) == 1) { - LL_TIM_ClearFlag_UPDATE(TIM3); - // update_interupt++; - } + if (LL_TIM_IsActiveFlag_CC1(TIM3) == 1) { + LL_TIM_ClearFlag_CC1(TIM3); + } + + if (LL_TIM_IsActiveFlag_UPDATE(TIM3) == 1) { + LL_TIM_ClearFlag_UPDATE(TIM3); + // update_interupt++; + } } void DMA1_Channel1_IRQHandler(void) // ADC { - if (LL_DMA_IsActiveFlag_TC1(DMA1) == 1) { - /* Clear flag DMA global interrupt */ - /* (global interrupt flag: half transfer and transfer complete flags) */ - LL_DMA_ClearFlag_GI1(DMA1); - ADC_DMA_Callback(); - /* Call interruption treatment function */ - // AdcDmaTransferComplete_Callback(); - } - - /* Check whether DMA transfer error caused the DMA interruption */ - if (LL_DMA_IsActiveFlag_TE1(DMA1) == 1) { - /* Clear flag DMA transfer error */ - LL_DMA_ClearFlag_TE1(DMA1); - - /* Call interruption treatment function */ - } + if (LL_DMA_IsActiveFlag_TC1(DMA1) == 1) { + /* Clear flag DMA global interrupt */ + /* (global interrupt flag: half transfer and transfer complete flags) */ + LL_DMA_ClearFlag_GI1(DMA1); + ADC_DMA_Callback(); + /* Call interruption treatment function */ + // AdcDmaTransferComplete_Callback(); + } + + /* Check whether DMA transfer error caused the DMA interruption */ + if (LL_DMA_IsActiveFlag_TE1(DMA1) == 1) { + /* Clear flag DMA transfer error */ + LL_DMA_ClearFlag_TE1(DMA1); + + /* Call interruption treatment function */ + } } void TIM1_BRK_UP_TRG_COM_IRQHandler(void) { - if (LL_TIM_IsActiveFlag_CC1(TIM1) == 1) { - LL_TIM_ClearFlag_CC1(TIM1); - } - - if (LL_TIM_IsActiveFlag_UPDATE(TIM1) == 1) { - LL_TIM_ClearFlag_UPDATE(TIM1); - // doPWMChanges(); - } + if (LL_TIM_IsActiveFlag_CC1(TIM1) == 1) { + LL_TIM_ClearFlag_CC1(TIM1); + } + + if (LL_TIM_IsActiveFlag_UPDATE(TIM1) == 1) { + LL_TIM_ClearFlag_UPDATE(TIM1); + // doPWMChanges(); + } } void EXTI4_15_IRQHandler(void) { - LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_15); - processDshot(); + LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_15); + processDshot(); } /* USER CODE END 1 */ diff --git a/Mcu/f051/Src/system_stm32f0xx.c b/Mcu/f051/Src/system_stm32f0xx.c index f7f1811f..8b8afddc 100644 --- a/Mcu/f051/Src/system_stm32f0xx.c +++ b/Mcu/f051/Src/system_stm32f0xx.c @@ -125,7 +125,8 @@ uint32_t SystemCoreClock = 8000000; const uint8_t AHBPrescTable[16] = { 0, 0, 0, 0, 0, 0, 0, 0, - 1, 2, 3, 4, 6, 7, 8, 9 }; + 1, 2, 3, 4, 6, 7, 8, 9 + }; const uint8_t APBPrescTable[8] = { 0, 0, 0, 0, 1, 2, 3, 4 }; /** @@ -151,12 +152,12 @@ const uint8_t APBPrescTable[8] = { 0, 0, 0, 0, 1, 2, 3, 4 }; */ void SystemInit(void) { - /* NOTE :SystemInit(): This function is called at startup just after reset - and before branch to main program. This call is made inside the - "startup_stm32f0xx.s" file. User can setups the default system clock - (System clock source, PLL Multiplier and Divider factors, AHB/APBx - prescalers and Flash settings). - */ + /* NOTE :SystemInit(): This function is called at startup just after reset + and before branch to main program. This call is made inside the + "startup_stm32f0xx.s" file. User can setups the default system clock + (System clock source, PLL Multiplier and Divider factors, AHB/APBx + prescalers and Flash settings). + */ } /** @@ -199,64 +200,64 @@ void SystemInit(void) */ void SystemCoreClockUpdate(void) { - uint32_t tmp = 0, pllmull = 0, pllsource = 0, predivfactor = 0; + uint32_t tmp = 0, pllmull = 0, pllsource = 0, predivfactor = 0; - /* Get SYSCLK source - * -------------------------------------------------------*/ - tmp = RCC->CFGR & RCC_CFGR_SWS; + /* Get SYSCLK source + * -------------------------------------------------------*/ + tmp = RCC->CFGR & RCC_CFGR_SWS; - switch (tmp) { - case RCC_CFGR_SWS_HSI: /* HSI used as system clock */ - SystemCoreClock = HSI_VALUE; - break; - case RCC_CFGR_SWS_HSE: /* HSE used as system clock */ - SystemCoreClock = HSE_VALUE; - break; - case RCC_CFGR_SWS_PLL: /* PLL used as system clock */ - /* Get PLL clock source and multiplication factor - * ----------------------*/ - pllmull = RCC->CFGR & RCC_CFGR_PLLMUL; - pllsource = RCC->CFGR & RCC_CFGR_PLLSRC; - pllmull = (pllmull >> 18) + 2; - predivfactor = (RCC->CFGR2 & RCC_CFGR2_PREDIV) + 1; + switch (tmp) { + case RCC_CFGR_SWS_HSI: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + case RCC_CFGR_SWS_HSE: /* HSE used as system clock */ + SystemCoreClock = HSE_VALUE; + break; + case RCC_CFGR_SWS_PLL: /* PLL used as system clock */ + /* Get PLL clock source and multiplication factor + * ----------------------*/ + pllmull = RCC->CFGR & RCC_CFGR_PLLMUL; + pllsource = RCC->CFGR & RCC_CFGR_PLLSRC; + pllmull = (pllmull >> 18) + 2; + predivfactor = (RCC->CFGR2 & RCC_CFGR2_PREDIV) + 1; - if (pllsource == RCC_CFGR_PLLSRC_HSE_PREDIV) { - /* HSE used as PLL clock source : SystemCoreClock = HSE/PREDIV * - * PLLMUL - */ - SystemCoreClock = (HSE_VALUE / predivfactor) * pllmull; - } + if (pllsource == RCC_CFGR_PLLSRC_HSE_PREDIV) { + /* HSE used as PLL clock source : SystemCoreClock = HSE/PREDIV * + * PLLMUL + */ + SystemCoreClock = (HSE_VALUE / predivfactor) * pllmull; + } #if defined(STM32F042x6) || defined(STM32F048xx) || defined(STM32F072xB) || defined(STM32F078xx) || defined(STM32F091xC) || defined(STM32F098xx) - else if (pllsource == RCC_CFGR_PLLSRC_HSI48_PREDIV) { - /* HSI48 used as PLL clock source : SystemCoreClock = HSI48/PREDIV * - * PLLMUL */ - SystemCoreClock = (HSI48_VALUE / predivfactor) * pllmull; - } + else if (pllsource == RCC_CFGR_PLLSRC_HSI48_PREDIV) { + /* HSI48 used as PLL clock source : SystemCoreClock = HSI48/PREDIV * + * PLLMUL */ + SystemCoreClock = (HSI48_VALUE / predivfactor) * pllmull; + } #endif /* STM32F042x6 || STM32F048xx || STM32F072xB || STM32F078xx || \ STM32F091xC || STM32F098xx */ - else { + else { #if defined(STM32F042x6) || defined(STM32F048xx) || defined(STM32F070x6) || defined(STM32F078xx) || defined(STM32F071xB) || defined(STM32F072xB) || defined(STM32F070xB) || defined(STM32F091xC) || defined(STM32F098xx) || defined(STM32F030xC) - /* HSI used as PLL clock source : SystemCoreClock = HSI/PREDIV * - * PLLMUL - */ - SystemCoreClock = (HSI_VALUE / predivfactor) * pllmull; + /* HSI used as PLL clock source : SystemCoreClock = HSI/PREDIV * + * PLLMUL + */ + SystemCoreClock = (HSI_VALUE / predivfactor) * pllmull; #else - /* HSI used as PLL clock source : SystemCoreClock = HSI/2 * PLLMUL */ - SystemCoreClock = (HSI_VALUE >> 1) * pllmull; + /* HSI used as PLL clock source : SystemCoreClock = HSI/2 * PLLMUL */ + SystemCoreClock = (HSI_VALUE >> 1) * pllmull; #endif /* STM32F042x6 || STM32F048xx || STM32F070x6 || \ STM32F071xB || STM32F072xB || STM32F078xx || STM32F070xB || \ STM32F091xC || STM32F098xx || STM32F030xC */ - } - break; - default: /* HSI used as system clock */ - SystemCoreClock = HSI_VALUE; - break; } - /* Compute HCLK clock frequency ----------------*/ - /* Get HCLK prescaler */ - tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; - /* HCLK clock frequency */ - SystemCoreClock >>= tmp; + break; + default: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + } + /* Compute HCLK clock frequency ----------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; + /* HCLK clock frequency */ + SystemCoreClock >>= tmp; } /** diff --git a/Mcu/f415/Inc/IO.h b/Mcu/f415/Inc/IO.h index 352e164a..317300f5 100644 --- a/Mcu/f415/Inc/IO.h +++ b/Mcu/f415/Inc/IO.h @@ -33,6 +33,6 @@ extern uint8_t degrees_celsius; extern uint16_t ADC_raw_volts; extern uint16_t servo_low_threshold; // anything below this point considered 0 extern uint16_t - servo_high_threshold; // anything above this point considered 2000 (max) +servo_high_threshold; // anything above this point considered 2000 (max) extern uint16_t servo_neutral; extern uint8_t servo_dead_band; diff --git a/Mcu/f415/Inc/serial_telemetry.h b/Mcu/f415/Inc/serial_telemetry.h index b4f1c5cf..fb8cd3b9 100644 --- a/Mcu/f415/Inc/serial_telemetry.h +++ b/Mcu/f415/Inc/serial_telemetry.h @@ -11,7 +11,7 @@ #define SERIAL_TELEMETRY_H_ void makeTelemPackage(uint8_t temp, uint16_t voltage, uint16_t current, - uint16_t consumption, uint16_t e_rpm); + uint16_t consumption, uint16_t e_rpm); void telem_UART_Init(void); void send_telem_DMA(); diff --git a/Mcu/f415/Inc/system_at32f415.h b/Mcu/f415/Inc/system_at32f415.h index 4c439802..dbd2cf78 100644 --- a/Mcu/f415/Inc/system_at32f415.h +++ b/Mcu/f415/Inc/system_at32f415.h @@ -56,7 +56,7 @@ extern "C" { */ extern unsigned int - system_core_clock; /*!< system clock frequency (core clock) */ +system_core_clock; /*!< system clock frequency (core clock) */ /** * @} diff --git a/Mcu/f415/Src/ADC.c b/Mcu/f415/Src/ADC.c index ff6aa536..ca64d350 100644 --- a/Mcu/f415/Src/ADC.c +++ b/Mcu/f415/Src/ADC.c @@ -20,82 +20,86 @@ extern uint16_t ADC_raw_current; extern uint16_t ADC_raw_input; void ADC_DMA_Callback() -{ // read dma buffer and set extern variables +{ + // read dma buffer and set extern variables #ifdef USE_ADC_INPUT - ADC_raw_temp = ADCDataDMA[3]; - ADC_raw_volts = ADCDataDMA[1] / 2; - ADC_raw_current = ADCDataDMA[2]; - ADC_raw_input = ADCDataDMA[0]; + ADC_raw_temp = ADCDataDMA[3]; + ADC_raw_volts = ADCDataDMA[1] / 2; + ADC_raw_current = ADCDataDMA[2]; + ADC_raw_input = ADCDataDMA[0]; #else - ADC_raw_temp = ADCDataDMA[2]; + ADC_raw_temp = ADCDataDMA[2]; #ifdef PA6_VOLTAGE - ADC_raw_volts = ADCDataDMA[1]; - ADC_raw_current = ADCDataDMA[0]; + ADC_raw_volts = ADCDataDMA[1]; + ADC_raw_current = ADCDataDMA[0]; #else - ADC_raw_volts = ADCDataDMA[0]; - ADC_raw_current = ADCDataDMA[1]; + ADC_raw_volts = ADCDataDMA[0]; + ADC_raw_current = ADCDataDMA[1]; #endif #endif } void ADC_Init(void) { - dma_init_type dma_init_struct; - crm_periph_clock_enable(CRM_DMA1_PERIPH_CLOCK, TRUE); - dma_flexible_config(DMA1,FLEX_CHANNEL1,DMA_FLEXIBLE_ADC1); - nvic_irq_enable(DMA1_Channel1_IRQn, 2, 0); - dma_reset(DMA1_CHANNEL1); - dma_default_para_init(&dma_init_struct); - dma_init_struct.buffer_size = 3; - dma_init_struct.direction = DMA_DIR_PERIPHERAL_TO_MEMORY; - dma_init_struct.memory_base_addr = (uint32_t)&ADCDataDMA; - dma_init_struct.memory_data_width = DMA_MEMORY_DATA_WIDTH_HALFWORD; - dma_init_struct.memory_inc_enable = TRUE; - dma_init_struct.peripheral_base_addr = (uint32_t) & (ADC1->odt); - dma_init_struct.peripheral_data_width = DMA_PERIPHERAL_DATA_WIDTH_HALFWORD; - dma_init_struct.peripheral_inc_enable = FALSE; - dma_init_struct.priority = DMA_PRIORITY_HIGH; - dma_init_struct.loop_mode_enable = TRUE; - dma_init(DMA1_CHANNEL1, &dma_init_struct); + dma_init_type dma_init_struct; + crm_periph_clock_enable(CRM_DMA1_PERIPH_CLOCK, TRUE); + dma_flexible_config(DMA1,FLEX_CHANNEL1,DMA_FLEXIBLE_ADC1); + nvic_irq_enable(DMA1_Channel1_IRQn, 2, 0); + dma_reset(DMA1_CHANNEL1); + dma_default_para_init(&dma_init_struct); + dma_init_struct.buffer_size = 3; + dma_init_struct.direction = DMA_DIR_PERIPHERAL_TO_MEMORY; + dma_init_struct.memory_base_addr = (uint32_t)&ADCDataDMA; + dma_init_struct.memory_data_width = DMA_MEMORY_DATA_WIDTH_HALFWORD; + dma_init_struct.memory_inc_enable = TRUE; + dma_init_struct.peripheral_base_addr = (uint32_t) & (ADC1->odt); + dma_init_struct.peripheral_data_width = DMA_PERIPHERAL_DATA_WIDTH_HALFWORD; + dma_init_struct.peripheral_inc_enable = FALSE; + dma_init_struct.priority = DMA_PRIORITY_HIGH; + dma_init_struct.loop_mode_enable = TRUE; + dma_init(DMA1_CHANNEL1, &dma_init_struct); - dma_interrupt_enable(DMA1_CHANNEL1, DMA_FDT_INT, TRUE); - dma_channel_enable(DMA1_CHANNEL1, TRUE); + dma_interrupt_enable(DMA1_CHANNEL1, DMA_FDT_INT, TRUE); + dma_channel_enable(DMA1_CHANNEL1, TRUE); - adc_base_config_type adc_base_struct; - crm_periph_clock_enable(CRM_ADC1_PERIPH_CLOCK, TRUE); - crm_adc_clock_div_set(CRM_ADC_DIV_6); + adc_base_config_type adc_base_struct; + crm_periph_clock_enable(CRM_ADC1_PERIPH_CLOCK, TRUE); + crm_adc_clock_div_set(CRM_ADC_DIV_6); - adc_base_default_para_init(&adc_base_struct); - adc_base_struct.sequence_mode = TRUE; - adc_base_struct.repeat_mode = TRUE; - adc_base_struct.data_align = ADC_RIGHT_ALIGNMENT; - adc_base_struct.ordinary_channel_length = 3; - adc_base_config(ADC1, &adc_base_struct); + adc_base_default_para_init(&adc_base_struct); + adc_base_struct.sequence_mode = TRUE; + adc_base_struct.repeat_mode = TRUE; + adc_base_struct.data_align = ADC_RIGHT_ALIGNMENT; + adc_base_struct.ordinary_channel_length = 3; + adc_base_config(ADC1, &adc_base_struct); - adc_ordinary_channel_set(ADC1, ADC_CHANNEL_3, 1, ADC_SAMPLETIME_28_5); - adc_ordinary_channel_set(ADC1, ADC_CHANNEL_6, 2, ADC_SAMPLETIME_28_5); - adc_ordinary_channel_set(ADC1, ADC_CHANNEL_16, 3, ADC_SAMPLETIME_28_5); + adc_ordinary_channel_set(ADC1, ADC_CHANNEL_3, 1, ADC_SAMPLETIME_28_5); + adc_ordinary_channel_set(ADC1, ADC_CHANNEL_6, 2, ADC_SAMPLETIME_28_5); + adc_ordinary_channel_set(ADC1, ADC_CHANNEL_16, 3, ADC_SAMPLETIME_28_5); - adc_tempersensor_vintrv_enable(TRUE); - adc_ordinary_conversion_trigger_set(ADC1, ADC12_ORDINARY_TRIG_SOFTWARE, TRUE); + adc_tempersensor_vintrv_enable(TRUE); + adc_ordinary_conversion_trigger_set(ADC1, ADC12_ORDINARY_TRIG_SOFTWARE, TRUE); - adc_dma_mode_enable(ADC1, TRUE); + adc_dma_mode_enable(ADC1, TRUE); - adc_enable(ADC1, TRUE); - adc_calibration_init(ADC1); - while (adc_calibration_init_status_get(ADC1)) - ; - adc_calibration_start(ADC1); - while (adc_calibration_status_get(ADC1)) - ; + adc_enable(ADC1, TRUE); + adc_calibration_init(ADC1); + while (adc_calibration_init_status_get(ADC1)) + ; + adc_calibration_start(ADC1); + while (adc_calibration_status_get(ADC1)) + ; } -void startADCConversion() { adc_ordinary_software_trigger_enable(ADC1, TRUE); } +void startADCConversion() +{ + adc_ordinary_software_trigger_enable(ADC1, TRUE); +} int16_t getConvertedDegrees(uint16_t adcrawtemp) { - return (12600 - (int32_t)adcrawtemp * 33000 / 4096) / -42 + 5; + return (12600 - (int32_t)adcrawtemp * 33000 / 4096) / -42 + 5; } #endif // USE_ADC \ No newline at end of file diff --git a/Mcu/f415/Src/IO.c b/Mcu/f415/Src/IO.c index 46676613..87f93e3f 100644 --- a/Mcu/f415/Src/IO.c +++ b/Mcu/f415/Src/IO.c @@ -21,102 +21,108 @@ uint8_t buffer_padding = 7; void changeToOutput() { - INPUT_DMA_CHANNEL->ctrl |= DMA_DIR_MEMORY_TO_PERIPHERAL; - tmr_reset(IC_TIMER_REGISTER); + INPUT_DMA_CHANNEL->ctrl |= DMA_DIR_MEMORY_TO_PERIPHERAL; + tmr_reset(IC_TIMER_REGISTER); #ifdef USE_TIMER_3_CHANNEL_1 - IC_TIMER_REGISTER->cm1 = 0x60; // oc mode pwm - IC_TIMER_REGISTER->cctrl = 0x3; // + IC_TIMER_REGISTER->cm1 = 0x60; // oc mode pwm + IC_TIMER_REGISTER->cctrl = 0x3; // #endif #ifdef USE_TIMER_2_CHANNEL_3 - IC_TIMER_REGISTER->cm2 = 0x60; // oc mode pwm - IC_TIMER_REGISTER->cctrl = 0x300; // + IC_TIMER_REGISTER->cm2 = 0x60; // oc mode pwm + IC_TIMER_REGISTER->cctrl = 0x300; // #endif - IC_TIMER_REGISTER->div = output_timer_prescaler; - IC_TIMER_REGISTER->pr = 95; - out_put = 1; - IC_TIMER_REGISTER->swevt_bit.ovfswtr = TRUE; + IC_TIMER_REGISTER->div = output_timer_prescaler; + IC_TIMER_REGISTER->pr = 95; + out_put = 1; + IC_TIMER_REGISTER->swevt_bit.ovfswtr = TRUE; } void changeToInput() { - gpio_mode_QUICK(INPUT_PIN_PORT, GPIO_MODE_INPUT, GPIO_PULL_NONE, INPUT_PIN); - INPUT_DMA_CHANNEL->ctrl |= DMA_DIR_PERIPHERAL_TO_MEMORY; - GPIOB->scr = INPUT_PIN; - IC_TIMER_REGISTER->cval = 0; - tmr_reset(IC_TIMER_REGISTER); - #ifdef USE_TIMER_3_CHANNEL_1 - IC_TIMER_REGISTER->cm1 = 0x41; - IC_TIMER_REGISTER->cctrl = 0xB; + gpio_mode_QUICK(INPUT_PIN_PORT, GPIO_MODE_INPUT, GPIO_PULL_NONE, INPUT_PIN); + INPUT_DMA_CHANNEL->ctrl |= DMA_DIR_PERIPHERAL_TO_MEMORY; + GPIOB->scr = INPUT_PIN; + IC_TIMER_REGISTER->cval = 0; + tmr_reset(IC_TIMER_REGISTER); +#ifdef USE_TIMER_3_CHANNEL_1 + IC_TIMER_REGISTER->cm1 = 0x41; + IC_TIMER_REGISTER->cctrl = 0xB; #endif #ifdef USE_TIMER_2_CHANNEL_3 - IC_TIMER_REGISTER->cm2 = 0x41; - IC_TIMER_REGISTER->cctrl = 0xB00; + IC_TIMER_REGISTER->cm2 = 0x41; + IC_TIMER_REGISTER->cctrl = 0xB00; #endif - IC_TIMER_REGISTER->div = ic_timer_prescaler; - IC_TIMER_REGISTER->pr = 0xFFFF; + IC_TIMER_REGISTER->div = ic_timer_prescaler; + IC_TIMER_REGISTER->pr = 0xFFFF; - IC_TIMER_REGISTER->swevt_bit.ovfswtr = TRUE; - out_put = 0; + IC_TIMER_REGISTER->swevt_bit.ovfswtr = TRUE; + out_put = 0; } void receiveDshotDma() { - changeToInput(); + changeToInput(); #ifdef USE_TIMER_3_CHANNEL_1 - INPUT_DMA_CHANNEL->paddr = (uint32_t)&IC_TIMER_REGISTER->c1dt; - IC_TIMER_REGISTER->iden |= TMR_C1_DMA_REQUEST; + INPUT_DMA_CHANNEL->paddr = (uint32_t)&IC_TIMER_REGISTER->c1dt; + IC_TIMER_REGISTER->iden |= TMR_C1_DMA_REQUEST; #endif #ifdef USE_TIMER_2_CHANNEL_3 - INPUT_DMA_CHANNEL->paddr = (uint32_t)&IC_TIMER_REGISTER->c3dt; - IC_TIMER_REGISTER->iden |= TMR_C3_DMA_REQUEST; + INPUT_DMA_CHANNEL->paddr = (uint32_t)&IC_TIMER_REGISTER->c3dt; + IC_TIMER_REGISTER->iden |= TMR_C3_DMA_REQUEST; #endif - INPUT_DMA_CHANNEL->maddr = (uint32_t)&dma_buffer; - INPUT_DMA_CHANNEL->dtcnt = buffersize; - IC_TIMER_REGISTER->ctrl1_bit.tmren = TRUE; - INPUT_DMA_CHANNEL->ctrl = 0x0000098b; + INPUT_DMA_CHANNEL->maddr = (uint32_t)&dma_buffer; + INPUT_DMA_CHANNEL->dtcnt = buffersize; + IC_TIMER_REGISTER->ctrl1_bit.tmren = TRUE; + INPUT_DMA_CHANNEL->ctrl = 0x0000098b; } void sendDshotDma() { - changeToOutput(); + changeToOutput(); #ifdef USE_TIMER_3_CHANNEL_1 - INPUT_DMA_CHANNEL->paddr = (uint32_t)&IC_TIMER_REGISTER->c1dt; - IC_TIMER_REGISTER->iden |= TMR_C1_DMA_REQUEST; + INPUT_DMA_CHANNEL->paddr = (uint32_t)&IC_TIMER_REGISTER->c1dt; + IC_TIMER_REGISTER->iden |= TMR_C1_DMA_REQUEST; #endif #ifdef USE_TIMER_2_CHANNEL_3 - INPUT_DMA_CHANNEL->paddr = (uint32_t)&IC_TIMER_REGISTER->c3dt; - IC_TIMER_REGISTER->iden |= TMR_C3_DMA_REQUEST; + INPUT_DMA_CHANNEL->paddr = (uint32_t)&IC_TIMER_REGISTER->c3dt; + IC_TIMER_REGISTER->iden |= TMR_C3_DMA_REQUEST; #endif - INPUT_DMA_CHANNEL->maddr = (uint32_t)&gcr; - INPUT_DMA_CHANNEL->dtcnt = 23 + buffer_padding; - INPUT_DMA_CHANNEL->ctrl |= DMA_FDT_INT; - INPUT_DMA_CHANNEL->ctrl |= DMA_DTERR_INT; - INPUT_DMA_CHANNEL->ctrl_bit.chen = TRUE; - IC_TIMER_REGISTER->brk_bit.oen = TRUE; - IC_TIMER_REGISTER->ctrl1_bit.tmren = TRUE; - gpio_mode_QUICK(INPUT_PIN_PORT, GPIO_MODE_MUX, GPIO_PULL_NONE, INPUT_PIN); + INPUT_DMA_CHANNEL->maddr = (uint32_t)&gcr; + INPUT_DMA_CHANNEL->dtcnt = 23 + buffer_padding; + INPUT_DMA_CHANNEL->ctrl |= DMA_FDT_INT; + INPUT_DMA_CHANNEL->ctrl |= DMA_DTERR_INT; + INPUT_DMA_CHANNEL->ctrl_bit.chen = TRUE; + IC_TIMER_REGISTER->brk_bit.oen = TRUE; + IC_TIMER_REGISTER->ctrl1_bit.tmren = TRUE; + gpio_mode_QUICK(INPUT_PIN_PORT, GPIO_MODE_MUX, GPIO_PULL_NONE, INPUT_PIN); } -uint8_t getInputPinState() { return (INPUT_PIN_PORT->idt & INPUT_PIN); } +uint8_t getInputPinState() +{ + return (INPUT_PIN_PORT->idt & INPUT_PIN); +} void setInputPolarityRising() { - IC_TIMER_REGISTER->cctrl_bit.c1p = TMR_INPUT_RISING_EDGE; + IC_TIMER_REGISTER->cctrl_bit.c1p = TMR_INPUT_RISING_EDGE; } void setInputPullDown() { - gpio_mode_QUICK(INPUT_PIN_PORT, GPIO_MODE_INPUT, GPIO_PULL_DOWN, INPUT_PIN); + gpio_mode_QUICK(INPUT_PIN_PORT, GPIO_MODE_INPUT, GPIO_PULL_DOWN, INPUT_PIN); } void setInputPullUp() { - gpio_mode_QUICK(INPUT_PIN_PORT, GPIO_MODE_INPUT, GPIO_PULL_UP, INPUT_PIN); + gpio_mode_QUICK(INPUT_PIN_PORT, GPIO_MODE_INPUT, GPIO_PULL_UP, INPUT_PIN); } -void enableHalfTransferInt() { INPUT_DMA_CHANNEL->ctrl |= DMA_HDT_INT; } +void enableHalfTransferInt() +{ + INPUT_DMA_CHANNEL->ctrl |= DMA_HDT_INT; +} void setInputPullNone() { - gpio_mode_QUICK(INPUT_PIN_PORT, GPIO_MODE_MUX, GPIO_PULL_NONE, INPUT_PIN); + gpio_mode_QUICK(INPUT_PIN_PORT, GPIO_MODE_MUX, GPIO_PULL_NONE, INPUT_PIN); } diff --git a/Mcu/f415/Src/at32f415_it.c b/Mcu/f415/Src/at32f415_it.c index b0462bd7..ad7551d4 100644 --- a/Mcu/f415/Src/at32f415_it.c +++ b/Mcu/f415/Src/at32f415_it.c @@ -42,9 +42,9 @@ void NMI_Handler(void) { } */ void HardFault_Handler(void) { - /* Go to infinite loop when Hard Fault exception occurs */ - while (1) { - } + /* Go to infinite loop when Hard Fault exception occurs */ + while (1) { + } } /** @@ -54,9 +54,9 @@ void HardFault_Handler(void) */ void MemManage_Handler(void) { - /* Go to infinite loop when Memory Manage exception occurs */ - while (1) { - } + /* Go to infinite loop when Memory Manage exception occurs */ + while (1) { + } } /** @@ -66,9 +66,9 @@ void MemManage_Handler(void) */ void BusFault_Handler(void) { - /* Go to infinite loop when Bus Fault exception occurs */ - while (1) { - } + /* Go to infinite loop when Bus Fault exception occurs */ + while (1) { + } } /** @@ -78,9 +78,9 @@ void BusFault_Handler(void) */ void UsageFault_Handler(void) { - /* Go to infinite loop when Usage Fault exception occurs */ - while (1) { - } + /* Go to infinite loop when Usage Fault exception occurs */ + while (1) { + } } /** @@ -113,49 +113,49 @@ void SysTick_Handler(void) { } void DMA1_Channel1_IRQHandler(void) { - if (dma_flag_get(DMA1_FDT1_FLAG) == SET) { - DMA1->clr = DMA1_GL1_FLAG; + if (dma_flag_get(DMA1_FDT1_FLAG) == SET) { + DMA1->clr = DMA1_GL1_FLAG; #ifdef USE_ADC - ADC_DMA_Callback(); + ADC_DMA_Callback(); #endif - if (dma_flag_get(DMA1_DTERR1_FLAG) == SET) { - DMA1->clr = DMA1_GL1_FLAG; - } + if (dma_flag_get(DMA1_DTERR1_FLAG) == SET) { + DMA1->clr = DMA1_GL1_FLAG; } + } } void DMA1_Channel4_IRQHandler(void) { - if (dma_flag_get(DMA1_FDT4_FLAG) == SET) { - DMA1->clr = DMA1_GL4_FLAG; - DMA1_CHANNEL4->ctrl_bit.chen = FALSE; - } - if (dma_flag_get(DMA1_DTERR2_FLAG) == SET) { - DMA1->clr = DMA1_GL4_FLAG; - DMA1_CHANNEL4->ctrl_bit.chen = FALSE; - } + if (dma_flag_get(DMA1_FDT4_FLAG) == SET) { + DMA1->clr = DMA1_GL4_FLAG; + DMA1_CHANNEL4->ctrl_bit.chen = FALSE; + } + if (dma_flag_get(DMA1_DTERR2_FLAG) == SET) { + DMA1->clr = DMA1_GL4_FLAG; + DMA1_CHANNEL4->ctrl_bit.chen = FALSE; + } } void DMA1_Channel6_IRQHandler(void) { - if (dma_flag_get(DMA1_HDT6_FLAG) == SET) { - if (servoPwm) { - IC_TIMER_REGISTER->cctrl_bit.c1p = TMR_INPUT_FALLING_EDGE; - DMA1->clr = DMA1_HDT6_FLAG; - } - } - - if (dma_flag_get(DMA1_FDT6_FLAG) == SET) { - DMA1->clr = DMA1_GL6_FLAG; - INPUT_DMA_CHANNEL->ctrl_bit.chen = FALSE; - transfercomplete(); - EXINT->swtrg = EXINT_LINE_15; - } - if (dma_flag_get(DMA1_DTERR6_FLAG) == SET) { - DMA1->clr = DMA1_GL6_FLAG; - INPUT_DMA_CHANNEL->ctrl_bit.chen = FALSE; - transfercomplete(); + if (dma_flag_get(DMA1_HDT6_FLAG) == SET) { + if (servoPwm) { + IC_TIMER_REGISTER->cctrl_bit.c1p = TMR_INPUT_FALLING_EDGE; + DMA1->clr = DMA1_HDT6_FLAG; } + } + + if (dma_flag_get(DMA1_FDT6_FLAG) == SET) { + DMA1->clr = DMA1_GL6_FLAG; + INPUT_DMA_CHANNEL->ctrl_bit.chen = FALSE; + transfercomplete(); + EXINT->swtrg = EXINT_LINE_15; + } + if (dma_flag_get(DMA1_DTERR6_FLAG) == SET) { + DMA1->clr = DMA1_GL6_FLAG; + INPUT_DMA_CHANNEL->ctrl_bit.chen = FALSE; + transfercomplete(); + } } /** @@ -164,11 +164,11 @@ void DMA1_Channel6_IRQHandler(void) */ void CMP1_IRQHandler(void) { - if ((EXINT->intsts & EXTI_LINE) != (uint32_t)RESET) { - // EXTI->PND = EXTI_LINE; - EXINT->intsts = EXTI_LINE; - interruptRoutine(); - } + if ((EXINT->intsts & EXTI_LINE) != (uint32_t)RESET) { + // EXTI->PND = EXTI_LINE; + EXINT->intsts = EXTI_LINE; + interruptRoutine(); + } } /** @@ -176,18 +176,18 @@ void CMP1_IRQHandler(void) */ void TMR1_BRK_TMR9_IRQHandler(void) { - /* USER CODE BEGIN TIM6_DAC_IRQn 0 */ - // TIM6->DIER &= ~(0x1UL << (0U)); - TMR9->ists = (uint16_t)~TMR_OVF_FLAG; - TMR1->ists = 0x00; - // timer_interrupt_flag_clear(TIMER13, TIMER_INT_FLAG_UP); - tenKhzRoutine(); + /* USER CODE BEGIN TIM6_DAC_IRQn 0 */ + // TIM6->DIER &= ~(0x1UL << (0U)); + TMR9->ists = (uint16_t)~TMR_OVF_FLAG; + TMR1->ists = 0x00; + // timer_interrupt_flag_clear(TIMER13, TIMER_INT_FLAG_UP); + tenKhzRoutine(); - /* USER CODE END TIM6_DAC_IRQn 0 */ + /* USER CODE END TIM6_DAC_IRQn 0 */ - /* USER CODE BEGIN TIM6_DAC_IRQn 1 */ + /* USER CODE BEGIN TIM6_DAC_IRQn 1 */ - /* USER CODE END TIM6_DAC_IRQn 1 */ + /* USER CODE END TIM6_DAC_IRQn 1 */ } /** @@ -195,27 +195,27 @@ void TMR1_BRK_TMR9_IRQHandler(void) */ void TMR1_TRG_HALL_TMR11_IRQHandler(void) { - /* USER CODE BEGIN TIM14_IRQn 0 */ - // if(LL_TIM_IsActiveFlag_UPDATE(TIM14) == 1) - // { - // timer_interrupt_flag_clear(TIMER15, TIMER_INT_FLAG_UP); + /* USER CODE BEGIN TIM14_IRQn 0 */ + // if(LL_TIM_IsActiveFlag_UPDATE(TIM14) == 1) + // { + // timer_interrupt_flag_clear(TIMER15, TIMER_INT_FLAG_UP); - TMR11->ists = 0x00; - TMR1->ists = 0x00; - PeriodElapsedCallback(); + TMR11->ists = 0x00; + TMR1->ists = 0x00; + PeriodElapsedCallback(); - // } + // } - /* USER CODE END TIM14_IRQn 0 */ - /* USER CODE BEGIN TIM14_IRQn 1 */ + /* USER CODE END TIM14_IRQn 0 */ + /* USER CODE BEGIN TIM14_IRQn 1 */ - /* USER CODE END TIM14_IRQn 1 */ + /* USER CODE END TIM14_IRQn 1 */ } void TMR1_OVF_TMR10_IRQHandler(void) { - TMR10->ists = (uint16_t)~TMR_OVF_FLAG; - TMR10->ists = (uint16_t)~TMR_C1_FLAG; + TMR10->ists = (uint16_t)~TMR_OVF_FLAG; + TMR10->ists = (uint16_t)~TMR_C1_FLAG; } /** @@ -224,22 +224,22 @@ void TMR1_OVF_TMR10_IRQHandler(void) */ void USART1_IRQHandler(void) { - /* USER CODE BEGIN USART1_IRQn 0 */ + /* USER CODE BEGIN USART1_IRQn 0 */ - /* USER CODE END USART1_IRQn 0 */ - /* USER CODE BEGIN USART1_IRQn 1 */ + /* USER CODE END USART1_IRQn 0 */ + /* USER CODE BEGIN USART1_IRQn 1 */ - /* USER CODE END USART1_IRQn 1 */ + /* USER CODE END USART1_IRQn 1 */ } void TMR3_GLOBAL_IRQHandler(void) { - if ((TMR3->ists & TMR_C1_FLAG) != (uint16_t)RESET) { - TMR3->ists = (uint16_t)~TMR_C1_FLAG; - } - if ((TMR3->ists & TMR_OVF_FLAG) != (uint16_t)RESET) { - TMR3->ists = (uint16_t)~TMR_OVF_FLAG; - } + if ((TMR3->ists & TMR_C1_FLAG) != (uint16_t)RESET) { + TMR3->ists = (uint16_t)~TMR_C1_FLAG; + } + if ((TMR3->ists & TMR_OVF_FLAG) != (uint16_t)RESET) { + TMR3->ists = (uint16_t)~TMR_OVF_FLAG; + } } // void DMA_Channel0_IRQHandler(void) // ADC @@ -265,10 +265,10 @@ void TMR3_GLOBAL_IRQHandler(void) void EXINT15_10_IRQHandler(void) { - if ((EXINT->intsts & EXINT_LINE_15) != (uint32_t)RESET) { - EXINT->intsts = EXINT_LINE_15; - processDshot(); - } + if ((EXINT->intsts & EXINT_LINE_15) != (uint32_t)RESET) { + EXINT->intsts = EXINT_LINE_15; + processDshot(); + } } /******************************************************************************/ diff --git a/Mcu/f415/Src/comparator.c b/Mcu/f415/Src/comparator.c index 8ac1cbc5..bfef3d40 100644 --- a/Mcu/f415/Src/comparator.c +++ b/Mcu/f415/Src/comparator.c @@ -9,62 +9,65 @@ #include "targets.h" -uint8_t getCompOutputLevel() { return CMP->ctrlsts1_bit.cmp1value; } +uint8_t getCompOutputLevel() +{ + return CMP->ctrlsts1_bit.cmp1value; +} void maskPhaseInterrupts() { - // EXTI->IMR &= (0 << 21); - // LL_EXTI_ClearFlag_0_31(EXTI_LINE); + // EXTI->IMR &= (0 << 21); + // LL_EXTI_ClearFlag_0_31(EXTI_LINE); - // EXTI_INTEN &= ~(uint32_t)EXTI_LINE; - EXINT->inten &= ~(uint32_t)EXTI_LINE; + // EXTI_INTEN &= ~(uint32_t)EXTI_LINE; + EXINT->inten &= ~(uint32_t)EXTI_LINE; - // EXTI_PD = (uint32_t)EXTI_LINE; - EXINT->intsts = (uint32_t)EXTI_LINE; + // EXTI_PD = (uint32_t)EXTI_LINE; + EXINT->intsts = (uint32_t)EXTI_LINE; } void enableCompInterrupts() { - // EXTI->IMR |= (1 << 21); - // EXTI_INTEN |= (uint32_t)EXTI_LINE; - EXINT->inten |= (uint32_t)EXTI_LINE; - // EXTI->PND = EXTI_LINE; + // EXTI->IMR |= (1 << 21); + // EXTI_INTEN |= (uint32_t)EXTI_LINE; + EXINT->inten |= (uint32_t)EXTI_LINE; + // EXTI->PND = EXTI_LINE; } void changeCompInput() { - // TIM3->CNT = 0; - // HAL_COMP_Stop_IT(&hcomp1); // done in comparator interrupt - // routine + // TIM3->CNT = 0; + // HAL_COMP_Stop_IT(&hcomp1); // done in comparator interrupt + // routine - if (step == 1 || step == 4) { // c floating - // COMP->CSR = PHASE_C_COMP; - // cmp_mode_init(CMP_HIGHSPEED, PHASE_C_COMP, CMP_HYSTERESIS_NO); - // COMP->CTRLSTS1 |= PHASE_C_COMP; - CMP->ctrlsts1 = PHASE_C_COMP; - } - if (step == 2 || step == 5) { // a floating - // COMP->CSR = PHASE_A_COMP; - // cmp_mode_init(CMP_HIGHSPEED, PHASE_A_COMP, CMP_HYSTERESIS_NO); - // COMP->CTRLSTS1 |= PHASE_A_COMP; - CMP->ctrlsts1 = PHASE_A_COMP; - } - if (step == 3 || step == 6) { // b floating - // COMP->CSR = PHASE_B_COMP; - // cmp_mode_init(CMP_HIGHSPEED, PHASE_B_COMP, CMP_HYSTERESIS_NO); - // COMP->CTRLSTS1 |= PHASE_B_COMP; - CMP->ctrlsts1 = PHASE_B_COMP; - } - if (rising) { - // EXTI->RTSR = 0x0; - // EXTI->FTSR = 0x200000; - EXINT->polcfg1 &= ~(uint32_t)EXTI_LINE; - EXINT->polcfg2 |= (uint32_t)EXTI_LINE; - } else { - // falling bemf - // EXTI->FTSR = 0x0; - // EXTI->RTSR = 0x200000; - EXINT->polcfg1 |= (uint32_t)EXTI_LINE; - EXINT->polcfg2 &= ~(uint32_t)EXTI_LINE; - } + if (step == 1 || step == 4) { // c floating + // COMP->CSR = PHASE_C_COMP; + // cmp_mode_init(CMP_HIGHSPEED, PHASE_C_COMP, CMP_HYSTERESIS_NO); + // COMP->CTRLSTS1 |= PHASE_C_COMP; + CMP->ctrlsts1 = PHASE_C_COMP; + } + if (step == 2 || step == 5) { // a floating + // COMP->CSR = PHASE_A_COMP; + // cmp_mode_init(CMP_HIGHSPEED, PHASE_A_COMP, CMP_HYSTERESIS_NO); + // COMP->CTRLSTS1 |= PHASE_A_COMP; + CMP->ctrlsts1 = PHASE_A_COMP; + } + if (step == 3 || step == 6) { // b floating + // COMP->CSR = PHASE_B_COMP; + // cmp_mode_init(CMP_HIGHSPEED, PHASE_B_COMP, CMP_HYSTERESIS_NO); + // COMP->CTRLSTS1 |= PHASE_B_COMP; + CMP->ctrlsts1 = PHASE_B_COMP; + } + if (rising) { + // EXTI->RTSR = 0x0; + // EXTI->FTSR = 0x200000; + EXINT->polcfg1 &= ~(uint32_t)EXTI_LINE; + EXINT->polcfg2 |= (uint32_t)EXTI_LINE; + } else { + // falling bemf + // EXTI->FTSR = 0x0; + // EXTI->RTSR = 0x200000; + EXINT->polcfg1 |= (uint32_t)EXTI_LINE; + EXINT->polcfg2 &= ~(uint32_t)EXTI_LINE; + } } diff --git a/Mcu/f415/Src/eeprom.c b/Mcu/f415/Src/eeprom.c index 21d69cb3..751c9e89 100644 --- a/Mcu/f415/Src/eeprom.c +++ b/Mcu/f415/Src/eeprom.c @@ -16,45 +16,45 @@ */ static inline uint32_t sector_size() { - const uint16_t *F_SIZE = (const uint16_t *)0x1FFFF7E0; - if (*F_SIZE <= 128) { - // 1k sectors for 128k flash or less - return 1024; - } - // 256k flash is 2k sectors - return 2048; + const uint16_t *F_SIZE = (const uint16_t *)0x1FFFF7E0; + if (*F_SIZE <= 128) { + // 1k sectors for 128k flash or less + return 1024; + } + // 256k flash is 2k sectors + return 2048; } void save_flash_nolib(uint8_t* data, int length, uint32_t add) { - if ((add & 0x3) != 0 || (length & 0x3) != 0) { - return; - } - /* - we need the data to be 32 bit aligned - */ - const uint32_t word_length = length / 4; - - // unlock flash - flash_unlock(); - - // erase page if address even divisable by sector size - if ((add % sector_size()) == 0) { - flash_sector_erase(add); - } - - uint32_t index = 0; - while (index < word_length) { - uint32_t word; - memcpy(&word, &data[index*4], sizeof(word)); - flash_word_program(add + (index * 4), word); - flash_flag_clear(FLASH_PROGRAM_ERROR | FLASH_EPP_ERROR | FLASH_OPERATE_DONE); - index++; - } - flash_lock(); + if ((add & 0x3) != 0 || (length & 0x3) != 0) { + return; + } + /* + we need the data to be 32 bit aligned + */ + const uint32_t word_length = length / 4; + + // unlock flash + flash_unlock(); + + // erase page if address even divisable by sector size + if ((add % sector_size()) == 0) { + flash_sector_erase(add); + } + + uint32_t index = 0; + while (index < word_length) { + uint32_t word; + memcpy(&word, &data[index*4], sizeof(word)); + flash_word_program(add + (index * 4), word); + flash_flag_clear(FLASH_PROGRAM_ERROR | FLASH_EPP_ERROR | FLASH_OPERATE_DONE); + index++; + } + flash_lock(); } void read_flash_bin(uint8_t* data, uint32_t add, int out_buff_len) { - memcpy(data, (void*)add, out_buff_len); + memcpy(data, (void*)add, out_buff_len); } diff --git a/Mcu/f415/Src/peripherals.c b/Mcu/f415/Src/peripherals.c index 48473b8c..f54b2b01 100644 --- a/Mcu/f415/Src/peripherals.c +++ b/Mcu/f415/Src/peripherals.c @@ -20,208 +20,211 @@ void initCorePeripherals(void) { - system_clock_config(); - // LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_SYSCFG); - // LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_PWR); - - // fmc_wscnt_set(2); - // fmc_prefetch_enable(); - MX_GPIO_Init(); - MX_DMA_Init(); - TIM1_Init(); - TIM4_Init(); - TIM9_Init(); - AT_COMP_Init(); - TIM11_Init(); - TIM10_Init(); - - UN_TIM_Init(); + system_clock_config(); + // LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_SYSCFG); + // LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_PWR); + + // fmc_wscnt_set(2); + // fmc_prefetch_enable(); + MX_GPIO_Init(); + MX_DMA_Init(); + TIM1_Init(); + TIM4_Init(); + TIM9_Init(); + AT_COMP_Init(); + TIM11_Init(); + TIM10_Init(); + + UN_TIM_Init(); #ifdef USE_SERIAL_TELEMETRY - telem_UART_Init(); + telem_UART_Init(); #endif } // COMP_InitType COMP_InitStructure; // EXTI_InitType EXTI_InitStructure; -void initAfterJump(void) { __enable_irq(); } +void initAfterJump(void) +{ + __enable_irq(); +} void system_clock_config(void) { - // flash_psr_set(FLASH_WAIT_CYCLE_3); - // crm_reset(); - // crm_clock_source_enable(CRM_CLOCK_SOURCE_HICK, TRUE); - // while(crm_flag_get(CRM_HICK_STABLE_FLAG) != SET) - // { - // } - // crm_pll_config(CRM_PLL_SOURCE_HICK, CRM_PLL_MULT_30); - // crm_clock_source_enable(CRM_CLOCK_SOURCE_PLL, TRUE); - // while(crm_flag_get(CRM_PLL_STABLE_FLAG) != SET) - // { - // } - // crm_ahb_div_set(CRM_AHB_DIV_1); - // crm_apb2_div_set(CRM_APB2_DIV_1); - // crm_apb1_div_set(CRM_APB1_DIV_1); - // crm_auto_step_mode_enable(TRUE); - // crm_sysclk_switch(CRM_SCLK_PLL); - // while(crm_sysclk_switch_status_get() != CRM_SCLK_PLL) - // { - // } - // crm_auto_step_mode_enable(FALSE); - // system_core_clock_update(); - /* config flash psr register */ - - /// 144 mhz hick setup - - flash_psr_set(FLASH_WAIT_CYCLE_4); - crm_reset(); - - crm_clock_source_enable(CRM_CLOCK_SOURCE_HICK, TRUE); - while (crm_flag_get(CRM_HICK_STABLE_FLAG) != SET) { - } - crm_pll_config(CRM_PLL_SOURCE_HICK, CRM_PLL_MULT_36); - crm_clock_source_enable(CRM_CLOCK_SOURCE_PLL, TRUE); - while (crm_flag_get(CRM_PLL_STABLE_FLAG) != SET) { - } - crm_ahb_div_set(CRM_AHB_DIV_1); - crm_apb2_div_set(CRM_APB2_DIV_2); - crm_apb1_div_set(CRM_APB1_DIV_2); - crm_auto_step_mode_enable(TRUE); - crm_sysclk_switch(CRM_SCLK_PLL); - while (crm_sysclk_switch_status_get() != CRM_SCLK_PLL) { - } - crm_auto_step_mode_enable(FALSE); - system_core_clock_update(); + // flash_psr_set(FLASH_WAIT_CYCLE_3); + // crm_reset(); + // crm_clock_source_enable(CRM_CLOCK_SOURCE_HICK, TRUE); + // while(crm_flag_get(CRM_HICK_STABLE_FLAG) != SET) + // { + // } + // crm_pll_config(CRM_PLL_SOURCE_HICK, CRM_PLL_MULT_30); + // crm_clock_source_enable(CRM_CLOCK_SOURCE_PLL, TRUE); + // while(crm_flag_get(CRM_PLL_STABLE_FLAG) != SET) + // { + // } + // crm_ahb_div_set(CRM_AHB_DIV_1); + // crm_apb2_div_set(CRM_APB2_DIV_1); + // crm_apb1_div_set(CRM_APB1_DIV_1); + // crm_auto_step_mode_enable(TRUE); + // crm_sysclk_switch(CRM_SCLK_PLL); + // while(crm_sysclk_switch_status_get() != CRM_SCLK_PLL) + // { + // } + // crm_auto_step_mode_enable(FALSE); + // system_core_clock_update(); + /* config flash psr register */ + + /// 144 mhz hick setup + + flash_psr_set(FLASH_WAIT_CYCLE_4); + crm_reset(); + + crm_clock_source_enable(CRM_CLOCK_SOURCE_HICK, TRUE); + while (crm_flag_get(CRM_HICK_STABLE_FLAG) != SET) { + } + crm_pll_config(CRM_PLL_SOURCE_HICK, CRM_PLL_MULT_36); + crm_clock_source_enable(CRM_CLOCK_SOURCE_PLL, TRUE); + while (crm_flag_get(CRM_PLL_STABLE_FLAG) != SET) { + } + crm_ahb_div_set(CRM_AHB_DIV_1); + crm_apb2_div_set(CRM_APB2_DIV_2); + crm_apb1_div_set(CRM_APB1_DIV_2); + crm_auto_step_mode_enable(TRUE); + crm_sysclk_switch(CRM_SCLK_PLL); + while (crm_sysclk_switch_status_get() != CRM_SCLK_PLL) { + } + crm_auto_step_mode_enable(FALSE); + system_core_clock_update(); } void AT_COMP_Init(void) { - // COMP_StructInit(&COMP_InitStructure); - - crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE); - crm_periph_clock_enable(CRM_CMP_PERIPH_CLOCK, TRUE); - - /* configure PA1 as comparator input */ - gpio_mode_QUICK(GPIOA, GPIO_MODE_ANALOG, GPIO_PULL_NONE, GPIO_PINS_1); - gpio_mode_QUICK(GPIOA, GPIO_MODE_ANALOG, GPIO_PULL_NONE, GPIO_PINS_5); - /* configure comparator channel0 */ - // cmp_mode_init(CMP_HIGHSPEED, CMP_PA5, CMP_HYSTERESIS_NO); - - // COMP_InitStructure.COMP_INMInput = COMP_INMInput_IN3; - // COMP_InitStructure.COMP_Output = COMP_Output_None; - // COMP_InitStructure.COMP_OutPolarity = - // COMP_OutPolarity_NonInverted; - // COMP_InitStructure.COMP_Hysteresis = COMP_Hysteresis_No; - // COMP_InitStructure.COMP_Mode = COMP_Mode_Fast; - // COMP_Init(COMP1_Selection, &COMP_InitStructure); - // cmp_enable(); - // delay_1ms(1); - - // cmp_output_init(CMP_OUTPUT_NONE, CMP_OUTPUT_POLARITY_NOINVERTED); - - /* initialize exti line21 */ - // EXTI_StructInit(&EXTI_InitStructure); - // EXTI_InitStructure.EXTI_Line = EXTI_LINE; - // EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt; - // EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising; - // EXTI_InitStructure.EXTI_LineEnable = ENABLE; - // EXTI_Init(&EXTI_InitStructure); - // EXTI_ClearFlag(EXTI_LINE); - /* configure ADC_CMP NVIC */ - // nvic_irq_enable(ADC_COMP_IRQHandler, 0); - // NVIC_InitType NVIC_InitStructure; - - // NVIC_InitStructure.NVIC_IRQChannel = ADC_COMP_IRQn; - // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; - // NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - // NVIC_Init(&NVIC_InitStructure); - - NVIC_SetPriority(CMP1_IRQn, 0); - NVIC_EnableIRQ(CMP1_IRQn); - cmp_enable(CMP1_SELECTION, TRUE); + // COMP_StructInit(&COMP_InitStructure); + + crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_CMP_PERIPH_CLOCK, TRUE); + + /* configure PA1 as comparator input */ + gpio_mode_QUICK(GPIOA, GPIO_MODE_ANALOG, GPIO_PULL_NONE, GPIO_PINS_1); + gpio_mode_QUICK(GPIOA, GPIO_MODE_ANALOG, GPIO_PULL_NONE, GPIO_PINS_5); + /* configure comparator channel0 */ + // cmp_mode_init(CMP_HIGHSPEED, CMP_PA5, CMP_HYSTERESIS_NO); + + // COMP_InitStructure.COMP_INMInput = COMP_INMInput_IN3; + // COMP_InitStructure.COMP_Output = COMP_Output_None; + // COMP_InitStructure.COMP_OutPolarity = + // COMP_OutPolarity_NonInverted; + // COMP_InitStructure.COMP_Hysteresis = COMP_Hysteresis_No; + // COMP_InitStructure.COMP_Mode = COMP_Mode_Fast; + // COMP_Init(COMP1_Selection, &COMP_InitStructure); + // cmp_enable(); + // delay_1ms(1); + + // cmp_output_init(CMP_OUTPUT_NONE, CMP_OUTPUT_POLARITY_NOINVERTED); + + /* initialize exti line21 */ + // EXTI_StructInit(&EXTI_InitStructure); + // EXTI_InitStructure.EXTI_Line = EXTI_LINE; + // EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt; + // EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising; + // EXTI_InitStructure.EXTI_LineEnable = ENABLE; + // EXTI_Init(&EXTI_InitStructure); + // EXTI_ClearFlag(EXTI_LINE); + /* configure ADC_CMP NVIC */ + // nvic_irq_enable(ADC_COMP_IRQHandler, 0); + // NVIC_InitType NVIC_InitStructure; + + // NVIC_InitStructure.NVIC_IRQChannel = ADC_COMP_IRQn; + // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; + // NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + // NVIC_Init(&NVIC_InitStructure); + + NVIC_SetPriority(CMP1_IRQn, 0); + NVIC_EnableIRQ(CMP1_IRQn); + cmp_enable(CMP1_SELECTION, TRUE); } void MX_IWDG_Init(void) { - // fwdgt_config(4000,FWDGT_PSC_DIV16); - // fwdgt_enable(); - - WDT->cmd = WDT_CMD_UNLOCK; - WDT->cmd = WDT_CMD_ENABLE; - WDT->div = WDT_CLK_DIV_64; - WDT->rld = 4000; - WDT->cmd = WDT_CMD_RELOAD; + // fwdgt_config(4000,FWDGT_PSC_DIV16); + // fwdgt_enable(); + + WDT->cmd = WDT_CMD_UNLOCK; + WDT->cmd = WDT_CMD_ENABLE; + WDT->div = WDT_CLK_DIV_64; + WDT->rld = 4000; + WDT->cmd = WDT_CMD_RELOAD; } void TIM1_Init(void) { - crm_periph_clock_enable(CRM_IOMUX_PERIPH_CLOCK, TRUE); - crm_periph_clock_enable(CRM_TMR1_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_IOMUX_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_TMR1_PERIPH_CLOCK, TRUE); - gpio_pin_remap_config(TMR1_GMUX_0001, TRUE); + gpio_pin_remap_config(TMR1_GMUX_0001, TRUE); - TMR1->pr = TIM1_AUTORELOAD; - TMR1->div = 0; + TMR1->pr = TIM1_AUTORELOAD; + TMR1->div = 0; - TMR1->cm1 = 0x6868; // Channel 1 and 2 in PWM output mode - TMR1->cm2 = 0x68; // channel 3 in PWM output mode + TMR1->cm1 = 0x6868; // Channel 1 and 2 in PWM output mode + TMR1->cm2 = 0x68; // channel 3 in PWM output mode - tmr_output_channel_buffer_enable(TMR1, TMR_SELECT_CHANNEL_1, TRUE); - tmr_output_channel_buffer_enable(TMR1, TMR_SELECT_CHANNEL_2, TRUE); - tmr_output_channel_buffer_enable(TMR1, TMR_SELECT_CHANNEL_3, TRUE); + tmr_output_channel_buffer_enable(TMR1, TMR_SELECT_CHANNEL_1, TRUE); + tmr_output_channel_buffer_enable(TMR1, TMR_SELECT_CHANNEL_2, TRUE); + tmr_output_channel_buffer_enable(TMR1, TMR_SELECT_CHANNEL_3, TRUE); - tmr_period_buffer_enable(TMR1, TRUE); + tmr_period_buffer_enable(TMR1, TRUE); - TMR1->brk_bit.dtc = DEAD_TIME; - crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE); - crm_periph_clock_enable(CRM_GPIOB_PERIPH_CLOCK, TRUE); + TMR1->brk_bit.dtc = DEAD_TIME; + crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_GPIOB_PERIPH_CLOCK, TRUE); - /*configure PA8/PA9/PA10(TIMER0/CH0/CH1/CH2) as alternate function*/ - gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_A_GPIO_LOW); + /*configure PA8/PA9/PA10(TIMER0/CH0/CH1/CH2) as alternate function*/ + gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_A_GPIO_LOW); - gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_B_GPIO_LOW); + gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_B_GPIO_LOW); - gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_C_GPIO_LOW); + gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_C_GPIO_LOW); - /*configure PB13/PB14/PB15(TIMER0/CH0N/CH1N/CH2N) as alternate function*/ - gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_A_GPIO_HIGH); + /*configure PB13/PB14/PB15(TIMER0/CH0N/CH1N/CH2N) as alternate function*/ + gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_A_GPIO_HIGH); - gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_B_GPIO_HIGH); + gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_B_GPIO_HIGH); - gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_C_GPIO_HIGH); + gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_C_GPIO_HIGH); } void TIM4_Init(void) { - crm_periph_clock_enable(CRM_TMR4_PERIPH_CLOCK, TRUE); - TMR4->pr = 0xFFFF; - TMR4->div = 74; + crm_periph_clock_enable(CRM_TMR4_PERIPH_CLOCK, TRUE); + TMR4->pr = 0xFFFF; + TMR4->div = 74; } void TIM9_Init(void) { - crm_periph_clock_enable(CRM_TMR9_PERIPH_CLOCK, TRUE); - TMR9->pr = 1000000 / LOOP_FREQUENCY_HZ; - TMR9->div = 143; - NVIC_SetPriority(TMR1_BRK_TMR9_IRQn, 3); - NVIC_EnableIRQ(TMR1_BRK_TMR9_IRQn); + crm_periph_clock_enable(CRM_TMR9_PERIPH_CLOCK, TRUE); + TMR9->pr = 1000000 / LOOP_FREQUENCY_HZ; + TMR9->div = 143; + NVIC_SetPriority(TMR1_BRK_TMR9_IRQn, 3); + NVIC_EnableIRQ(TMR1_BRK_TMR9_IRQn); } void TIM11_Init(void) { - crm_periph_clock_enable(CRM_TMR11_PERIPH_CLOCK, TRUE); - TMR11->pr = 500; - TMR11->div = 75; - TMR11->ctrl1_bit.prben = TRUE; + crm_periph_clock_enable(CRM_TMR11_PERIPH_CLOCK, TRUE); + TMR11->pr = 500; + TMR11->div = 75; + TMR11->ctrl1_bit.prben = TRUE; - NVIC_SetPriority(TMR1_TRG_HALL_TMR11_IRQn, 0); - NVIC_EnableIRQ(TMR1_TRG_HALL_TMR11_IRQn); + NVIC_SetPriority(TMR1_TRG_HALL_TMR11_IRQn, 0); + NVIC_EnableIRQ(TMR1_TRG_HALL_TMR11_IRQn); } /* @@ -229,173 +232,182 @@ void TIM11_Init(void) */ void TIM10_Init(void) { - crm_periph_clock_enable(CRM_TMR10_PERIPH_CLOCK, TRUE); - TMR10->pr = 0xFFFF; - TMR10->div = 143; - TMR10->ctrl1_bit.prben = TRUE; + crm_periph_clock_enable(CRM_TMR10_PERIPH_CLOCK, TRUE); + TMR10->pr = 0xFFFF; + TMR10->div = 143; + TMR10->ctrl1_bit.prben = TRUE; } void MX_DMA_Init(void) { - crm_periph_clock_enable(CRM_DMA1_PERIPH_CLOCK, TRUE); - NVIC_SetPriority(DMA1_Channel6_IRQn, 1); - NVIC_EnableIRQ(DMA1_Channel6_IRQn); + crm_periph_clock_enable(CRM_DMA1_PERIPH_CLOCK, TRUE); + NVIC_SetPriority(DMA1_Channel6_IRQn, 1); + NVIC_EnableIRQ(DMA1_Channel6_IRQn); } void MX_GPIO_Init(void) { - // /* GPIO Ports Clock Enable */ - // LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); - // LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); - - // /**/ - // LL_GPIO_ResetOutputPin(GPIOA, LL_GPIO_PIN_15); - - // /**/ - // #ifdef USE_ALKAS_DEBUG_LED - // LL_GPIO_InitTypeDef GPIO_InitStruct = {0}; - - // GPIO_InitStruct.Pin = LL_GPIO_PIN_15; - // GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; - // GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - // GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - // GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - // LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - // #endif + // /* GPIO Ports Clock Enable */ + // LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); + // LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); + + // /**/ + // LL_GPIO_ResetOutputPin(GPIOA, LL_GPIO_PIN_15); + + // /**/ + // #ifdef USE_ALKAS_DEBUG_LED + // LL_GPIO_InitTypeDef GPIO_InitStruct = {0}; + + // GPIO_InitStruct.Pin = LL_GPIO_PIN_15; + // GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; + // GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + // GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + // GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + // LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + // #endif } void UN_TIM_Init(void) { - crm_periph_clock_enable(CRM_IOMUX_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_IOMUX_PERIPH_CLOCK, TRUE); #ifdef USE_TIMER_3_CHANNEL_1 - crm_periph_clock_enable(CRM_GPIOB_PERIPH_CLOCK, TRUE); - crm_periph_clock_enable(CRM_TMR3_PERIPH_CLOCK, TRUE); - gpio_pin_remap_config(SWJTAG_MUX_010, TRUE); - gpio_pin_remap_config(TMR3_MUX_10, TRUE); - dma_flexible_config(DMA1,FLEX_CHANNEL6,DMA_FLEXIBLE_TMR3_CH1); + crm_periph_clock_enable(CRM_GPIOB_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_TMR3_PERIPH_CLOCK, TRUE); + gpio_pin_remap_config(SWJTAG_MUX_010, TRUE); + gpio_pin_remap_config(TMR3_MUX_10, TRUE); + dma_flexible_config(DMA1,FLEX_CHANNEL6,DMA_FLEXIBLE_TMR3_CH1); #endif #ifdef USE_TIMER_2_CHANNEL_3 - crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE); - crm_periph_clock_enable(CRM_TMR2_PERIPH_CLOCK, TRUE); - gpio_pin_remap_config(SWJTAG_MUX_010, TRUE); - gpio_pin_remap_config(TMR2_MUX_01, TRUE); - dma_flexible_config(DMA1,FLEX_CHANNEL6,DMA_FLEXIBLE_TMR2_CH3); + crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_TMR2_PERIPH_CLOCK, TRUE); + gpio_pin_remap_config(SWJTAG_MUX_010, TRUE); + gpio_pin_remap_config(TMR2_MUX_01, TRUE); + dma_flexible_config(DMA1,FLEX_CHANNEL6,DMA_FLEXIBLE_TMR2_CH3); #endif - crm_periph_clock_enable(CRM_DMA1_PERIPH_CLOCK, TRUE); - gpio_mode_QUICK(INPUT_PIN_PORT, GPIO_MODE_INPUT, GPIO_PULL_NONE, INPUT_PIN); - INPUT_DMA_CHANNEL->ctrl = 0X98a; // PERIPHERAL HALF WORD, MEMORY WORD , - // MEMORY INC ENABLE , TC AND ERROR INTS - - NVIC_SetPriority(IC_DMA_IRQ_NAME, 1); - NVIC_EnableIRQ(IC_DMA_IRQ_NAME); - - IC_TIMER_REGISTER->pr = 0xFFFF; - IC_TIMER_REGISTER->div = 16; - IC_TIMER_REGISTER->ctrl1_bit.prben = TRUE; - gpio_mode_QUICK(INPUT_PIN_PORT, GPIO_MODE_INPUT, GPIO_PULL_NONE, INPUT_PIN); - IC_TIMER_REGISTER->ctrl1_bit.tmren = TRUE; + crm_periph_clock_enable(CRM_DMA1_PERIPH_CLOCK, TRUE); + gpio_mode_QUICK(INPUT_PIN_PORT, GPIO_MODE_INPUT, GPIO_PULL_NONE, INPUT_PIN); + INPUT_DMA_CHANNEL->ctrl = 0X98a; // PERIPHERAL HALF WORD, MEMORY WORD , + // MEMORY INC ENABLE , TC AND ERROR INTS + + NVIC_SetPriority(IC_DMA_IRQ_NAME, 1); + NVIC_EnableIRQ(IC_DMA_IRQ_NAME); + + IC_TIMER_REGISTER->pr = 0xFFFF; + IC_TIMER_REGISTER->div = 16; + IC_TIMER_REGISTER->ctrl1_bit.prben = TRUE; + gpio_mode_QUICK(INPUT_PIN_PORT, GPIO_MODE_INPUT, GPIO_PULL_NONE, INPUT_PIN); + IC_TIMER_REGISTER->ctrl1_bit.tmren = TRUE; } #ifdef USE_RGB_LED // has 3 color led void LED_GPIO_init() { - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - - /* GPIO Ports Clock Enable */ - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); - - LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_8); - LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_5); - LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_3); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_8; - GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_5; - GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_3; - GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + + /* GPIO Ports Clock Enable */ + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); + + LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_8); + LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_5); + LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_3); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_8; + GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_5; + GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_3; + GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); } #endif void reloadWatchDogCounter() { - WDT->cmd = WDT_CMD_RELOAD; + WDT->cmd = WDT_CMD_RELOAD; } -void setPWMCompare1(uint16_t compareone) { TMR1->c1dt = compareone; } -void setPWMCompare2(uint16_t comparetwo) { TMR1->c2dt = comparetwo; } -void setPWMCompare3(uint16_t comparethree) { TMR1->c3dt = comparethree; } +void setPWMCompare1(uint16_t compareone) +{ + TMR1->c1dt = compareone; +} +void setPWMCompare2(uint16_t comparetwo) +{ + TMR1->c2dt = comparetwo; +} +void setPWMCompare3(uint16_t comparethree) +{ + TMR1->c3dt = comparethree; +} void generatePwmTimerEvent() { - TMR1->swevt |= TMR_OVERFLOW_SWTRIG; - ; + TMR1->swevt |= TMR_OVERFLOW_SWTRIG; + ; } void resetInputCaptureTimer() { - IC_TIMER_REGISTER->pr = 0; - IC_TIMER_REGISTER->cval = 0; + IC_TIMER_REGISTER->pr = 0; + IC_TIMER_REGISTER->cval = 0; } void enableCorePeripherals() { - tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_1, TRUE); - tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_2, TRUE); - tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_3, TRUE); - tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_1C, TRUE); - tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_2C, TRUE); - tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_3C, TRUE); - - TMR1->ctrl1_bit.tmren = TRUE; - TMR1->brk_bit.oen = TRUE; - TMR1->swevt |= TMR_OVERFLOW_SWTRIG; + tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_1, TRUE); + tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_2, TRUE); + tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_3, TRUE); + tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_1C, TRUE); + tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_2C, TRUE); + tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_3C, TRUE); + + TMR1->ctrl1_bit.tmren = TRUE; + TMR1->brk_bit.oen = TRUE; + TMR1->swevt |= TMR_OVERFLOW_SWTRIG; #ifdef USE_RGB_LED - LED_GPIO_init(); - GPIOB->scr = LL_GPIO_PIN_8; // turn on red - GPIOB->clr = LL_GPIO_PIN_5; - GPIOB->clr = LL_GPIO_PIN_3; // + LED_GPIO_init(); + GPIOB->scr = LL_GPIO_PIN_8; // turn on red + GPIOB->clr = LL_GPIO_PIN_5; + GPIOB->clr = LL_GPIO_PIN_3; // #endif #ifndef BRUSHED_MODE - COM_TIMER->ctrl1_bit.tmren = TRUE; - COM_TIMER->swevt |= TMR_OVERFLOW_SWTRIG; - COM_TIMER->iden &= ~TMR_OVF_INT; + COM_TIMER->ctrl1_bit.tmren = TRUE; + COM_TIMER->swevt |= TMR_OVERFLOW_SWTRIG; + COM_TIMER->iden &= ~TMR_OVF_INT; #endif - UTILITY_TIMER->ctrl1_bit.tmren = TRUE; - INTERVAL_TIMER->ctrl1_bit.tmren = TRUE; - INTERVAL_TIMER->swevt |= TMR_OVERFLOW_SWTRIG; - TEN_KHZ_TIMER->ctrl1_bit.tmren = TRUE; - TEN_KHZ_TIMER->swevt |= TMR_OVERFLOW_SWTRIG; - TEN_KHZ_TIMER->iden |= TMR_OVF_INT; + UTILITY_TIMER->ctrl1_bit.tmren = TRUE; + INTERVAL_TIMER->ctrl1_bit.tmren = TRUE; + INTERVAL_TIMER->swevt |= TMR_OVERFLOW_SWTRIG; + TEN_KHZ_TIMER->ctrl1_bit.tmren = TRUE; + TEN_KHZ_TIMER->swevt |= TMR_OVERFLOW_SWTRIG; + TEN_KHZ_TIMER->iden |= TMR_OVF_INT; #ifdef USE_ADC - ADC_Init(); + ADC_Init(); #endif #ifdef USE_ADC_INPUT #else - tmr_channel_enable(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, TRUE); - IC_TIMER_REGISTER->ctrl1_bit.tmren = TRUE; + tmr_channel_enable(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, TRUE); + IC_TIMER_REGISTER->ctrl1_bit.tmren = TRUE; #endif - NVIC_SetPriority(EXINT15_10_IRQn, 2); - NVIC_EnableIRQ(EXINT15_10_IRQn); - EXINT->inten |= EXINT_LINE_15; + NVIC_SetPriority(EXINT15_10_IRQn, 2); + NVIC_EnableIRQ(EXINT15_10_IRQn); + EXINT->inten |= EXINT_LINE_15; } diff --git a/Mcu/f415/Src/phaseouts.c b/Mcu/f415/Src/phaseouts.c index ca8605ab..efa52ffe 100644 --- a/Mcu/f415/Src/phaseouts.c +++ b/Mcu/f415/Src/phaseouts.c @@ -30,33 +30,34 @@ extern char prop_brake_active; #endif void proportionalBrake() -{ // alternate all channels between braking (ABC LOW) - // and coasting (ABC float) put lower channel into - // alternate mode and turn upper OFF for each - // channel - // turn all HIGH channels off for ABC - - // gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, - // GPIO_PULL_NONE, PHASE_A_GPIO_HIGH); - gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_A_GPIO_HIGH); - PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; - - gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_B_GPIO_HIGH); - PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; - - gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_C_GPIO_HIGH); - PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; - - // set low channel to PWM, duty cycle will now control braking - gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_A_GPIO_LOW); - gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_B_GPIO_LOW); - gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_C_GPIO_LOW); +{ + // alternate all channels between braking (ABC LOW) + // and coasting (ABC float) put lower channel into + // alternate mode and turn upper OFF for each + // channel + // turn all HIGH channels off for ABC + + // gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, + // GPIO_PULL_NONE, PHASE_A_GPIO_HIGH); + gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_A_GPIO_HIGH); + PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; + + gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_B_GPIO_HIGH); + PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; + + gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_C_GPIO_HIGH); + PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; + + // set low channel to PWM, duty cycle will now control braking + gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_A_GPIO_LOW); + gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_B_GPIO_LOW); + gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_C_GPIO_LOW); } // void phaseCPWM() { @@ -87,35 +88,35 @@ void proportionalBrake() void phaseBPWM() { - if (!eepromBuffer.comp_pwm) { - gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_B_GPIO_LOW); - PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; - } else { - gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_B_GPIO_LOW); // low - } - gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_B_GPIO_HIGH); // high + if (!eepromBuffer.comp_pwm) { + gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_B_GPIO_LOW); + PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; + } else { + gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_B_GPIO_LOW); // low + } + gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_B_GPIO_HIGH); // high } void phaseBFLOAT() { - gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_B_GPIO_LOW); - PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; - gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_B_GPIO_HIGH); - PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; + gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_B_GPIO_LOW); + PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; + gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_B_GPIO_HIGH); + PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; } void phaseBLOW() { - // low mosfet on - gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_B_GPIO_LOW); - PHASE_B_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_B_GPIO_LOW; - gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_B_GPIO_HIGH); - PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; + // low mosfet on + gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_B_GPIO_LOW); + PHASE_B_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_B_GPIO_LOW; + gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_B_GPIO_HIGH); + PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; } //////////////////////////////PHASE @@ -123,38 +124,38 @@ void phaseBLOW() void phaseCPWM() { - if (!eepromBuffer.comp_pwm) { - gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_C_GPIO_LOW); - PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; - } else { - gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_C_GPIO_LOW); - ; - } - gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_C_GPIO_HIGH); + if (!eepromBuffer.comp_pwm) { + gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_C_GPIO_LOW); + PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; + } else { + gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_C_GPIO_LOW); + ; + } + gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_C_GPIO_HIGH); } void phaseCFLOAT() { - // floating - gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_C_GPIO_LOW); - PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; - gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_C_GPIO_HIGH); - PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; + // floating + gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_C_GPIO_LOW); + PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; + gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_C_GPIO_HIGH); + PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; } void phaseCLOW() { - gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_C_GPIO_LOW); - PHASE_C_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_C_GPIO_LOW; - gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_C_GPIO_HIGH); - PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; + gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_C_GPIO_LOW); + PHASE_C_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_C_GPIO_LOW; + gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_C_GPIO_HIGH); + PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; } ///////////////////////////////////////////////PHASE 3 @@ -162,36 +163,36 @@ void phaseCLOW() void phaseAPWM() { - if (!eepromBuffer.comp_pwm) { - gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_A_GPIO_LOW); - PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; - } else { - gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_A_GPIO_LOW); - } - gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_A_GPIO_HIGH); + if (!eepromBuffer.comp_pwm) { + gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_A_GPIO_LOW); + PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; + } else { + gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_A_GPIO_LOW); + } + gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_A_GPIO_HIGH); } void phaseAFLOAT() { - gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_A_GPIO_LOW); - PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; - gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_A_GPIO_HIGH); - PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; + gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_A_GPIO_LOW); + PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; + gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_A_GPIO_HIGH); + PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; } void phaseALOW() { - gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_A_GPIO_LOW); - PHASE_A_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_A_GPIO_LOW; - gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_A_GPIO_HIGH); - PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; + gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_A_GPIO_LOW); + PHASE_A_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_A_GPIO_LOW; + gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_A_GPIO_HIGH); + PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; } #else @@ -199,38 +200,38 @@ void phaseALOW() //////////////////////////////////PHASE 1////////////////////// void phaseBPWM() { - if (!eepromBuffer.comp_pwm) { // for future - // gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, - // GPIO_PULL_NONE, PHASE_B_GPIO_LOW); - // PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); // high pwm + if (!eepromBuffer.comp_pwm) { // for future + // gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, + // GPIO_PULL_NONE, PHASE_B_GPIO_LOW); + // PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); // high pwm } void phaseBFLOAT() { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable off - PHASE_B_GPIO_PORT_ENABLE->BRR = PHASE_B_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); // pwm off - PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable off + PHASE_B_GPIO_PORT_ENABLE->BRR = PHASE_B_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); // pwm off + PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; } void phaseBLOW() { - // low mosfet on - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); // pwm off - PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; + // low mosfet on + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); // pwm off + PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; } //////////////////////////////PHASE @@ -238,39 +239,39 @@ void phaseBLOW() void phaseCPWM() { - if (!eepromBuffer.comp_pwm) { - // gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, - // GPIO_PULL_NONE, - // PHASE_C_GPIO_LOW); PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = - // PHASE_C_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + // gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, + // GPIO_PULL_NONE, + // PHASE_C_GPIO_LOW); PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = + // PHASE_C_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); } void phaseCFLOAT() { - // floating - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable off - PHASE_C_GPIO_PORT_ENABLE->BRR = PHASE_C_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; + // floating + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable off + PHASE_C_GPIO_PORT_ENABLE->BRR = PHASE_C_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; } void phaseCLOW() { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; } ///////////////////////////////////////////////PHASE 3 @@ -278,114 +279,116 @@ void phaseCLOW() void phaseAPWM() { - if (!eepromBuffer.comp_pwm) { - // gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, - // GPIO_PULL_NONE, - // PHASE_A_GPIO_LOW); PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = - // PHASE_A_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + // gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, + // GPIO_PULL_NONE, + // PHASE_A_GPIO_LOW); PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = + // PHASE_A_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); } void phaseAFLOAT() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BRR = PHASE_A_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BRR = PHASE_A_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; } void phaseALOW() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; } #endif void allOff() { - phaseAFLOAT(); - phaseBFLOAT(); - phaseCFLOAT(); + phaseAFLOAT(); + phaseBFLOAT(); + phaseCFLOAT(); } void comStep(int newStep) { - switch (newStep) { - case 1: // A-B - phaseCFLOAT(); - phaseBLOW(); - phaseAPWM(); - break; - - case 2: // C-B - phaseAFLOAT(); - phaseBLOW(); - phaseCPWM(); - break; - - case 3: // C-A - phaseBFLOAT(); - phaseALOW(); - phaseCPWM(); - break; - - case 4: // B-A - phaseCFLOAT(); - phaseALOW(); - phaseBPWM(); - break; - - case 5: // B-C - phaseAFLOAT(); - phaseCLOW(); - phaseBPWM(); - break; - - case 6: // A-C - phaseBFLOAT(); - phaseCLOW(); - phaseAPWM(); - break; - } -} + switch (newStep) { + case 1: // A-B + phaseCFLOAT(); + phaseBLOW(); + phaseAPWM(); + break; -void fullBrake() -{ // full braking shorting all low sides - phaseALOW(); + case 2: // C-B + phaseAFLOAT(); phaseBLOW(); + phaseCPWM(); + break; + + case 3: // C-A + phaseBFLOAT(); + phaseALOW(); + phaseCPWM(); + break; + + case 4: // B-A + phaseCFLOAT(); + phaseALOW(); + phaseBPWM(); + break; + + case 5: // B-C + phaseAFLOAT(); phaseCLOW(); + phaseBPWM(); + break; + + case 6: // A-C + phaseBFLOAT(); + phaseCLOW(); + phaseAPWM(); + break; + } +} + +void fullBrake() +{ + // full braking shorting all low sides + phaseALOW(); + phaseBLOW(); + phaseCLOW(); } void allpwm() -{ // for stepper_sine - phaseAPWM(); - phaseBPWM(); - phaseCPWM(); +{ + // for stepper_sine + phaseAPWM(); + phaseBPWM(); + phaseCPWM(); } void twoChannelForward() { - phaseAPWM(); - phaseBLOW(); - phaseCPWM(); + phaseAPWM(); + phaseBLOW(); + phaseCPWM(); } void twoChannelReverse() { - phaseALOW(); - phaseBPWM(); - phaseCLOW(); + phaseALOW(); + phaseBPWM(); + phaseCLOW(); } diff --git a/Mcu/f415/Src/serial_telemetry.c b/Mcu/f415/Src/serial_telemetry.c index 6d8b7109..a689c70e 100644 --- a/Mcu/f415/Src/serial_telemetry.c +++ b/Mcu/f415/Src/serial_telemetry.c @@ -11,94 +11,97 @@ uint8_t aTxBuffer[10]; uint8_t nbDataToTransmit = sizeof(aTxBuffer); void send_telem_DMA() -{ // set data length and enable channel to start transfer - DMA1_CHANNEL4->dtcnt = nbDataToTransmit; - DMA1_CHANNEL4->ctrl_bit.chen = TRUE; +{ + // set data length and enable channel to start transfer + DMA1_CHANNEL4->dtcnt = nbDataToTransmit; + DMA1_CHANNEL4->ctrl_bit.chen = TRUE; } uint8_t update_crc8(uint8_t crc, uint8_t crc_seed) { - uint8_t crc_u, i; - crc_u = crc; - crc_u ^= crc_seed; - for (i = 0; i < 8; i++) - crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); - return (crc_u); + uint8_t crc_u, i; + crc_u = crc; + crc_u ^= crc_seed; + for (i = 0; i < 8; i++) { + crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); + } + return (crc_u); } uint8_t get_crc8(uint8_t* Buf, uint8_t BufLen) { - uint8_t crc = 0, i; - for (i = 0; i < BufLen; i++) - crc = update_crc8(Buf[i], crc); - return (crc); + uint8_t crc = 0, i; + for (i = 0; i < BufLen; i++) { + crc = update_crc8(Buf[i], crc); + } + return (crc); } void makeTelemPackage(uint8_t temp, uint16_t voltage, uint16_t current, - uint16_t consumption, uint16_t e_rpm) + uint16_t consumption, uint16_t e_rpm) { - aTxBuffer[0] = temp; // temperature + aTxBuffer[0] = temp; // temperature - aTxBuffer[1] = (voltage >> 8) & 0xFF; // voltage hB - aTxBuffer[2] = voltage & 0xFF; // voltage lowB + aTxBuffer[1] = (voltage >> 8) & 0xFF; // voltage hB + aTxBuffer[2] = voltage & 0xFF; // voltage lowB - aTxBuffer[3] = (current >> 8) & 0xFF; // current - aTxBuffer[4] = current & 0xFF; // divide by 10 for Amps + aTxBuffer[3] = (current >> 8) & 0xFF; // current + aTxBuffer[4] = current & 0xFF; // divide by 10 for Amps - aTxBuffer[5] = (consumption >> 8) & 0xFF; // consumption - aTxBuffer[6] = consumption & 0xFF; // in mah + aTxBuffer[5] = (consumption >> 8) & 0xFF; // consumption + aTxBuffer[6] = consumption & 0xFF; // in mah - aTxBuffer[7] = (e_rpm >> 8) & 0xFF; // - aTxBuffer[8] = e_rpm & 0xFF; // eRpM *100 + aTxBuffer[7] = (e_rpm >> 8) & 0xFF; // + aTxBuffer[8] = e_rpm & 0xFF; // eRpM *100 - aTxBuffer[9] = get_crc8(aTxBuffer, 9); + aTxBuffer[9] = get_crc8(aTxBuffer, 9); } void telem_UART_Init(void) { - gpio_init_type gpio_init_struct; - - crm_periph_clock_enable(CRM_USART1_PERIPH_CLOCK, TRUE); - crm_periph_clock_enable(CRM_GPIOB_PERIPH_CLOCK, TRUE); - crm_periph_clock_enable(CRM_DMA1_PERIPH_CLOCK, TRUE); - - /* configure the usart2 tx pin */ - gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER; - gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL; - gpio_init_struct.gpio_mode = GPIO_MODE_MUX; - gpio_init_struct.gpio_pins = GPIO_PINS_6; - gpio_init_struct.gpio_pull = GPIO_PULL_NONE; - gpio_init(GPIOB, &gpio_init_struct); - // gpio_pin_mux_config(GPIOB, GPIO_PINS_SOURCE6, GPIO_MUX_0); - - dma_reset(DMA1_CHANNEL4); - dma_flexible_config(DMA1,FLEX_CHANNEL4,DMA_FLEXIBLE_UART1_TX); - dma_init_type dma_init_struct; - dma_default_para_init(&dma_init_struct); - dma_init_struct.buffer_size = nbDataToTransmit; - dma_init_struct.direction = DMA_DIR_MEMORY_TO_PERIPHERAL; - dma_init_struct.memory_base_addr = (uint32_t)&aTxBuffer; - dma_init_struct.memory_data_width = DMA_MEMORY_DATA_WIDTH_BYTE; - dma_init_struct.memory_inc_enable = TRUE; - dma_init_struct.peripheral_base_addr = (uint32_t)&USART1->dt; - dma_init_struct.peripheral_data_width = DMA_PERIPHERAL_DATA_WIDTH_BYTE; - dma_init_struct.peripheral_inc_enable = FALSE; - dma_init_struct.priority = DMA_PRIORITY_LOW; - dma_init_struct.loop_mode_enable = FALSE; - dma_init(DMA1_CHANNEL4, &dma_init_struct); - - DMA1_CHANNEL4->ctrl |= DMA_FDT_INT; - DMA1_CHANNEL4->ctrl |= DMA_DTERR_INT; - - /* configure usart1 param */ - - gpio_pin_remap_config(USART1_MUX, TRUE); - usart_init(USART1, 115200, USART_DATA_8BITS, USART_STOP_1_BIT); - usart_transmitter_enable(USART1, TRUE); - usart_receiver_enable(USART1, TRUE); - usart_single_line_halfduplex_select(USART1, TRUE); - usart_dma_transmitter_enable(USART1, TRUE); - usart_enable(USART1, TRUE); - - nvic_irq_enable(DMA1_Channel4_IRQn, 3, 0); + gpio_init_type gpio_init_struct; + + crm_periph_clock_enable(CRM_USART1_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_GPIOB_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_DMA1_PERIPH_CLOCK, TRUE); + + /* configure the usart2 tx pin */ + gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER; + gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL; + gpio_init_struct.gpio_mode = GPIO_MODE_MUX; + gpio_init_struct.gpio_pins = GPIO_PINS_6; + gpio_init_struct.gpio_pull = GPIO_PULL_NONE; + gpio_init(GPIOB, &gpio_init_struct); + // gpio_pin_mux_config(GPIOB, GPIO_PINS_SOURCE6, GPIO_MUX_0); + + dma_reset(DMA1_CHANNEL4); + dma_flexible_config(DMA1,FLEX_CHANNEL4,DMA_FLEXIBLE_UART1_TX); + dma_init_type dma_init_struct; + dma_default_para_init(&dma_init_struct); + dma_init_struct.buffer_size = nbDataToTransmit; + dma_init_struct.direction = DMA_DIR_MEMORY_TO_PERIPHERAL; + dma_init_struct.memory_base_addr = (uint32_t)&aTxBuffer; + dma_init_struct.memory_data_width = DMA_MEMORY_DATA_WIDTH_BYTE; + dma_init_struct.memory_inc_enable = TRUE; + dma_init_struct.peripheral_base_addr = (uint32_t)&USART1->dt; + dma_init_struct.peripheral_data_width = DMA_PERIPHERAL_DATA_WIDTH_BYTE; + dma_init_struct.peripheral_inc_enable = FALSE; + dma_init_struct.priority = DMA_PRIORITY_LOW; + dma_init_struct.loop_mode_enable = FALSE; + dma_init(DMA1_CHANNEL4, &dma_init_struct); + + DMA1_CHANNEL4->ctrl |= DMA_FDT_INT; + DMA1_CHANNEL4->ctrl |= DMA_DTERR_INT; + + /* configure usart1 param */ + + gpio_pin_remap_config(USART1_MUX, TRUE); + usart_init(USART1, 115200, USART_DATA_8BITS, USART_STOP_1_BIT); + usart_transmitter_enable(USART1, TRUE); + usart_receiver_enable(USART1, TRUE); + usart_single_line_halfduplex_select(USART1, TRUE); + usart_dma_transmitter_enable(USART1, TRUE); + usart_enable(USART1, TRUE); + + nvic_irq_enable(DMA1_Channel4_IRQn, 3, 0); } \ No newline at end of file diff --git a/Mcu/f415/Src/system_at32f415.c b/Mcu/f415/Src/system_at32f415.c index 7898c7c8..caef87e7 100644 --- a/Mcu/f415/Src/system_at32f415.c +++ b/Mcu/f415/Src/system_at32f415.c @@ -70,47 +70,47 @@ unsigned int system_core_clock = HICK_VALUE; /*!< system clock frequency (core c */ void SystemInit(void) { - /* enable low power mode */ - CRM->apb1en_bit.pwcen = 1; - *(volatile uint8_t*)(0x40007050) |= (uint8_t)(0x1 << 2); - CRM->apb1en_bit.pwcen = 0; + /* enable low power mode */ + CRM->apb1en_bit.pwcen = 1; + *(volatile uint8_t*)(0x40007050) |= (uint8_t)(0x1 << 2); + CRM->apb1en_bit.pwcen = 0; - /* reset the crm clock configuration to the default reset state(for debug - * purpose) */ - /* set hicken bit */ - CRM->ctrl_bit.hicken = TRUE; + /* reset the crm clock configuration to the default reset state(for debug + * purpose) */ + /* set hicken bit */ + CRM->ctrl_bit.hicken = TRUE; - /* wait hick stable */ - while (CRM->ctrl_bit.hickstbl != SET) - ; + /* wait hick stable */ + while (CRM->ctrl_bit.hickstbl != SET) + ; - /* hick used as system clock */ - CRM->cfg_bit.sclksel = CRM_SCLK_HICK; + /* hick used as system clock */ + CRM->cfg_bit.sclksel = CRM_SCLK_HICK; - /* wait sclk switch status */ - while (CRM->cfg_bit.sclksts != CRM_SCLK_HICK) - ; + /* wait sclk switch status */ + while (CRM->cfg_bit.sclksts != CRM_SCLK_HICK) + ; - /* reset hexten, hextbyps, cfden and pllen bits */ - CRM->ctrl &= ~(0x010D0000U); + /* reset hexten, hextbyps, cfden and pllen bits */ + CRM->ctrl &= ~(0x010D0000U); - /* reset cfg register, include sclk switch, ahbdiv, apb1div, apb2div, adcdiv, - clkout pllrcs, pllhextdiv, pllmult, usbdiv and pllrange bits */ - CRM->cfg = 0; + /* reset cfg register, include sclk switch, ahbdiv, apb1div, apb2div, adcdiv, + clkout pllrcs, pllhextdiv, pllmult, usbdiv and pllrange bits */ + CRM->cfg = 0; - /* reset pllfr, pllms, pllns and pllfref bits */ - CRM->pll = (0x00001F10U); + /* reset pllfr, pllms, pllns and pllfref bits */ + CRM->pll = (0x00001F10U); - /* reset clkout[3], usbbufs, hickdiv, clkoutdiv */ - CRM->misc1 = 0x00100000; + /* reset clkout[3], usbbufs, hickdiv, clkoutdiv */ + CRM->misc1 = 0x00100000; - /* disable all interrupts enable and clear pending bits */ - CRM->clkint = 0x009F0000; + /* disable all interrupts enable and clear pending bits */ + CRM->clkint = 0x009F0000; #ifdef VECT_TAB_SRAM - SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* vector table relocation in internal sram. */ + SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* vector table relocation in internal sram. */ #else - SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* vector table relocation in internal flash. */ + SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* vector table relocation in internal flash. */ #endif } @@ -124,83 +124,85 @@ void SystemInit(void) */ void system_core_clock_update(void) { - uint32_t pll_mult = 0, pll_mult_h = 0, pll_clock_source = 0, temp = 0, - div_value = 0; - uint32_t pllrcsfreq = 0, pll_ms = 0, pll_ns = 0, pll_fr = 0; - crm_sclk_type sclk_source; - - static const uint8_t sys_ahb_div_table[16] = { 0, 0, 0, 0, 0, 0, 0, 0, - 1, 2, 3, 4, 6, 7, 8, 9 }; - - /* get sclk source */ - sclk_source = crm_sysclk_switch_status_get(); - - switch (sclk_source) { - case CRM_SCLK_HICK: - if (((CRM->misc2_bit.hick_to_sclk) != RESET) && ((CRM->misc1_bit.hickdiv) != RESET)) - system_core_clock = HICK_VALUE * 6; - else - system_core_clock = HICK_VALUE; - break; - case CRM_SCLK_HEXT: - system_core_clock = HEXT_VALUE; - break; - case CRM_SCLK_PLL: - pll_clock_source = CRM->cfg_bit.pllrcs; - if (CRM->pll_bit.pllcfgen == FALSE) { - /* get multiplication factor */ - pll_mult = CRM->cfg_bit.pllmult_l; - pll_mult_h = CRM->cfg_bit.pllmult_h; - /* process high bits */ - if ((pll_mult_h != 0U) || (pll_mult == 15U)) { - pll_mult += ((16U * pll_mult_h) + 1U); - } else { - pll_mult += 2U; - } - - if (pll_clock_source == 0x00) { - /* hick divided by 2 selected as pll clock entry */ - system_core_clock = (HICK_VALUE >> 1) * pll_mult; - } else { - /* hext selected as pll clock entry */ - if (CRM->cfg_bit.pllhextdiv != RESET) { - /* hext clock divided by 2 */ - system_core_clock = (HEXT_VALUE / 2) * pll_mult; - } else { - system_core_clock = HEXT_VALUE * pll_mult; - } - } + uint32_t pll_mult = 0, pll_mult_h = 0, pll_clock_source = 0, temp = 0, + div_value = 0; + uint32_t pllrcsfreq = 0, pll_ms = 0, pll_ns = 0, pll_fr = 0; + crm_sclk_type sclk_source; + + static const uint8_t sys_ahb_div_table[16] = { 0, 0, 0, 0, 0, 0, 0, 0, + 1, 2, 3, 4, 6, 7, 8, 9 + }; + + /* get sclk source */ + sclk_source = crm_sysclk_switch_status_get(); + + switch (sclk_source) { + case CRM_SCLK_HICK: + if (((CRM->misc2_bit.hick_to_sclk) != RESET) && ((CRM->misc1_bit.hickdiv) != RESET)) { + system_core_clock = HICK_VALUE * 6; + } else { + system_core_clock = HICK_VALUE; + } + break; + case CRM_SCLK_HEXT: + system_core_clock = HEXT_VALUE; + break; + case CRM_SCLK_PLL: + pll_clock_source = CRM->cfg_bit.pllrcs; + if (CRM->pll_bit.pllcfgen == FALSE) { + /* get multiplication factor */ + pll_mult = CRM->cfg_bit.pllmult_l; + pll_mult_h = CRM->cfg_bit.pllmult_h; + /* process high bits */ + if ((pll_mult_h != 0U) || (pll_mult == 15U)) { + pll_mult += ((16U * pll_mult_h) + 1U); + } else { + pll_mult += 2U; + } + + if (pll_clock_source == 0x00) { + /* hick divided by 2 selected as pll clock entry */ + system_core_clock = (HICK_VALUE >> 1) * pll_mult; + } else { + /* hext selected as pll clock entry */ + if (CRM->cfg_bit.pllhextdiv != RESET) { + /* hext clock divided by 2 */ + system_core_clock = (HEXT_VALUE / 2) * pll_mult; + } else { + system_core_clock = HEXT_VALUE * pll_mult; + } + } + } else { + pll_ms = CRM->pll_bit.pllms; + pll_ns = CRM->pll_bit.pllns; + pll_fr = CRM->pll_bit.pllfr; + + if (pll_clock_source == 0x00) { + /* hick divided by 2 selected as pll clock entry */ + pllrcsfreq = (HICK_VALUE >> 1); + } else { + /* hext selected as pll clock entry */ + if (CRM->cfg_bit.pllhextdiv != RESET) { + /* hext clock divided by 2 */ + pllrcsfreq = (HEXT_VALUE / 2); } else { - pll_ms = CRM->pll_bit.pllms; - pll_ns = CRM->pll_bit.pllns; - pll_fr = CRM->pll_bit.pllfr; - - if (pll_clock_source == 0x00) { - /* hick divided by 2 selected as pll clock entry */ - pllrcsfreq = (HICK_VALUE >> 1); - } else { - /* hext selected as pll clock entry */ - if (CRM->cfg_bit.pllhextdiv != RESET) { - /* hext clock divided by 2 */ - pllrcsfreq = (HEXT_VALUE / 2); - } else { - pllrcsfreq = HEXT_VALUE; - } - } - system_core_clock = (uint32_t)(((uint64_t)pllrcsfreq * pll_ns) / (pll_ms * (0x1 << pll_fr))); + pllrcsfreq = HEXT_VALUE; } - break; - default: - system_core_clock = HICK_VALUE; - break; + } + system_core_clock = (uint32_t)(((uint64_t)pllrcsfreq * pll_ns) / (pll_ms * (0x1 << pll_fr))); } - - /* compute sclk, ahbclk frequency */ - /* get ahb division */ - temp = CRM->cfg_bit.ahbdiv; - div_value = sys_ahb_div_table[temp]; - /* ahbclk frequency */ - system_core_clock = system_core_clock >> div_value; + break; + default: + system_core_clock = HICK_VALUE; + break; + } + + /* compute sclk, ahbclk frequency */ + /* get ahb division */ + temp = CRM->cfg_bit.ahbdiv; + div_value = sys_ahb_div_table[temp]; + /* ahbclk frequency */ + system_core_clock = system_core_clock >> div_value; } /** * @} diff --git a/Mcu/f421/Inc/IO.h b/Mcu/f421/Inc/IO.h index 352e164a..317300f5 100644 --- a/Mcu/f421/Inc/IO.h +++ b/Mcu/f421/Inc/IO.h @@ -33,6 +33,6 @@ extern uint8_t degrees_celsius; extern uint16_t ADC_raw_volts; extern uint16_t servo_low_threshold; // anything below this point considered 0 extern uint16_t - servo_high_threshold; // anything above this point considered 2000 (max) +servo_high_threshold; // anything above this point considered 2000 (max) extern uint16_t servo_neutral; extern uint8_t servo_dead_band; diff --git a/Mcu/f421/Inc/serial_telemetry.h b/Mcu/f421/Inc/serial_telemetry.h index b4f1c5cf..fb8cd3b9 100644 --- a/Mcu/f421/Inc/serial_telemetry.h +++ b/Mcu/f421/Inc/serial_telemetry.h @@ -11,7 +11,7 @@ #define SERIAL_TELEMETRY_H_ void makeTelemPackage(uint8_t temp, uint16_t voltage, uint16_t current, - uint16_t consumption, uint16_t e_rpm); + uint16_t consumption, uint16_t e_rpm); void telem_UART_Init(void); void send_telem_DMA(); diff --git a/Mcu/f421/Inc/system_at32f421.h b/Mcu/f421/Inc/system_at32f421.h index 38c3be61..cbb23719 100644 --- a/Mcu/f421/Inc/system_at32f421.h +++ b/Mcu/f421/Inc/system_at32f421.h @@ -56,7 +56,7 @@ extern "C" { */ extern unsigned int - system_core_clock; /*!< system clock frequency (core clock) */ +system_core_clock; /*!< system clock frequency (core clock) */ /** * @} diff --git a/Mcu/f421/Src/ADC.c b/Mcu/f421/Src/ADC.c index a836c47c..e036be20 100644 --- a/Mcu/f421/Src/ADC.c +++ b/Mcu/f421/Src/ADC.c @@ -22,23 +22,24 @@ extern uint16_t ADC_raw_current; extern uint16_t ADC_raw_input; void ADC_DMA_Callback() -{ // read dma buffer and set extern variables +{ + // read dma buffer and set extern variables #ifdef PA6_NTC_ONLY - ADC_raw_temp = ADCDataDMA[0]; + ADC_raw_temp = ADCDataDMA[0]; #else #ifdef USE_ADC_INPUT - ADC_raw_temp = ADCDataDMA[3]; - ADC_raw_volts = ADCDataDMA[1] / 2; - ADC_raw_current = ADCDataDMA[2]; - ADC_raw_input = ADCDataDMA[0]; + ADC_raw_temp = ADCDataDMA[3]; + ADC_raw_volts = ADCDataDMA[1] / 2; + ADC_raw_current = ADCDataDMA[2]; + ADC_raw_input = ADCDataDMA[0]; #else - ADC_raw_temp = ADCDataDMA[2]; + ADC_raw_temp = ADCDataDMA[2]; #ifdef PA6_VOLTAGE - ADC_raw_volts = ADCDataDMA[1]; - ADC_raw_current = ADCDataDMA[0]; + ADC_raw_volts = ADCDataDMA[1]; + ADC_raw_current = ADCDataDMA[0]; #else - ADC_raw_volts = ADCDataDMA[0]; - ADC_raw_current = ADCDataDMA[1]; + ADC_raw_volts = ADCDataDMA[0]; + ADC_raw_current = ADCDataDMA[1]; #endif #endif #endif @@ -47,72 +48,72 @@ void ADC_DMA_Callback() void ADC_Init(void) { #ifdef PA2_VOLTAGE - gpio_mode_QUICK(GPIOA, GPIO_MODE_ANALOG, GPIO_PULL_NONE, GPIO_PINS_2); + gpio_mode_QUICK(GPIOA, GPIO_MODE_ANALOG, GPIO_PULL_NONE, GPIO_PINS_2); #else - gpio_mode_QUICK(GPIOA, GPIO_MODE_ANALOG, GPIO_PULL_NONE, GPIO_PINS_6); + gpio_mode_QUICK(GPIOA, GPIO_MODE_ANALOG, GPIO_PULL_NONE, GPIO_PINS_6); #endif - gpio_mode_QUICK(GPIOA, GPIO_MODE_ANALOG, GPIO_PULL_NONE, GPIO_PINS_3); - - dma_init_type dma_init_struct; - crm_periph_clock_enable(CRM_DMA1_PERIPH_CLOCK, TRUE); - // nvic_irq_enable(DMA1_Channel1_IRQn, 3, 0); - dma_reset(DMA1_CHANNEL1); - dma_default_para_init(&dma_init_struct); + gpio_mode_QUICK(GPIOA, GPIO_MODE_ANALOG, GPIO_PULL_NONE, GPIO_PINS_3); + + dma_init_type dma_init_struct; + crm_periph_clock_enable(CRM_DMA1_PERIPH_CLOCK, TRUE); + // nvic_irq_enable(DMA1_Channel1_IRQn, 3, 0); + dma_reset(DMA1_CHANNEL1); + dma_default_para_init(&dma_init_struct); #ifdef PA6_NTC_ONLY - dma_init_struct.buffer_size = 1; + dma_init_struct.buffer_size = 1; #else - dma_init_struct.buffer_size = 3; + dma_init_struct.buffer_size = 3; #endif - dma_init_struct.direction = DMA_DIR_PERIPHERAL_TO_MEMORY; - dma_init_struct.memory_base_addr = (uint32_t)&ADCDataDMA; - dma_init_struct.memory_data_width = DMA_MEMORY_DATA_WIDTH_HALFWORD; - dma_init_struct.memory_inc_enable = TRUE; - dma_init_struct.peripheral_base_addr = (uint32_t)&ADC1->odt; - dma_init_struct.peripheral_data_width = DMA_PERIPHERAL_DATA_WIDTH_HALFWORD; - dma_init_struct.peripheral_inc_enable = FALSE; - dma_init_struct.priority = DMA_PRIORITY_HIGH; - dma_init_struct.loop_mode_enable = TRUE; - dma_init(DMA1_CHANNEL1, &dma_init_struct); + dma_init_struct.direction = DMA_DIR_PERIPHERAL_TO_MEMORY; + dma_init_struct.memory_base_addr = (uint32_t)&ADCDataDMA; + dma_init_struct.memory_data_width = DMA_MEMORY_DATA_WIDTH_HALFWORD; + dma_init_struct.memory_inc_enable = TRUE; + dma_init_struct.peripheral_base_addr = (uint32_t)&ADC1->odt; + dma_init_struct.peripheral_data_width = DMA_PERIPHERAL_DATA_WIDTH_HALFWORD; + dma_init_struct.peripheral_inc_enable = FALSE; + dma_init_struct.priority = DMA_PRIORITY_HIGH; + dma_init_struct.loop_mode_enable = TRUE; + dma_init(DMA1_CHANNEL1, &dma_init_struct); - // dma_interrupt_enable(DMA1_CHANNEL1, DMA_FDT_INT, TRUE); - dma_channel_enable(DMA1_CHANNEL1, TRUE); + // dma_interrupt_enable(DMA1_CHANNEL1, DMA_FDT_INT, TRUE); + dma_channel_enable(DMA1_CHANNEL1, TRUE); - adc_base_config_type adc_base_struct; - crm_periph_clock_enable(CRM_ADC1_PERIPH_CLOCK, TRUE); - crm_adc_clock_div_set(CRM_ADC_DIV_16); + adc_base_config_type adc_base_struct; + crm_periph_clock_enable(CRM_ADC1_PERIPH_CLOCK, TRUE); + crm_adc_clock_div_set(CRM_ADC_DIV_16); - adc_base_default_para_init(&adc_base_struct); - adc_base_struct.sequence_mode = TRUE; - adc_base_struct.repeat_mode = TRUE; - adc_base_struct.data_align = ADC_RIGHT_ALIGNMENT; + adc_base_default_para_init(&adc_base_struct); + adc_base_struct.sequence_mode = TRUE; + adc_base_struct.repeat_mode = TRUE; + adc_base_struct.data_align = ADC_RIGHT_ALIGNMENT; #ifdef PA6_NTC_ONLY - adc_base_struct.ordinary_channel_length = 1; - adc_base_config(ADC1, &adc_base_struct); - adc_ordinary_channel_set(ADC1, ADC_CHANNEL_6, 1, ADC_SAMPLETIME_28_5); + adc_base_struct.ordinary_channel_length = 1; + adc_base_config(ADC1, &adc_base_struct); + adc_ordinary_channel_set(ADC1, ADC_CHANNEL_6, 1, ADC_SAMPLETIME_28_5); #else - adc_base_struct.ordinary_channel_length = 3; - adc_base_config(ADC1, &adc_base_struct); - adc_ordinary_channel_set(ADC1, ADC_CHANNEL_VOLTAGE, 1, ADC_SAMPLETIME_28_5); - adc_ordinary_channel_set(ADC1, ADC_CHANNEL_CURRENT, 2, ADC_SAMPLETIME_28_5); - adc_ordinary_channel_set(ADC1, ADC_CHANNEL_TEMP, 3, ADC_SAMPLETIME_28_5); + adc_base_struct.ordinary_channel_length = 3; + adc_base_config(ADC1, &adc_base_struct); + adc_ordinary_channel_set(ADC1, ADC_CHANNEL_VOLTAGE, 1, ADC_SAMPLETIME_28_5); + adc_ordinary_channel_set(ADC1, ADC_CHANNEL_CURRENT, 2, ADC_SAMPLETIME_28_5); + adc_ordinary_channel_set(ADC1, ADC_CHANNEL_TEMP, 3, ADC_SAMPLETIME_28_5); #endif - adc_tempersensor_vintrv_enable(TRUE); - adc_ordinary_conversion_trigger_set(ADC1, ADC12_ORDINARY_TRIG_SOFTWARE, TRUE); + adc_tempersensor_vintrv_enable(TRUE); + adc_ordinary_conversion_trigger_set(ADC1, ADC12_ORDINARY_TRIG_SOFTWARE, TRUE); - adc_dma_mode_enable(ADC1, TRUE); + adc_dma_mode_enable(ADC1, TRUE); - adc_enable(ADC1, TRUE); - adc_calibration_init(ADC1); - while (adc_calibration_init_status_get(ADC1)) - ; - adc_calibration_start(ADC1); - while (adc_calibration_status_get(ADC1)) - ; + adc_enable(ADC1, TRUE); + adc_calibration_init(ADC1); + while (adc_calibration_init_status_get(ADC1)) + ; + adc_calibration_start(ADC1); + while (adc_calibration_status_get(ADC1)) + ; } int16_t getConvertedDegrees(uint16_t adcrawtemp) { - return (12600 - (int32_t)adcrawtemp * 33000 / 4096) / -42 + 15; + return (12600 - (int32_t)adcrawtemp * 33000 / 4096) / -42 + 15; } #endif // USE_ADC diff --git a/Mcu/f421/Src/IO.c b/Mcu/f421/Src/IO.c index 84a59b65..d1037005 100644 --- a/Mcu/f421/Src/IO.c +++ b/Mcu/f421/Src/IO.c @@ -20,77 +20,80 @@ uint8_t buffer_padding = 7; void changeToOutput() { - INPUT_DMA_CHANNEL->ctrl |= DMA_DIR_MEMORY_TO_PERIPHERAL; - tmr_reset(IC_TIMER_REGISTER); - IC_TIMER_REGISTER->cm1 = 0x60; // oc mode pwm - IC_TIMER_REGISTER->cctrl = 0x3; // - IC_TIMER_REGISTER->div = output_timer_prescaler; - IC_TIMER_REGISTER->pr = 76; // 76 to start + INPUT_DMA_CHANNEL->ctrl |= DMA_DIR_MEMORY_TO_PERIPHERAL; + tmr_reset(IC_TIMER_REGISTER); + IC_TIMER_REGISTER->cm1 = 0x60; // oc mode pwm + IC_TIMER_REGISTER->cctrl = 0x3; // + IC_TIMER_REGISTER->div = output_timer_prescaler; + IC_TIMER_REGISTER->pr = 76; // 76 to start - out_put = 1; - IC_TIMER_REGISTER->swevt_bit.ovfswtr = TRUE; + out_put = 1; + IC_TIMER_REGISTER->swevt_bit.ovfswtr = TRUE; } void changeToInput() { - INPUT_DMA_CHANNEL->ctrl |= DMA_DIR_PERIPHERAL_TO_MEMORY; - tmr_reset(IC_TIMER_REGISTER); - IC_TIMER_REGISTER->cm1 = 0x41; - IC_TIMER_REGISTER->cctrl = 0xB; - IC_TIMER_REGISTER->div = ic_timer_prescaler; - IC_TIMER_REGISTER->pr = 0xFFFF; - IC_TIMER_REGISTER->swevt_bit.ovfswtr = TRUE; - out_put = 0; + INPUT_DMA_CHANNEL->ctrl |= DMA_DIR_PERIPHERAL_TO_MEMORY; + tmr_reset(IC_TIMER_REGISTER); + IC_TIMER_REGISTER->cm1 = 0x41; + IC_TIMER_REGISTER->cctrl = 0xB; + IC_TIMER_REGISTER->div = ic_timer_prescaler; + IC_TIMER_REGISTER->pr = 0xFFFF; + IC_TIMER_REGISTER->swevt_bit.ovfswtr = TRUE; + out_put = 0; } void receiveDshotDma() { - changeToInput(); - IC_TIMER_REGISTER->cval = 0; - INPUT_DMA_CHANNEL->paddr = (uint32_t)&IC_TIMER_REGISTER->c1dt; - INPUT_DMA_CHANNEL->maddr = (uint32_t)&dma_buffer; - INPUT_DMA_CHANNEL->dtcnt = buffersize; - IC_TIMER_REGISTER->iden |= TMR_C1_DMA_REQUEST; - IC_TIMER_REGISTER->ctrl1_bit.tmren = TRUE; - INPUT_DMA_CHANNEL->ctrl = 0x0000098b; + changeToInput(); + IC_TIMER_REGISTER->cval = 0; + INPUT_DMA_CHANNEL->paddr = (uint32_t)&IC_TIMER_REGISTER->c1dt; + INPUT_DMA_CHANNEL->maddr = (uint32_t)&dma_buffer; + INPUT_DMA_CHANNEL->dtcnt = buffersize; + IC_TIMER_REGISTER->iden |= TMR_C1_DMA_REQUEST; + IC_TIMER_REGISTER->ctrl1_bit.tmren = TRUE; + INPUT_DMA_CHANNEL->ctrl = 0x0000098b; } void sendDshotDma() { - changeToOutput(); - INPUT_DMA_CHANNEL->paddr = (uint32_t)&IC_TIMER_REGISTER->c1dt; - INPUT_DMA_CHANNEL->maddr = (uint32_t)&gcr; - INPUT_DMA_CHANNEL->dtcnt = 23 + buffer_padding; - INPUT_DMA_CHANNEL->ctrl |= DMA_FDT_INT; - INPUT_DMA_CHANNEL->ctrl |= DMA_DTERR_INT; - INPUT_DMA_CHANNEL->ctrl_bit.chen = TRUE; - IC_TIMER_REGISTER->iden |= TMR_C1_DMA_REQUEST; - IC_TIMER_REGISTER->brk_bit.oen = TRUE; - IC_TIMER_REGISTER->ctrl1_bit.tmren = TRUE; + changeToOutput(); + INPUT_DMA_CHANNEL->paddr = (uint32_t)&IC_TIMER_REGISTER->c1dt; + INPUT_DMA_CHANNEL->maddr = (uint32_t)&gcr; + INPUT_DMA_CHANNEL->dtcnt = 23 + buffer_padding; + INPUT_DMA_CHANNEL->ctrl |= DMA_FDT_INT; + INPUT_DMA_CHANNEL->ctrl |= DMA_DTERR_INT; + INPUT_DMA_CHANNEL->ctrl_bit.chen = TRUE; + IC_TIMER_REGISTER->iden |= TMR_C1_DMA_REQUEST; + IC_TIMER_REGISTER->brk_bit.oen = TRUE; + IC_TIMER_REGISTER->ctrl1_bit.tmren = TRUE; } uint8_t getInputPinState() { - uint8_t state = INPUT_PIN_PORT->idt & INPUT_PIN; - return state; + uint8_t state = INPUT_PIN_PORT->idt & INPUT_PIN; + return state; } void setInputPolarityRising() { - IC_TIMER_REGISTER->cctrl_bit.c1p = TMR_INPUT_RISING_EDGE; + IC_TIMER_REGISTER->cctrl_bit.c1p = TMR_INPUT_RISING_EDGE; } void setInputPullDown() { - gpio_mode_set(INPUT_PIN_PORT, GPIO_MODE_MUX, GPIO_PULL_DOWN, INPUT_PIN); + gpio_mode_set(INPUT_PIN_PORT, GPIO_MODE_MUX, GPIO_PULL_DOWN, INPUT_PIN); } void setInputPullUp() { - gpio_mode_set(INPUT_PIN_PORT, GPIO_MODE_MUX, GPIO_PULL_UP, INPUT_PIN); + gpio_mode_set(INPUT_PIN_PORT, GPIO_MODE_MUX, GPIO_PULL_UP, INPUT_PIN); } -void enableHalfTransferInt() { INPUT_DMA_CHANNEL->ctrl |= DMA_HDT_INT; } +void enableHalfTransferInt() +{ + INPUT_DMA_CHANNEL->ctrl |= DMA_HDT_INT; +} void setInputPullNone() { - gpio_mode_set(INPUT_PIN_PORT, GPIO_MODE_MUX, GPIO_PULL_NONE, INPUT_PIN); + gpio_mode_set(INPUT_PIN_PORT, GPIO_MODE_MUX, GPIO_PULL_NONE, INPUT_PIN); } diff --git a/Mcu/f421/Src/WS2812.c b/Mcu/f421/Src/WS2812.c index a3122c48..9cc5dbc3 100644 --- a/Mcu/f421/Src/WS2812.c +++ b/Mcu/f421/Src/WS2812.c @@ -14,9 +14,9 @@ void waitClockCycles(uint16_t cycles) { - UTILITY_TIMER->cval = 0; - while (UTILITY_TIMER->cval < cycles) { - } + UTILITY_TIMER->cval = 0; + while (UTILITY_TIMER->cval < cycles) { + } } // void sendBit(uint8_t inbit){ @@ -36,30 +36,30 @@ void waitClockCycles(uint16_t cycles) void sendBit(uint8_t inbit) { - GPIOB->scr = WS2812_PIN; - waitClockCycles(CPU_FREQUENCY_MHZ >> (2 - inbit)); - GPIOB->clr = WS2812_PIN; - waitClockCycles(CPU_FREQUENCY_MHZ >> (1 + inbit)); + GPIOB->scr = WS2812_PIN; + waitClockCycles(CPU_FREQUENCY_MHZ >> (2 - inbit)); + GPIOB->clr = WS2812_PIN; + waitClockCycles(CPU_FREQUENCY_MHZ >> (1 + inbit)); } void send_LED_RGB(uint8_t red, uint8_t green, uint8_t blue) { - __disable_irq(); - UTILITY_TIMER->div = 0; - UTILITY_TIMER->swevt |= TMR_OVERFLOW_SWTRIG; - uint32_t twenty_four_bit_color_number = green << 16 | red << 8 | blue; - for (int i = 0; i < 24; i++) { - sendBit((twenty_four_bit_color_number >> (23 - i)) & 1); - } - GPIOB->clr = WS2812_PIN; - UTILITY_TIMER->div = CPU_FREQUENCY_MHZ; - UTILITY_TIMER->swevt |= TMR_OVERFLOW_SWTRIG; - __enable_irq(); + __disable_irq(); + UTILITY_TIMER->div = 0; + UTILITY_TIMER->swevt |= TMR_OVERFLOW_SWTRIG; + uint32_t twenty_four_bit_color_number = green << 16 | red << 8 | blue; + for (int i = 0; i < 24; i++) { + sendBit((twenty_four_bit_color_number >> (23 - i)) & 1); + } + GPIOB->clr = WS2812_PIN; + UTILITY_TIMER->div = CPU_FREQUENCY_MHZ; + UTILITY_TIMER->swevt |= TMR_OVERFLOW_SWTRIG; + __enable_irq(); } void WS2812_Init(void) { - gpio_mode_QUICK(GPIOB, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, WS2812_PIN); + gpio_mode_QUICK(GPIOB, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, WS2812_PIN); } #endif diff --git a/Mcu/f421/Src/at32f421_it.c b/Mcu/f421/Src/at32f421_it.c index c9268d65..567a8b86 100644 --- a/Mcu/f421/Src/at32f421_it.c +++ b/Mcu/f421/Src/at32f421_it.c @@ -24,9 +24,9 @@ int exti_int = 0; void HardFault_Handler(void) { - /* Go to infinite loop when Hard Fault exception occurs */ - while (1) { - } + /* Go to infinite loop when Hard Fault exception occurs */ + while (1) { + } } /** @@ -36,9 +36,9 @@ void HardFault_Handler(void) */ void MemManage_Handler(void) { - /* Go to infinite loop when Memory Manage exception occurs */ - while (1) { - } + /* Go to infinite loop when Memory Manage exception occurs */ + while (1) { + } } /** @@ -48,9 +48,9 @@ void MemManage_Handler(void) */ void BusFault_Handler(void) { - /* Go to infinite loop when Bus Fault exception occurs */ - while (1) { - } + /* Go to infinite loop when Bus Fault exception occurs */ + while (1) { + } } /** @@ -60,9 +60,9 @@ void BusFault_Handler(void) */ void UsageFault_Handler(void) { - /* Go to infinite loop when Usage Fault exception occurs */ - while (1) { - } + /* Go to infinite loop when Usage Fault exception occurs */ + while (1) { + } } /** @@ -95,79 +95,79 @@ void SysTick_Handler(void) { } void DMA1_Channel1_IRQHandler(void) { - if (dma_flag_get(DMA1_FDT1_FLAG) == SET) { - DMA1->clr = DMA1_GL1_FLAG; + if (dma_flag_get(DMA1_FDT1_FLAG) == SET) { + DMA1->clr = DMA1_GL1_FLAG; #ifdef USE_ADC - ADC_DMA_Callback(); + ADC_DMA_Callback(); #endif - if (dma_flag_get(DMA1_DTERR1_FLAG) == SET) { - DMA1->clr = DMA1_GL1_FLAG; - } + if (dma_flag_get(DMA1_DTERR1_FLAG) == SET) { + DMA1->clr = DMA1_GL1_FLAG; } + } } void DMA1_Channel3_2_IRQHandler(void) { - if (dma_flag_get(DMA1_FDT2_FLAG) == SET) { - DMA1->clr = DMA1_GL2_FLAG; - DMA1_CHANNEL2->ctrl_bit.chen = FALSE; - } - if (dma_flag_get(DMA1_DTERR2_FLAG) == SET) { - DMA1->clr = DMA1_GL2_FLAG; - DMA1_CHANNEL2->ctrl_bit.chen = FALSE; - } + if (dma_flag_get(DMA1_FDT2_FLAG) == SET) { + DMA1->clr = DMA1_GL2_FLAG; + DMA1_CHANNEL2->ctrl_bit.chen = FALSE; + } + if (dma_flag_get(DMA1_DTERR2_FLAG) == SET) { + DMA1->clr = DMA1_GL2_FLAG; + DMA1_CHANNEL2->ctrl_bit.chen = FALSE; + } } void DMA1_Channel5_4_IRQHandler(void) { #ifdef USE_TIMER_15_CHANNEL_1 - if (dshot) { - DMA1->clr = DMA1_GL5_FLAG; - INPUT_DMA_CHANNEL->ctrl_bit.chen = FALSE; - transfercomplete(); - EXINT->swtrg = EXINT_LINE_15; - return; - } - // if (dma_flag_get(DMA1_HDT5_FLAG) == SET) { - // if (servoPwm) { - // IC_TIMER_REGISTER->cctrl_bit.c1p = TMR_INPUT_FALLING_EDGE; - // DMA1->clr = DMA1_HDT5_FLAG; - // } - // } + if (dshot) { + DMA1->clr = DMA1_GL5_FLAG; + INPUT_DMA_CHANNEL->ctrl_bit.chen = FALSE; + transfercomplete(); + EXINT->swtrg = EXINT_LINE_15; + return; + } + // if (dma_flag_get(DMA1_HDT5_FLAG) == SET) { + // if (servoPwm) { + // IC_TIMER_REGISTER->cctrl_bit.c1p = TMR_INPUT_FALLING_EDGE; + // DMA1->clr = DMA1_HDT5_FLAG; + // } + // } - if (dma_flag_get(DMA1_FDT5_FLAG) == SET) { - DMA1->clr = DMA1_GL5_FLAG; - INPUT_DMA_CHANNEL->ctrl_bit.chen = FALSE; - transfercomplete(); - EXINT->swtrg = EXINT_LINE_15; - } - if (dma_flag_get(DMA1_DTERR5_FLAG) == SET) { - DMA1->clr = DMA1_GL5_FLAG; - } + if (dma_flag_get(DMA1_FDT5_FLAG) == SET) { + DMA1->clr = DMA1_GL5_FLAG; + INPUT_DMA_CHANNEL->ctrl_bit.chen = FALSE; + transfercomplete(); + EXINT->swtrg = EXINT_LINE_15; + } + if (dma_flag_get(DMA1_DTERR5_FLAG) == SET) { + DMA1->clr = DMA1_GL5_FLAG; + } #endif #ifdef USE_TIMER_3_CHANNEL_1 - if (dshot) { - DMA1->clr = DMA1_GL4_FLAG; - INPUT_DMA_CHANNEL->ctrl_bit.chen = FALSE; - transfercomplete(); - EXINT->swtrg = EXINT_LINE_15; - return; - } - if (dma_flag_get(DMA1_HDT4_FLAG) == SET) { - if (servoPwm) { - IC_TIMER_REGISTER->cctrl_bit.c1p = TMR_INPUT_FALLING_EDGE; - DMA1->clr = DMA1_HDT4_FLAG; - } - } - if (dma_flag_get(DMA1_FDT4_FLAG) == SET) { - DMA1->clr = DMA1_GL4_FLAG; - INPUT_DMA_CHANNEL->ctrl_bit.chen = FALSE; - transfercomplete(); - EXINT->swtrg = EXINT_LINE_15; - } - if (dma_flag_get(DMA1_DTERR4_FLAG) == SET) { - DMA1->clr = DMA1_GL4_FLAG; + if (dshot) { + DMA1->clr = DMA1_GL4_FLAG; + INPUT_DMA_CHANNEL->ctrl_bit.chen = FALSE; + transfercomplete(); + EXINT->swtrg = EXINT_LINE_15; + return; + } + if (dma_flag_get(DMA1_HDT4_FLAG) == SET) { + if (servoPwm) { + IC_TIMER_REGISTER->cctrl_bit.c1p = TMR_INPUT_FALLING_EDGE; + DMA1->clr = DMA1_HDT4_FLAG; } + } + if (dma_flag_get(DMA1_FDT4_FLAG) == SET) { + DMA1->clr = DMA1_GL4_FLAG; + INPUT_DMA_CHANNEL->ctrl_bit.chen = FALSE; + transfercomplete(); + EXINT->swtrg = EXINT_LINE_15; + } + if (dma_flag_get(DMA1_DTERR4_FLAG) == SET) { + DMA1->clr = DMA1_GL4_FLAG; + } #endif } @@ -177,12 +177,12 @@ void DMA1_Channel5_4_IRQHandler(void) */ void ADC1_CMP_IRQHandler(void) { -if ((EXINT->intsts & EXTI_LINE) != (uint32_t)RESET) { - EXINT->intsts = EXTI_LINE; - if((INTERVAL_TIMER->cval) > (commutation_interval >> 1)){ - interruptRoutine(); + if ((EXINT->intsts & EXTI_LINE) != (uint32_t)RESET) { + EXINT->intsts = EXTI_LINE; + if ((INTERVAL_TIMER->cval) > (commutation_interval >> 1)) { + interruptRoutine(); } - } + } } /** @@ -190,23 +190,23 @@ if ((EXINT->intsts & EXTI_LINE) != (uint32_t)RESET) { */ void TMR14_GLOBAL_IRQHandler(void) { - TMR14->ists = (uint16_t)~TMR_OVF_FLAG; - tenKhzRoutine(); + TMR14->ists = (uint16_t)~TMR_OVF_FLAG; + tenKhzRoutine(); } /** * @brief This function handles TIM14 global interrupt. */ void TMR16_GLOBAL_IRQHandler(void) -{ - TMR16->ists = 0x00; - PeriodElapsedCallback(); +{ + TMR16->ists = 0x00; + PeriodElapsedCallback(); } void TMR15_GLOBAL_IRQHandler(void) { - TMR15->ists = (uint16_t)~TMR_OVF_FLAG; - TMR15->ists = (uint16_t)~TMR_C1_FLAG; + TMR15->ists = (uint16_t)~TMR_OVF_FLAG; + TMR15->ists = (uint16_t)~TMR_C1_FLAG; } /** @@ -215,22 +215,22 @@ void TMR15_GLOBAL_IRQHandler(void) */ void USART1_IRQHandler(void) { - /* USER CODE BEGIN USART1_IRQn 0 */ + /* USER CODE BEGIN USART1_IRQn 0 */ - /* USER CODE END USART1_IRQn 0 */ - /* USER CODE BEGIN USART1_IRQn 1 */ + /* USER CODE END USART1_IRQn 0 */ + /* USER CODE BEGIN USART1_IRQn 1 */ - /* USER CODE END USART1_IRQn 1 */ + /* USER CODE END USART1_IRQn 1 */ } void TMR3_GLOBAL_IRQHandler(void) { - if ((TMR3->ists & TMR_C1_FLAG) != (uint16_t)RESET) { - TMR3->ists = (uint16_t)~TMR_C1_FLAG; - } - if ((TMR3->ists & TMR_OVF_FLAG) != (uint16_t)RESET) { - TMR3->ists = (uint16_t)~TMR_OVF_FLAG; - } + if ((TMR3->ists & TMR_C1_FLAG) != (uint16_t)RESET) { + TMR3->ists = (uint16_t)~TMR_C1_FLAG; + } + if ((TMR3->ists & TMR_OVF_FLAG) != (uint16_t)RESET) { + TMR3->ists = (uint16_t)~TMR_OVF_FLAG; + } } // void DMA_Channel0_IRQHandler(void) // ADC @@ -256,11 +256,11 @@ void TMR3_GLOBAL_IRQHandler(void) void EXINT15_4_IRQHandler(void) { - exti_int++; - if ((EXINT->intsts & EXINT_LINE_15) != (uint32_t)RESET) { - EXINT->intsts = EXINT_LINE_15; - processDshot(); - } + exti_int++; + if ((EXINT->intsts & EXINT_LINE_15) != (uint32_t)RESET) { + EXINT->intsts = EXINT_LINE_15; + processDshot(); + } } /******************************************************************************/ diff --git a/Mcu/f421/Src/comparator.c b/Mcu/f421/Src/comparator.c index bd4fcbf8..7fe69d1d 100644 --- a/Mcu/f421/Src/comparator.c +++ b/Mcu/f421/Src/comparator.c @@ -9,36 +9,42 @@ #include "targets.h" -uint8_t getCompOutputLevel() { return CMP->ctrlsts_bit.cmpvalue; } +uint8_t getCompOutputLevel() +{ + return CMP->ctrlsts_bit.cmpvalue; +} void maskPhaseInterrupts() { - EXINT->inten &= ~EXTI_LINE; - EXINT->intsts = EXTI_LINE; + EXINT->inten &= ~EXTI_LINE; + EXINT->intsts = EXTI_LINE; } -void enableCompInterrupts() { EXINT->inten |= EXTI_LINE; } +void enableCompInterrupts() +{ + EXINT->inten |= EXTI_LINE; +} void changeCompInput() { -// if (step == 1 || step == 4) { // c floating -// CMP->ctrlsts = PHASE_C_COMP; -// } -// if (step == 2 || step == 5) { // a floating -// CMP->ctrlsts = PHASE_A_COMP; -// } -// if (step == 3 || step == 6) { // b floating -// CMP->ctrlsts = PHASE_B_COMP; -// } -// if (rising) { + // if (step == 1 || step == 4) { // c floating + // CMP->ctrlsts = PHASE_C_COMP; + // } + // if (step == 2 || step == 5) { // a floating + // CMP->ctrlsts = PHASE_A_COMP; + // } + // if (step == 3 || step == 6) { // b floating + // CMP->ctrlsts = PHASE_B_COMP; + // } + // if (rising) { -// EXINT->polcfg1 = 0; -// EXINT->polcfg2 |= (uint32_t)EXTI_LINE; -// } else { -// // falling bemf -// EXINT->polcfg1 |= (uint32_t)EXTI_LINE; -// EXINT->polcfg2 = 0; -// } - EXINT->polcfg1 = !rising << 21; - EXINT->polcfg2 = rising << 21; + // EXINT->polcfg1 = 0; + // EXINT->polcfg2 |= (uint32_t)EXTI_LINE; + // } else { + // // falling bemf + // EXINT->polcfg1 |= (uint32_t)EXTI_LINE; + // EXINT->polcfg2 = 0; + // } + EXINT->polcfg1 = !rising << 21; + EXINT->polcfg2 = rising << 21; } diff --git a/Mcu/f421/Src/eeprom.c b/Mcu/f421/Src/eeprom.c index 8c0122de..05fe71ac 100644 --- a/Mcu/f421/Src/eeprom.c +++ b/Mcu/f421/Src/eeprom.c @@ -11,45 +11,46 @@ // uint32_t FLASH_FKEY2 =0xCDEF89AB; void save_flash_nolib(uint8_t* data, int length, uint32_t add) -{ /// todo - - // fmc_wscnt_set(2); - - // fmc_prefetch_enable(); - - uint32_t data_to_FLASH[length / 4]; - memset(data_to_FLASH, 0, length / 4); - for (int i = 0; i < length / 4; i++) { - data_to_FLASH[i] = data[i * 4 + 3] << 24 | data[i * 4 + 2] << 16 | data[i * 4 + 1] << 8 | data[i * 4]; // make 16 bit - } - volatile uint32_t data_length = length / 4; - - // unlock flash - - flash_unlock(); - - // erase page if address even divisable by 1024 - if ((add % 1024) == 0) { - flash_sector_erase(add); - } - - volatile uint32_t index = 0; - while (index < data_length) { - // fmc_word_program(add + (index*4),data_to_FLASH[index]); - flash_word_program(add + (index * 4), data_to_FLASH[index]); - // fmc_flag_clear(FMC_FLAG_END | - // FMC_FLAG_WPERR | - // FMC_FLAG_PGERR); - flash_flag_clear(FLASH_PROGRAM_ERROR | FLASH_EPP_ERROR | FLASH_OPERATE_DONE); - index++; - } - flash_lock(); +{ + /// todo + + // fmc_wscnt_set(2); + + // fmc_prefetch_enable(); + + uint32_t data_to_FLASH[length / 4]; + memset(data_to_FLASH, 0, length / 4); + for (int i = 0; i < length / 4; i++) { + data_to_FLASH[i] = data[i * 4 + 3] << 24 | data[i * 4 + 2] << 16 | data[i * 4 + 1] << 8 | data[i * 4]; // make 16 bit + } + volatile uint32_t data_length = length / 4; + + // unlock flash + + flash_unlock(); + + // erase page if address even divisable by 1024 + if ((add % 1024) == 0) { + flash_sector_erase(add); + } + + volatile uint32_t index = 0; + while (index < data_length) { + // fmc_word_program(add + (index*4),data_to_FLASH[index]); + flash_word_program(add + (index * 4), data_to_FLASH[index]); + // fmc_flag_clear(FMC_FLAG_END | + // FMC_FLAG_WPERR | + // FMC_FLAG_PGERR); + flash_flag_clear(FLASH_PROGRAM_ERROR | FLASH_EPP_ERROR | FLASH_OPERATE_DONE); + index++; + } + flash_lock(); } void read_flash_bin(uint8_t* data, uint32_t add, int out_buff_len) { - // volatile uint32_t read_data; - for (int i = 0; i < out_buff_len; i++) { - data[i] = *(uint8_t*)(add + i); - } + // volatile uint32_t read_data; + for (int i = 0; i < out_buff_len; i++) { + data[i] = *(uint8_t*)(add + i); + } } \ No newline at end of file diff --git a/Mcu/f421/Src/peripherals.c b/Mcu/f421/Src/peripherals.c index 3696fc5d..292fedb1 100644 --- a/Mcu/f421/Src/peripherals.c +++ b/Mcu/f421/Src/peripherals.c @@ -23,180 +23,183 @@ void initCorePeripherals(void) { - system_clock_config(); - - MX_GPIO_Init(); - MX_DMA_Init(); - TIM1_Init(); - TIM6_Init(); - TIM14_Init(); - AT_COMP_Init(); - TIM17_Init(); - TIM16_Init(); - - UN_TIM_Init(); + system_clock_config(); + + MX_GPIO_Init(); + MX_DMA_Init(); + TIM1_Init(); + TIM6_Init(); + TIM14_Init(); + AT_COMP_Init(); + TIM17_Init(); + TIM16_Init(); + + UN_TIM_Init(); #ifdef USE_SERIAL_TELEMETRY - telem_UART_Init(); + telem_UART_Init(); #endif #ifdef USE_LED_STRIP - WS2812_Init(); + WS2812_Init(); #endif } -void initAfterJump(void) { __enable_irq(); } +void initAfterJump(void) +{ + __enable_irq(); +} void system_clock_config(void) { - flash_psr_set(FLASH_WAIT_CYCLE_3); - crm_reset(); - crm_clock_source_enable(CRM_CLOCK_SOURCE_HICK, TRUE); - while (crm_flag_get(CRM_HICK_STABLE_FLAG) != SET) { - } - crm_pll_config(CRM_PLL_SOURCE_HICK, CRM_PLL_MULT_30); - crm_clock_source_enable(CRM_CLOCK_SOURCE_PLL, TRUE); - while (crm_flag_get(CRM_PLL_STABLE_FLAG) != SET) { - } - crm_ahb_div_set(CRM_AHB_DIV_1); - crm_apb2_div_set(CRM_APB2_DIV_1); - crm_apb1_div_set(CRM_APB1_DIV_1); - crm_auto_step_mode_enable(TRUE); - crm_sysclk_switch(CRM_SCLK_PLL); - while (crm_sysclk_switch_status_get() != CRM_SCLK_PLL) { - } - crm_auto_step_mode_enable(FALSE); - system_core_clock_update(); + flash_psr_set(FLASH_WAIT_CYCLE_3); + crm_reset(); + crm_clock_source_enable(CRM_CLOCK_SOURCE_HICK, TRUE); + while (crm_flag_get(CRM_HICK_STABLE_FLAG) != SET) { + } + crm_pll_config(CRM_PLL_SOURCE_HICK, CRM_PLL_MULT_30); + crm_clock_source_enable(CRM_CLOCK_SOURCE_PLL, TRUE); + while (crm_flag_get(CRM_PLL_STABLE_FLAG) != SET) { + } + crm_ahb_div_set(CRM_AHB_DIV_1); + crm_apb2_div_set(CRM_APB2_DIV_1); + crm_apb1_div_set(CRM_APB1_DIV_1); + crm_auto_step_mode_enable(TRUE); + crm_sysclk_switch(CRM_SCLK_PLL); + while (crm_sysclk_switch_status_get() != CRM_SCLK_PLL) { + } + crm_auto_step_mode_enable(FALSE); + system_core_clock_update(); } void AT_COMP_Init(void) { - crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE); - crm_periph_clock_enable(CRM_CMP_PERIPH_CLOCK, TRUE); - gpio_mode_QUICK(GPIOA, GPIO_MODE_ANALOG, GPIO_PULL_NONE, GPIO_PINS_1); - gpio_mode_QUICK(GPIOA, GPIO_MODE_ANALOG, GPIO_PULL_NONE, GPIO_PINS_5); - NVIC_SetPriority(ADC1_CMP_IRQn, 0); - NVIC_EnableIRQ(ADC1_CMP_IRQn); - cmp_enable(CMP1_SELECTION, TRUE); + crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_CMP_PERIPH_CLOCK, TRUE); + gpio_mode_QUICK(GPIOA, GPIO_MODE_ANALOG, GPIO_PULL_NONE, GPIO_PINS_1); + gpio_mode_QUICK(GPIOA, GPIO_MODE_ANALOG, GPIO_PULL_NONE, GPIO_PINS_5); + NVIC_SetPriority(ADC1_CMP_IRQn, 0); + NVIC_EnableIRQ(ADC1_CMP_IRQn); + cmp_enable(CMP1_SELECTION, TRUE); } void MX_IWDG_Init(void) { - WDT->cmd = WDT_CMD_UNLOCK; - WDT->cmd = WDT_CMD_ENABLE; - WDT->div = WDT_CLK_DIV_32; - WDT->rld = 4000; - WDT->cmd = WDT_CMD_RELOAD; + WDT->cmd = WDT_CMD_UNLOCK; + WDT->cmd = WDT_CMD_ENABLE; + WDT->div = WDT_CLK_DIV_32; + WDT->rld = 4000; + WDT->cmd = WDT_CMD_RELOAD; } void TIM1_Init(void) { - crm_periph_clock_enable(CRM_TMR1_PERIPH_CLOCK, TRUE); - TMR1->pr = TIM1_AUTORELOAD; - TMR1->div = 0; + crm_periph_clock_enable(CRM_TMR1_PERIPH_CLOCK, TRUE); + TMR1->pr = TIM1_AUTORELOAD; + TMR1->div = 0; - TMR1->cm1 = 0x6868; // Channel 1 and 2 in PWM output mode - TMR1->cm2 = 0x68; // channel 3 in PWM output mode + TMR1->cm1 = 0x6868; // Channel 1 and 2 in PWM output mode + TMR1->cm2 = 0x68; // channel 3 in PWM output mode #ifdef USE_INVERTED_HIGH - tmr_output_channel_polarity_set(TMR1, TMR_SELECT_CHANNEL_1, - TMR_POLARITY_ACTIVE_LOW); - tmr_output_channel_polarity_set(TMR1, TMR_SELECT_CHANNEL_2, - TMR_POLARITY_ACTIVE_LOW); - tmr_output_channel_polarity_set(TMR1, TMR_SELECT_CHANNEL_3, - TMR_POLARITY_ACTIVE_LOW); + tmr_output_channel_polarity_set(TMR1, TMR_SELECT_CHANNEL_1, + TMR_POLARITY_ACTIVE_LOW); + tmr_output_channel_polarity_set(TMR1, TMR_SELECT_CHANNEL_2, + TMR_POLARITY_ACTIVE_LOW); + tmr_output_channel_polarity_set(TMR1, TMR_SELECT_CHANNEL_3, + TMR_POLARITY_ACTIVE_LOW); #endif - tmr_output_channel_buffer_enable(TMR1, TMR_SELECT_CHANNEL_1, TRUE); - tmr_output_channel_buffer_enable(TMR1, TMR_SELECT_CHANNEL_2, TRUE); - tmr_output_channel_buffer_enable(TMR1, TMR_SELECT_CHANNEL_3, TRUE); - - tmr_period_buffer_enable(TMR1, TRUE); - TMR1->brk_bit.dtc = DEAD_TIME; - crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE); - crm_periph_clock_enable(CRM_GPIOB_PERIPH_CLOCK, TRUE); - - /*configure PA8/PA9/PA10(TIMER0/CH0/CH1/CH2) as alternate function*/ - gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_A_GPIO_LOW); - - gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_B_GPIO_LOW); - - gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_C_GPIO_LOW); - - gpio_pin_mux_config(PHASE_A_GPIO_PORT_LOW, PHASE_A_PIN_SOURCE_LOW, - GPIO_MUX_2); - gpio_pin_mux_config(PHASE_B_GPIO_PORT_LOW, PHASE_B_PIN_SOURCE_LOW, - GPIO_MUX_2); - gpio_pin_mux_config(PHASE_C_GPIO_PORT_LOW, PHASE_C_PIN_SOURCE_LOW, - GPIO_MUX_2); - - /*configure PB13/PB14/PB15(TIMER0/CH0N/CH1N/CH2N) as alternate function*/ - gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_A_GPIO_HIGH); - // gpio_output_options_set(GPIOB, GPIO_OTYPE_PP, - // GPIO_OSPEED_50MHZ,PHASE_A_GPIO_HIGH); - - gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_B_GPIO_HIGH); - // gpio_output_options_set(GPIOB, GPIO_OTYPE_PP, - // GPIO_OSPEED_50MHZ,PHASE_B_GPIO_HIGH); - - gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_C_GPIO_HIGH); - // gpio_output_options_set(GPIOB, GPIO_OTYPE_PP, - // GPIO_OSPEED_50MHZ,PHASE_C_GPIO_HIGH); - - gpio_pin_mux_config(PHASE_A_GPIO_PORT_HIGH, PHASE_A_PIN_SOURCE_HIGH, - GPIO_MUX_2); - gpio_pin_mux_config(PHASE_B_GPIO_PORT_HIGH, PHASE_B_PIN_SOURCE_HIGH, - GPIO_MUX_2); - gpio_pin_mux_config(PHASE_C_GPIO_PORT_HIGH, PHASE_C_PIN_SOURCE_HIGH, - GPIO_MUX_2); + tmr_output_channel_buffer_enable(TMR1, TMR_SELECT_CHANNEL_1, TRUE); + tmr_output_channel_buffer_enable(TMR1, TMR_SELECT_CHANNEL_2, TRUE); + tmr_output_channel_buffer_enable(TMR1, TMR_SELECT_CHANNEL_3, TRUE); + + tmr_period_buffer_enable(TMR1, TRUE); + TMR1->brk_bit.dtc = DEAD_TIME; + crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_GPIOB_PERIPH_CLOCK, TRUE); + + /*configure PA8/PA9/PA10(TIMER0/CH0/CH1/CH2) as alternate function*/ + gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_A_GPIO_LOW); + + gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_B_GPIO_LOW); + + gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_C_GPIO_LOW); + + gpio_pin_mux_config(PHASE_A_GPIO_PORT_LOW, PHASE_A_PIN_SOURCE_LOW, + GPIO_MUX_2); + gpio_pin_mux_config(PHASE_B_GPIO_PORT_LOW, PHASE_B_PIN_SOURCE_LOW, + GPIO_MUX_2); + gpio_pin_mux_config(PHASE_C_GPIO_PORT_LOW, PHASE_C_PIN_SOURCE_LOW, + GPIO_MUX_2); + + /*configure PB13/PB14/PB15(TIMER0/CH0N/CH1N/CH2N) as alternate function*/ + gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_A_GPIO_HIGH); + // gpio_output_options_set(GPIOB, GPIO_OTYPE_PP, + // GPIO_OSPEED_50MHZ,PHASE_A_GPIO_HIGH); + + gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_B_GPIO_HIGH); + // gpio_output_options_set(GPIOB, GPIO_OTYPE_PP, + // GPIO_OSPEED_50MHZ,PHASE_B_GPIO_HIGH); + + gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_C_GPIO_HIGH); + // gpio_output_options_set(GPIOB, GPIO_OTYPE_PP, + // GPIO_OSPEED_50MHZ,PHASE_C_GPIO_HIGH); + + gpio_pin_mux_config(PHASE_A_GPIO_PORT_HIGH, PHASE_A_PIN_SOURCE_HIGH, + GPIO_MUX_2); + gpio_pin_mux_config(PHASE_B_GPIO_PORT_HIGH, PHASE_B_PIN_SOURCE_HIGH, + GPIO_MUX_2); + gpio_pin_mux_config(PHASE_C_GPIO_PORT_HIGH, PHASE_C_PIN_SOURCE_HIGH, + GPIO_MUX_2); } void TIM6_Init(void) { - crm_periph_clock_enable(CRM_TMR6_PERIPH_CLOCK, TRUE); - TMR6->pr = 0xFFFF; - TMR6->div = 59; + crm_periph_clock_enable(CRM_TMR6_PERIPH_CLOCK, TRUE); + TMR6->pr = 0xFFFF; + TMR6->div = 59; } void TIM14_Init(void) { - crm_periph_clock_enable(CRM_TMR14_PERIPH_CLOCK, TRUE); - TMR14->pr = 1000000 / LOOP_FREQUENCY_HZ; - TMR14->div = 119; + crm_periph_clock_enable(CRM_TMR14_PERIPH_CLOCK, TRUE); + TMR14->pr = 1000000 / LOOP_FREQUENCY_HZ; + TMR14->div = 119; - NVIC_SetPriority(TMR14_GLOBAL_IRQn, 3); - NVIC_EnableIRQ(TMR14_GLOBAL_IRQn); + NVIC_SetPriority(TMR14_GLOBAL_IRQn, 3); + NVIC_EnableIRQ(TMR14_GLOBAL_IRQn); - // TMR_Cmd(TMR14, ENABLE); + // TMR_Cmd(TMR14, ENABLE); } void TIM16_Init(void) { - crm_periph_clock_enable(CRM_TMR16_PERIPH_CLOCK, TRUE); - TMR16->pr = 500; - TMR16->div = 59; - NVIC_SetPriority(TMR16_GLOBAL_IRQn, 0); - NVIC_EnableIRQ(TMR16_GLOBAL_IRQn); + crm_periph_clock_enable(CRM_TMR16_PERIPH_CLOCK, TRUE); + TMR16->pr = 500; + TMR16->div = 59; + NVIC_SetPriority(TMR16_GLOBAL_IRQn, 0); + NVIC_EnableIRQ(TMR16_GLOBAL_IRQn); } void TIM17_Init(void) { - crm_periph_clock_enable(CRM_TMR17_PERIPH_CLOCK, TRUE); - TMR17->pr = 0xFFFF; - TMR17->div = 119; - TMR17->ctrl1_bit.prben = TRUE; + crm_periph_clock_enable(CRM_TMR17_PERIPH_CLOCK, TRUE); + TMR17->pr = 0xFFFF; + TMR17->div = 119; + TMR17->ctrl1_bit.prben = TRUE; - // TMR_Cmd(TMR15, ENABLE); + // TMR_Cmd(TMR15, ENABLE); } void MX_DMA_Init(void) { - crm_periph_clock_enable(CRM_DMA1_PERIPH_CLOCK, TRUE); - NVIC_SetPriority(DMA1_Channel5_4_IRQn, 1); - NVIC_EnableIRQ(DMA1_Channel5_4_IRQn); + crm_periph_clock_enable(CRM_DMA1_PERIPH_CLOCK, TRUE); + NVIC_SetPriority(DMA1_Channel5_4_IRQn, 1); + NVIC_EnableIRQ(DMA1_Channel5_4_IRQn); } void MX_GPIO_Init(void) { } @@ -204,134 +207,143 @@ void MX_GPIO_Init(void) { } void UN_TIM_Init(void) { #ifdef USE_TIMER_3_CHANNEL_1 - crm_periph_clock_enable(CRM_GPIOB_PERIPH_CLOCK, TRUE); - crm_periph_clock_enable(CRM_TMR3_PERIPH_CLOCK, TRUE); - gpio_mode_QUICK(INPUT_PIN_PORT, GPIO_MODE_MUX, GPIO_PULL_NONE, INPUT_PIN); - gpio_pin_mux_config(INPUT_PIN_PORT, INPUT_PIN_SOURCE, GPIO_MUX_1); + crm_periph_clock_enable(CRM_GPIOB_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_TMR3_PERIPH_CLOCK, TRUE); + gpio_mode_QUICK(INPUT_PIN_PORT, GPIO_MODE_MUX, GPIO_PULL_NONE, INPUT_PIN); + gpio_pin_mux_config(INPUT_PIN_PORT, INPUT_PIN_SOURCE, GPIO_MUX_1); #endif #ifdef USE_TIMER_15_CHANNEL_1 - crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE); - crm_periph_clock_enable(CRM_TMR15_PERIPH_CLOCK, TRUE); - gpio_mode_QUICK(INPUT_PIN_PORT, GPIO_MODE_MUX, GPIO_PULL_NONE, INPUT_PIN); + crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_TMR15_PERIPH_CLOCK, TRUE); + gpio_mode_QUICK(INPUT_PIN_PORT, GPIO_MODE_MUX, GPIO_PULL_NONE, INPUT_PIN); #endif - // RCC_AHBPeriphClockCmd(RCC_AHBPERIPH_DMA1,ENABLE); - - crm_periph_clock_enable(CRM_DMA1_PERIPH_CLOCK, TRUE); - INPUT_DMA_CHANNEL->ctrl = 0X98a; // PERIPHERAL HALF WORD, MEMROY WORD , - // MEMORY INC ENABLE , TC AND ERROR INTS - NVIC_SetPriority(IC_DMA_IRQ_NAME, 1); - NVIC_EnableIRQ(IC_DMA_IRQ_NAME); - IC_TIMER_REGISTER->pr = 0xFFFF; - IC_TIMER_REGISTER->div = 16; - IC_TIMER_REGISTER->ctrl1_bit.prben = TRUE; - IC_TIMER_REGISTER->ctrl1_bit.tmren = TRUE; + // RCC_AHBPeriphClockCmd(RCC_AHBPERIPH_DMA1,ENABLE); + + crm_periph_clock_enable(CRM_DMA1_PERIPH_CLOCK, TRUE); + INPUT_DMA_CHANNEL->ctrl = 0X98a; // PERIPHERAL HALF WORD, MEMROY WORD , + // MEMORY INC ENABLE , TC AND ERROR INTS + NVIC_SetPriority(IC_DMA_IRQ_NAME, 1); + NVIC_EnableIRQ(IC_DMA_IRQ_NAME); + IC_TIMER_REGISTER->pr = 0xFFFF; + IC_TIMER_REGISTER->div = 16; + IC_TIMER_REGISTER->ctrl1_bit.prben = TRUE; + IC_TIMER_REGISTER->ctrl1_bit.tmren = TRUE; } #ifdef USE_RGB_LED // has 3 color led void LED_GPIO_init() { - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - - /* GPIO Ports Clock Enable */ - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); - - LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_8); - LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_5); - LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_3); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_8; - GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_5; - GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_3; - GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + + /* GPIO Ports Clock Enable */ + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); + + LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_8); + LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_5); + LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_3); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_8; + GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_5; + GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_3; + GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); } #endif void reloadWatchDogCounter() { - WDT->cmd = WDT_CMD_RELOAD; + WDT->cmd = WDT_CMD_RELOAD; } -void setPWMCompare1(uint16_t compareone) { TMR1->c1dt = compareone; } -void setPWMCompare2(uint16_t comparetwo) { TMR1->c2dt = comparetwo; } -void setPWMCompare3(uint16_t comparethree) { TMR1->c3dt = comparethree; } +void setPWMCompare1(uint16_t compareone) +{ + TMR1->c1dt = compareone; +} +void setPWMCompare2(uint16_t comparetwo) +{ + TMR1->c2dt = comparetwo; +} +void setPWMCompare3(uint16_t comparethree) +{ + TMR1->c3dt = comparethree; +} void generatePwmTimerEvent() { - TMR1->swevt |= TMR_OVERFLOW_SWTRIG; - ; + TMR1->swevt |= TMR_OVERFLOW_SWTRIG; + ; } void resetInputCaptureTimer() { - IC_TIMER_REGISTER->pr = 0; - IC_TIMER_REGISTER->cval = 0; + IC_TIMER_REGISTER->pr = 0; + IC_TIMER_REGISTER->cval = 0; } void enableCorePeripherals() { - tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_1, TRUE); - tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_2, TRUE); - tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_3, TRUE); - tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_1C, TRUE); - tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_2C, TRUE); - tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_3C, TRUE); - - TMR1->ctrl1_bit.tmren = TRUE; - TMR1->brk_bit.oen = TRUE; - TMR1->swevt |= TMR_OVERFLOW_SWTRIG; + tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_1, TRUE); + tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_2, TRUE); + tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_3, TRUE); + tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_1C, TRUE); + tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_2C, TRUE); + tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_3C, TRUE); + + TMR1->ctrl1_bit.tmren = TRUE; + TMR1->brk_bit.oen = TRUE; + TMR1->swevt |= TMR_OVERFLOW_SWTRIG; #ifdef USE_RGB_LED - LED_GPIO_init(); - GPIOB->scr = LL_GPIO_PIN_8; // turn on red - GPIOB->clr = LL_GPIO_PIN_5; - GPIOB->clr = LL_GPIO_PIN_3; // + LED_GPIO_init(); + GPIOB->scr = LL_GPIO_PIN_8; // turn on red + GPIOB->clr = LL_GPIO_PIN_5; + GPIOB->clr = LL_GPIO_PIN_3; // #endif #ifndef BRUSHED_MODE - COM_TIMER->ctrl1_bit.tmren = TRUE; - COM_TIMER->swevt |= TMR_OVERFLOW_SWTRIG; - COM_TIMER->iden &= ~TMR_OVF_INT; + COM_TIMER->ctrl1_bit.tmren = TRUE; + COM_TIMER->swevt |= TMR_OVERFLOW_SWTRIG; + COM_TIMER->iden &= ~TMR_OVF_INT; #endif - UTILITY_TIMER->ctrl1_bit.tmren = TRUE; - INTERVAL_TIMER->ctrl1_bit.tmren = TRUE; - INTERVAL_TIMER->swevt |= TMR_OVERFLOW_SWTRIG; - TEN_KHZ_TIMER->ctrl1_bit.tmren = TRUE; - TEN_KHZ_TIMER->swevt |= TMR_OVERFLOW_SWTRIG; - TEN_KHZ_TIMER->iden |= TMR_OVF_INT; + UTILITY_TIMER->ctrl1_bit.tmren = TRUE; + INTERVAL_TIMER->ctrl1_bit.tmren = TRUE; + INTERVAL_TIMER->swevt |= TMR_OVERFLOW_SWTRIG; + TEN_KHZ_TIMER->ctrl1_bit.tmren = TRUE; + TEN_KHZ_TIMER->swevt |= TMR_OVERFLOW_SWTRIG; + TEN_KHZ_TIMER->iden |= TMR_OVF_INT; #ifdef USE_ADC - ADC_Init(); + ADC_Init(); #endif #ifdef USE_ADC_INPUT #else - tmr_channel_enable(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, TRUE); - IC_TIMER_REGISTER->ctrl1_bit.tmren = TRUE; + tmr_channel_enable(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, TRUE); + IC_TIMER_REGISTER->ctrl1_bit.tmren = TRUE; #endif - NVIC_SetPriority(EXINT15_4_IRQn, 2); - NVIC_EnableIRQ(EXINT15_4_IRQn); - EXINT->inten |= EXINT_LINE_15; - + NVIC_SetPriority(EXINT15_4_IRQn, 2); + NVIC_EnableIRQ(EXINT15_4_IRQn); + EXINT->inten |= EXINT_LINE_15; + #ifdef USE_PULSE_OUT - gpio_mode_QUICK(GPIOB, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, GPIO_PINS_8); + gpio_mode_QUICK(GPIOB, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, GPIO_PINS_8); #endif } diff --git a/Mcu/f421/Src/phaseouts.c b/Mcu/f421/Src/phaseouts.c index 9f2eb521..b3e42293 100644 --- a/Mcu/f421/Src/phaseouts.c +++ b/Mcu/f421/Src/phaseouts.c @@ -30,33 +30,34 @@ extern char prop_brake_active; #endif void proportionalBrake() -{ // alternate all channels between braking (ABC LOW) - // and coasting (ABC float) put lower channel into - // alternate mode and turn upper OFF for each - // channel - // turn all HIGH channels off for ABC - - // gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, - // GPIO_PULL_NONE, PHASE_A_GPIO_HIGH); - gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_A_GPIO_HIGH); - PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; - - gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_B_GPIO_HIGH); - PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; - - gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_C_GPIO_HIGH); - PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; - - // set low channel to PWM, duty cycle will now control braking - gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_A_GPIO_LOW); - gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_B_GPIO_LOW); - gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_C_GPIO_LOW); +{ + // alternate all channels between braking (ABC LOW) + // and coasting (ABC float) put lower channel into + // alternate mode and turn upper OFF for each + // channel + // turn all HIGH channels off for ABC + + // gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, + // GPIO_PULL_NONE, PHASE_A_GPIO_HIGH); + gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_A_GPIO_HIGH); + PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; + + gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_B_GPIO_HIGH); + PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; + + gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_C_GPIO_HIGH); + PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; + + // set low channel to PWM, duty cycle will now control braking + gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_A_GPIO_LOW); + gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_B_GPIO_LOW); + gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_C_GPIO_LOW); } // void phaseCPWM() { @@ -87,35 +88,35 @@ void proportionalBrake() void phaseBPWM() { - if (!eepromBuffer.comp_pwm) { - gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_B_GPIO_LOW); - PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; - } else { - gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_B_GPIO_LOW); // low - } - gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_B_GPIO_HIGH); // high + if (!eepromBuffer.comp_pwm) { + gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_B_GPIO_LOW); + PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; + } else { + gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_B_GPIO_LOW); // low + } + gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_B_GPIO_HIGH); // high } void phaseBFLOAT() { - gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_B_GPIO_LOW); - PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; - gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_B_GPIO_HIGH); - PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; + gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_B_GPIO_LOW); + PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; + gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_B_GPIO_HIGH); + PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; } void phaseBLOW() { - // low mosfet on - gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_B_GPIO_LOW); - PHASE_B_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_B_GPIO_LOW; - gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_B_GPIO_HIGH); - PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; + // low mosfet on + gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_B_GPIO_LOW); + PHASE_B_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_B_GPIO_LOW; + gpio_mode_QUICK(PHASE_B_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_B_GPIO_HIGH); + PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; } //////////////////////////////PHASE @@ -123,38 +124,38 @@ void phaseBLOW() void phaseCPWM() { - if (!eepromBuffer.comp_pwm) { - gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_C_GPIO_LOW); - PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; - } else { - gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_C_GPIO_LOW); - ; - } - gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_C_GPIO_HIGH); + if (!eepromBuffer.comp_pwm) { + gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_C_GPIO_LOW); + PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; + } else { + gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_C_GPIO_LOW); + ; + } + gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_C_GPIO_HIGH); } void phaseCFLOAT() { - // floating - gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_C_GPIO_LOW); - PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; - gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_C_GPIO_HIGH); - PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; + // floating + gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_C_GPIO_LOW); + PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; + gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_C_GPIO_HIGH); + PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; } void phaseCLOW() { - gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_C_GPIO_LOW); - PHASE_C_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_C_GPIO_LOW; - gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_C_GPIO_HIGH); - PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; + gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_C_GPIO_LOW); + PHASE_C_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_C_GPIO_LOW; + gpio_mode_QUICK(PHASE_C_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_C_GPIO_HIGH); + PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; } ///////////////////////////////////////////////PHASE 3 @@ -162,36 +163,36 @@ void phaseCLOW() void phaseAPWM() { - if (!eepromBuffer.comp_pwm) { - gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_A_GPIO_LOW); - PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; - } else { - gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_A_GPIO_LOW); - } - gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, - PHASE_A_GPIO_HIGH); + if (!eepromBuffer.comp_pwm) { + gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_A_GPIO_LOW); + PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; + } else { + gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_A_GPIO_LOW); + } + gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_MUX, GPIO_PULL_NONE, + PHASE_A_GPIO_HIGH); } void phaseAFLOAT() { - gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_A_GPIO_LOW); - PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; - gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_A_GPIO_HIGH); - PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; + gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_A_GPIO_LOW); + PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; + gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_A_GPIO_HIGH); + PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; } void phaseALOW() { - gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_A_GPIO_LOW); - PHASE_A_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_A_GPIO_LOW; - gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, - PHASE_A_GPIO_HIGH); - PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; + gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_A_GPIO_LOW); + PHASE_A_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_A_GPIO_LOW; + gpio_mode_QUICK(PHASE_A_GPIO_PORT_HIGH, GPIO_MODE_OUTPUT, GPIO_PULL_NONE, + PHASE_A_GPIO_HIGH); + PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; } #else @@ -199,38 +200,38 @@ void phaseALOW() //////////////////////////////////PHASE 1////////////////////// void phaseBPWM() { - if (!eepromBuffer.comp_pwm) { // for future - // gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, - // GPIO_PULL_NONE, PHASE_B_GPIO_LOW); - // PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); // high pwm + if (!eepromBuffer.comp_pwm) { // for future + // gpio_mode_QUICK(PHASE_B_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, + // GPIO_PULL_NONE, PHASE_B_GPIO_LOW); + // PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); // high pwm } void phaseBFLOAT() { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable off - PHASE_B_GPIO_PORT_ENABLE->BRR = PHASE_B_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); // pwm off - PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable off + PHASE_B_GPIO_PORT_ENABLE->BRR = PHASE_B_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); // pwm off + PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; } void phaseBLOW() { - // low mosfet on - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); // pwm off - PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; + // low mosfet on + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); // pwm off + PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; } //////////////////////////////PHASE @@ -238,39 +239,39 @@ void phaseBLOW() void phaseCPWM() { - if (!eepromBuffer.comp_pwm) { - // gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, - // GPIO_PULL_NONE, - // PHASE_C_GPIO_LOW); PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = - // PHASE_C_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + // gpio_mode_QUICK(PHASE_C_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, + // GPIO_PULL_NONE, + // PHASE_C_GPIO_LOW); PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = + // PHASE_C_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); } void phaseCFLOAT() { - // floating - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable off - PHASE_C_GPIO_PORT_ENABLE->BRR = PHASE_C_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; + // floating + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable off + PHASE_C_GPIO_PORT_ENABLE->BRR = PHASE_C_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; } void phaseCLOW() { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; } ///////////////////////////////////////////////PHASE 3 @@ -278,120 +279,122 @@ void phaseCLOW() void phaseAPWM() { - if (!eepromBuffer.comp_pwm) { - // gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, - // GPIO_PULL_NONE, - // PHASE_A_GPIO_LOW); PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = - // PHASE_A_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + // gpio_mode_QUICK(PHASE_A_GPIO_PORT_LOW, GPIO_MODE_OUTPUT, + // GPIO_PULL_NONE, + // PHASE_A_GPIO_LOW); PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = + // PHASE_A_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); } void phaseAFLOAT() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BRR = PHASE_A_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BRR = PHASE_A_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; } void phaseALOW() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; } #endif void allOff() { - phaseAFLOAT(); - phaseBFLOAT(); - phaseCFLOAT(); + phaseAFLOAT(); + phaseBFLOAT(); + phaseCFLOAT(); } void comStep(int newStep) { - switch (newStep) { - case 1: // A-B - phaseCFLOAT(); - phaseBLOW(); - phaseAPWM(); - CMP->ctrlsts = PHASE_C_COMP; - break; - - case 2: // C-B - phaseAFLOAT(); - phaseBLOW(); - phaseCPWM(); - CMP->ctrlsts = PHASE_A_COMP; - break; - - case 3: // C-A - phaseBFLOAT(); - phaseALOW(); - phaseCPWM(); - CMP->ctrlsts = PHASE_B_COMP; - break; - - case 4: // B-A - phaseCFLOAT(); - phaseALOW(); - phaseBPWM(); - CMP->ctrlsts = PHASE_C_COMP; - break; - - case 5: // B-C - phaseAFLOAT(); - phaseCLOW(); - phaseBPWM(); - CMP->ctrlsts = PHASE_A_COMP; - break; - - case 6: // A-C - phaseBFLOAT(); - phaseCLOW(); - phaseAPWM(); - CMP->ctrlsts = PHASE_B_COMP; - break; - } -} + switch (newStep) { + case 1: // A-B + phaseCFLOAT(); + phaseBLOW(); + phaseAPWM(); + CMP->ctrlsts = PHASE_C_COMP; + break; -void fullBrake() -{ // full braking shorting all low sides - phaseALOW(); + case 2: // C-B + phaseAFLOAT(); phaseBLOW(); + phaseCPWM(); + CMP->ctrlsts = PHASE_A_COMP; + break; + + case 3: // C-A + phaseBFLOAT(); + phaseALOW(); + phaseCPWM(); + CMP->ctrlsts = PHASE_B_COMP; + break; + + case 4: // B-A + phaseCFLOAT(); + phaseALOW(); + phaseBPWM(); + CMP->ctrlsts = PHASE_C_COMP; + break; + + case 5: // B-C + phaseAFLOAT(); phaseCLOW(); + phaseBPWM(); + CMP->ctrlsts = PHASE_A_COMP; + break; + + case 6: // A-C + phaseBFLOAT(); + phaseCLOW(); + phaseAPWM(); + CMP->ctrlsts = PHASE_B_COMP; + break; + } +} + +void fullBrake() +{ + // full braking shorting all low sides + phaseALOW(); + phaseBLOW(); + phaseCLOW(); } void allpwm() -{ // for stepper_sine - phaseAPWM(); - phaseBPWM(); - phaseCPWM(); +{ + // for stepper_sine + phaseAPWM(); + phaseBPWM(); + phaseCPWM(); } void twoChannelForward() { - phaseAPWM(); - phaseBLOW(); - phaseCPWM(); + phaseAPWM(); + phaseBLOW(); + phaseCPWM(); } void twoChannelReverse() { - phaseALOW(); - phaseBPWM(); - phaseCLOW(); + phaseALOW(); + phaseBPWM(); + phaseCLOW(); } diff --git a/Mcu/f421/Src/serial_telemetry.c b/Mcu/f421/Src/serial_telemetry.c index f9750d06..0059c226 100644 --- a/Mcu/f421/Src/serial_telemetry.c +++ b/Mcu/f421/Src/serial_telemetry.c @@ -11,92 +11,95 @@ uint8_t aTxBuffer[10]; uint8_t nbDataToTransmit = sizeof(aTxBuffer); void send_telem_DMA() -{ // set data length and enable channel to start transfer - DMA1_CHANNEL2->dtcnt = nbDataToTransmit; - DMA1_CHANNEL2->ctrl_bit.chen = TRUE; +{ + // set data length and enable channel to start transfer + DMA1_CHANNEL2->dtcnt = nbDataToTransmit; + DMA1_CHANNEL2->ctrl_bit.chen = TRUE; } uint8_t update_crc8(uint8_t crc, uint8_t crc_seed) { - uint8_t crc_u, i; - crc_u = crc; - crc_u ^= crc_seed; - for (i = 0; i < 8; i++) - crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); - return (crc_u); + uint8_t crc_u, i; + crc_u = crc; + crc_u ^= crc_seed; + for (i = 0; i < 8; i++) { + crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); + } + return (crc_u); } uint8_t get_crc8(uint8_t* Buf, uint8_t BufLen) { - uint8_t crc = 0, i; - for (i = 0; i < BufLen; i++) - crc = update_crc8(Buf[i], crc); - return (crc); + uint8_t crc = 0, i; + for (i = 0; i < BufLen; i++) { + crc = update_crc8(Buf[i], crc); + } + return (crc); } void makeTelemPackage(uint8_t temp, uint16_t voltage, uint16_t current, - uint16_t consumption, uint16_t e_rpm) + uint16_t consumption, uint16_t e_rpm) { - aTxBuffer[0] = temp; // temperature + aTxBuffer[0] = temp; // temperature - aTxBuffer[1] = (voltage >> 8) & 0xFF; // voltage hB - aTxBuffer[2] = voltage & 0xFF; // voltage lowB + aTxBuffer[1] = (voltage >> 8) & 0xFF; // voltage hB + aTxBuffer[2] = voltage & 0xFF; // voltage lowB - aTxBuffer[3] = (current >> 8) & 0xFF; // current - aTxBuffer[4] = current & 0xFF; // divide by 10 for Amps + aTxBuffer[3] = (current >> 8) & 0xFF; // current + aTxBuffer[4] = current & 0xFF; // divide by 10 for Amps - aTxBuffer[5] = (consumption >> 8) & 0xFF; // consumption - aTxBuffer[6] = consumption & 0xFF; // in mah + aTxBuffer[5] = (consumption >> 8) & 0xFF; // consumption + aTxBuffer[6] = consumption & 0xFF; // in mah - aTxBuffer[7] = (e_rpm >> 8) & 0xFF; // - aTxBuffer[8] = e_rpm & 0xFF; // eRpM *100 + aTxBuffer[7] = (e_rpm >> 8) & 0xFF; // + aTxBuffer[8] = e_rpm & 0xFF; // eRpM *100 - aTxBuffer[9] = get_crc8(aTxBuffer, 9); + aTxBuffer[9] = get_crc8(aTxBuffer, 9); } void telem_UART_Init(void) { - gpio_init_type gpio_init_struct; - - crm_periph_clock_enable(CRM_USART1_PERIPH_CLOCK, TRUE); - crm_periph_clock_enable(CRM_GPIOB_PERIPH_CLOCK, TRUE); - crm_periph_clock_enable(CRM_DMA1_PERIPH_CLOCK, TRUE); - - /* configure the usart2 tx pin */ - gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER; - gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL; - gpio_init_struct.gpio_mode = GPIO_MODE_MUX; - gpio_init_struct.gpio_pins = GPIO_PINS_6; - gpio_init_struct.gpio_pull = GPIO_PULL_UP; - gpio_init(GPIOB, &gpio_init_struct); - gpio_pin_mux_config(GPIOB, GPIO_PINS_SOURCE6, GPIO_MUX_0); - - dma_reset(DMA1_CHANNEL2); - - dma_init_type dma_init_struct; - dma_default_para_init(&dma_init_struct); - dma_init_struct.buffer_size = nbDataToTransmit; - dma_init_struct.direction = DMA_DIR_MEMORY_TO_PERIPHERAL; - dma_init_struct.memory_base_addr = (uint32_t)&aTxBuffer; - dma_init_struct.memory_data_width = DMA_MEMORY_DATA_WIDTH_BYTE; - dma_init_struct.memory_inc_enable = TRUE; - dma_init_struct.peripheral_base_addr = (uint32_t)&USART1->dt; - dma_init_struct.peripheral_data_width = DMA_PERIPHERAL_DATA_WIDTH_BYTE; - dma_init_struct.peripheral_inc_enable = FALSE; - dma_init_struct.priority = DMA_PRIORITY_LOW; - dma_init_struct.loop_mode_enable = FALSE; - dma_init(DMA1_CHANNEL2, &dma_init_struct); - - DMA1_CHANNEL2->ctrl |= DMA_FDT_INT; - DMA1_CHANNEL2->ctrl |= DMA_DTERR_INT; - - /* configure usart1 param */ - usart_init(USART1, 115200, USART_DATA_8BITS, USART_STOP_1_BIT); - usart_transmitter_enable(USART1, TRUE); - usart_receiver_enable(USART1, TRUE); - usart_single_line_halfduplex_select(USART1, TRUE); - usart_dma_transmitter_enable(USART1, TRUE); - usart_enable(USART1, TRUE); - - nvic_irq_enable(DMA1_Channel3_2_IRQn, 3, 0); + gpio_init_type gpio_init_struct; + + crm_periph_clock_enable(CRM_USART1_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_GPIOB_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_DMA1_PERIPH_CLOCK, TRUE); + + /* configure the usart2 tx pin */ + gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER; + gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL; + gpio_init_struct.gpio_mode = GPIO_MODE_MUX; + gpio_init_struct.gpio_pins = GPIO_PINS_6; + gpio_init_struct.gpio_pull = GPIO_PULL_UP; + gpio_init(GPIOB, &gpio_init_struct); + gpio_pin_mux_config(GPIOB, GPIO_PINS_SOURCE6, GPIO_MUX_0); + + dma_reset(DMA1_CHANNEL2); + + dma_init_type dma_init_struct; + dma_default_para_init(&dma_init_struct); + dma_init_struct.buffer_size = nbDataToTransmit; + dma_init_struct.direction = DMA_DIR_MEMORY_TO_PERIPHERAL; + dma_init_struct.memory_base_addr = (uint32_t)&aTxBuffer; + dma_init_struct.memory_data_width = DMA_MEMORY_DATA_WIDTH_BYTE; + dma_init_struct.memory_inc_enable = TRUE; + dma_init_struct.peripheral_base_addr = (uint32_t)&USART1->dt; + dma_init_struct.peripheral_data_width = DMA_PERIPHERAL_DATA_WIDTH_BYTE; + dma_init_struct.peripheral_inc_enable = FALSE; + dma_init_struct.priority = DMA_PRIORITY_LOW; + dma_init_struct.loop_mode_enable = FALSE; + dma_init(DMA1_CHANNEL2, &dma_init_struct); + + DMA1_CHANNEL2->ctrl |= DMA_FDT_INT; + DMA1_CHANNEL2->ctrl |= DMA_DTERR_INT; + + /* configure usart1 param */ + usart_init(USART1, 115200, USART_DATA_8BITS, USART_STOP_1_BIT); + usart_transmitter_enable(USART1, TRUE); + usart_receiver_enable(USART1, TRUE); + usart_single_line_halfduplex_select(USART1, TRUE); + usart_dma_transmitter_enable(USART1, TRUE); + usart_enable(USART1, TRUE); + + nvic_irq_enable(DMA1_Channel3_2_IRQn, 3, 0); } \ No newline at end of file diff --git a/Mcu/f421/Src/system_at32f421.c b/Mcu/f421/Src/system_at32f421.c index b3b1f51c..2b440a56 100644 --- a/Mcu/f421/Src/system_at32f421.c +++ b/Mcu/f421/Src/system_at32f421.c @@ -65,58 +65,58 @@ unsigned int system_core_clock = HICK_VALUE; /*!< system clock frequency (core c */ void SystemInit(void) { - /* reset the crm clock configuration to the default reset state(for debug - * purpose) */ - /* set hicken bit */ - CRM->ctrl_bit.hicken = TRUE; + /* reset the crm clock configuration to the default reset state(for debug + * purpose) */ + /* set hicken bit */ + CRM->ctrl_bit.hicken = TRUE; - /* wait hick stable */ - while (CRM->ctrl_bit.hickstbl != SET) - ; + /* wait hick stable */ + while (CRM->ctrl_bit.hickstbl != SET) + ; - /* hick used as system clock */ - CRM->cfg_bit.sclksel = CRM_SCLK_HICK; + /* hick used as system clock */ + CRM->cfg_bit.sclksel = CRM_SCLK_HICK; - /* wait sclk switch status */ - while (CRM->cfg_bit.sclksts != CRM_SCLK_HICK) - ; + /* wait sclk switch status */ + while (CRM->cfg_bit.sclksts != CRM_SCLK_HICK) + ; - /* reset hexten, hextbyps, cfden and pllen bits */ - CRM->ctrl &= ~(0x010D0000U); + /* reset hexten, hextbyps, cfden and pllen bits */ + CRM->ctrl &= ~(0x010D0000U); - /* reset cfg register, include sclk switch, ahbdiv, apb1div, apb2div, adcdiv, - clkout pllrcs, pllhextdiv, pllmult, usbdiv and pllrange bits */ - CRM->cfg = 0; + /* reset cfg register, include sclk switch, ahbdiv, apb1div, apb2div, adcdiv, + clkout pllrcs, pllhextdiv, pllmult, usbdiv and pllrange bits */ + CRM->cfg = 0; - /* reset pllfr, pllms, pllns and pllfref bits */ - CRM->pll = (0x00001F10U); + /* reset pllfr, pllms, pllns and pllfref bits */ + CRM->pll = (0x00001F10U); - /* reset clkout[3], usbbufs, hickdiv, clkoutdiv */ - CRM->misc1 = 0x00100000; + /* reset clkout[3], usbbufs, hickdiv, clkoutdiv */ + CRM->misc1 = 0x00100000; - /* disable all interrupts enable and clear pending bits */ - CRM->clkint = 0x009F0000; + /* disable all interrupts enable and clear pending bits */ + CRM->clkint = 0x009F0000; #if defined(AT32F421K8T7) || defined(AT32F421G8U7) || defined(AT32F421K6T7) || defined(AT32F421G6U7) || defined(AT32F421K4T7) || defined(AT32F421G4U7) - REG32(0x40021014) |= 0x00060000; - REG32(0x48000400) &= 0xFFFCFFCF; - REG32(0x48000400) |= 0x00010010; - REG32(0x48000404) &= 0xFFFFFEFB; - REG32(0x48000414) &= 0xFFFFFEFB; - REG32(0x48000414) |= 0x00000004; + REG32(0x40021014) |= 0x00060000; + REG32(0x48000400) &= 0xFFFCFFCF; + REG32(0x48000400) |= 0x00010010; + REG32(0x48000404) &= 0xFFFFFEFB; + REG32(0x48000414) &= 0xFFFFFEFB; + REG32(0x48000414) |= 0x00000004; #if defined(AT32F421G8U7) || defined(AT32F421G6U7) || defined(AT32F421G4U7) - REG32(0x48000000) &= 0xFC3FFFFF; - REG32(0x48000000) |= 0x01400000; - REG32(0x48000004) &= 0xFFFFE7FF; - REG32(0x48000014) &= 0xFFFFE7FF; + REG32(0x48000000) &= 0xFC3FFFFF; + REG32(0x48000000) |= 0x01400000; + REG32(0x48000004) &= 0xFFFFE7FF; + REG32(0x48000014) &= 0xFFFFE7FF; #endif - REG32(0x40021014) &= 0xFFF9FFFF; + REG32(0x40021014) &= 0xFFF9FFFF; #endif #ifdef VECT_TAB_SRAM - SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* vector table relocation in internal sram. */ + SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* vector table relocation in internal sram. */ #else - SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* vector table relocation in internal flash. */ + SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* vector table relocation in internal flash. */ #endif } @@ -130,83 +130,85 @@ void SystemInit(void) */ void system_core_clock_update(void) { - uint32_t pll_mult = 0, pll_mult_h = 0, pll_clock_source = 0, temp = 0, - div_value = 0; - uint32_t pllrcsfreq = 0, pll_ms = 0, pll_ns = 0, pll_fr = 0; - crm_sclk_type sclk_source; - - static const uint8_t sys_ahb_div_table[16] = { 0, 0, 0, 0, 0, 0, 0, 0, - 1, 2, 3, 4, 6, 7, 8, 9 }; - - /* get sclk source */ - sclk_source = crm_sysclk_switch_status_get(); - - switch (sclk_source) { - case CRM_SCLK_HICK: - if (((CRM->misc2_bit.hick_to_sclk) != RESET) && ((CRM->misc1_bit.hickdiv) != RESET)) - system_core_clock = HICK_VALUE * 6; - else - system_core_clock = HICK_VALUE; - break; - case CRM_SCLK_HEXT: - system_core_clock = HEXT_VALUE; - break; - case CRM_SCLK_PLL: - pll_clock_source = CRM->cfg_bit.pllrcs; - if (CRM->pll_bit.pllcfgen == FALSE) { - /* get multiplication factor */ - pll_mult = CRM->cfg_bit.pllmult_l; - pll_mult_h = CRM->cfg_bit.pllmult_h; - /* process high bits */ - if ((pll_mult_h != 0U) || (pll_mult == 15U)) { - pll_mult += ((16U * pll_mult_h) + 1U); - } else { - pll_mult += 2U; - } - - if (pll_clock_source == 0x00) { - /* hick divided by 2 selected as pll clock entry */ - system_core_clock = (HICK_VALUE >> 1) * pll_mult; - } else { - /* hext selected as pll clock entry */ - if (CRM->cfg_bit.pllhextdiv != RESET) { - /* hext clock divided by 2 */ - system_core_clock = (HEXT_VALUE / 2) * pll_mult; - } else { - system_core_clock = HEXT_VALUE * pll_mult; - } - } + uint32_t pll_mult = 0, pll_mult_h = 0, pll_clock_source = 0, temp = 0, + div_value = 0; + uint32_t pllrcsfreq = 0, pll_ms = 0, pll_ns = 0, pll_fr = 0; + crm_sclk_type sclk_source; + + static const uint8_t sys_ahb_div_table[16] = { 0, 0, 0, 0, 0, 0, 0, 0, + 1, 2, 3, 4, 6, 7, 8, 9 + }; + + /* get sclk source */ + sclk_source = crm_sysclk_switch_status_get(); + + switch (sclk_source) { + case CRM_SCLK_HICK: + if (((CRM->misc2_bit.hick_to_sclk) != RESET) && ((CRM->misc1_bit.hickdiv) != RESET)) { + system_core_clock = HICK_VALUE * 6; + } else { + system_core_clock = HICK_VALUE; + } + break; + case CRM_SCLK_HEXT: + system_core_clock = HEXT_VALUE; + break; + case CRM_SCLK_PLL: + pll_clock_source = CRM->cfg_bit.pllrcs; + if (CRM->pll_bit.pllcfgen == FALSE) { + /* get multiplication factor */ + pll_mult = CRM->cfg_bit.pllmult_l; + pll_mult_h = CRM->cfg_bit.pllmult_h; + /* process high bits */ + if ((pll_mult_h != 0U) || (pll_mult == 15U)) { + pll_mult += ((16U * pll_mult_h) + 1U); + } else { + pll_mult += 2U; + } + + if (pll_clock_source == 0x00) { + /* hick divided by 2 selected as pll clock entry */ + system_core_clock = (HICK_VALUE >> 1) * pll_mult; + } else { + /* hext selected as pll clock entry */ + if (CRM->cfg_bit.pllhextdiv != RESET) { + /* hext clock divided by 2 */ + system_core_clock = (HEXT_VALUE / 2) * pll_mult; + } else { + system_core_clock = HEXT_VALUE * pll_mult; + } + } + } else { + pll_ms = CRM->pll_bit.pllms; + pll_ns = CRM->pll_bit.pllns; + pll_fr = CRM->pll_bit.pllfr; + + if (pll_clock_source == 0x00) { + /* hick divided by 2 selected as pll clock entry */ + pllrcsfreq = (HICK_VALUE >> 1); + } else { + /* hext selected as pll clock entry */ + if (CRM->cfg_bit.pllhextdiv != RESET) { + /* hext clock divided by 2 */ + pllrcsfreq = (HEXT_VALUE / 2); } else { - pll_ms = CRM->pll_bit.pllms; - pll_ns = CRM->pll_bit.pllns; - pll_fr = CRM->pll_bit.pllfr; - - if (pll_clock_source == 0x00) { - /* hick divided by 2 selected as pll clock entry */ - pllrcsfreq = (HICK_VALUE >> 1); - } else { - /* hext selected as pll clock entry */ - if (CRM->cfg_bit.pllhextdiv != RESET) { - /* hext clock divided by 2 */ - pllrcsfreq = (HEXT_VALUE / 2); - } else { - pllrcsfreq = HEXT_VALUE; - } - } - system_core_clock = (uint32_t)(((uint64_t)pllrcsfreq * pll_ns) / (pll_ms * (0x1 << pll_fr))); + pllrcsfreq = HEXT_VALUE; } - break; - default: - system_core_clock = HICK_VALUE; - break; + } + system_core_clock = (uint32_t)(((uint64_t)pllrcsfreq * pll_ns) / (pll_ms * (0x1 << pll_fr))); } - - /* compute sclk, ahbclk frequency */ - /* get ahb division */ - temp = CRM->cfg_bit.ahbdiv; - div_value = sys_ahb_div_table[temp]; - /* ahbclk frequency */ - system_core_clock = system_core_clock >> div_value; + break; + default: + system_core_clock = HICK_VALUE; + break; + } + + /* compute sclk, ahbclk frequency */ + /* get ahb division */ + temp = CRM->cfg_bit.ahbdiv; + div_value = sys_ahb_div_table[temp]; + /* ahbclk frequency */ + system_core_clock = system_core_clock >> div_value; } /** * @} diff --git a/Mcu/g071/Inc/serial_telemetry.h b/Mcu/g071/Inc/serial_telemetry.h index fad4ee7a..77656728 100644 --- a/Mcu/g071/Inc/serial_telemetry.h +++ b/Mcu/g071/Inc/serial_telemetry.h @@ -11,7 +11,7 @@ #define SERIAL_TELEMETRY_H_ void makeTelemPackage(uint8_t temp, uint16_t voltage, uint16_t current, - uint16_t consumption, uint16_t e_rpm); + uint16_t consumption, uint16_t e_rpm); void telem_UART_Init(void); void send_telem_DMA(); diff --git a/Mcu/g071/Src/ADC.c b/Mcu/g071/Src/ADC.c index 55bbeb0d..06a1170d 100644 --- a/Mcu/g071/Src/ADC.c +++ b/Mcu/g071/Src/ADC.c @@ -21,179 +21,181 @@ extern uint16_t ADC_raw_input; (LL_ADC_DELAY_CALIB_ENABLE_ADC_CYCLES * 64) void ADC_DMA_Callback() -{ // read dma buffer and set extern variables +{ + // read dma buffer and set extern variables #ifdef USE_ADC_INPUT - ADC_raw_temp = ADCDataDMA[3]; - ADC_raw_volts = ADCDataDMA[1] / 2; - ADC_raw_current = ADCDataDMA[2]; - ADC_raw_input = ADCDataDMA[0]; + ADC_raw_temp = ADCDataDMA[3]; + ADC_raw_volts = ADCDataDMA[1] / 2; + ADC_raw_current = ADCDataDMA[2]; + ADC_raw_input = ADCDataDMA[0]; #else - ADC_raw_temp = ADCDataDMA[2]; - ADC_raw_volts = ADCDataDMA[1]; - ADC_raw_current = ADCDataDMA[0]; + ADC_raw_temp = ADCDataDMA[2]; + ADC_raw_volts = ADCDataDMA[1]; + ADC_raw_current = ADCDataDMA[0]; #endif } void enableADC_DMA() -{ // enables channel +{ + // enables channel - NVIC_SetPriority(DMA1_Channel2_3_IRQn, 3); - NVIC_EnableIRQ(DMA1_Channel2_3_IRQn); + NVIC_SetPriority(DMA1_Channel2_3_IRQn, 3); + NVIC_EnableIRQ(DMA1_Channel2_3_IRQn); - LL_DMA_ConfigAddresses( - DMA1, LL_DMA_CHANNEL_2, - LL_ADC_DMA_GetRegAddr(ADC1, LL_ADC_DMA_REG_REGULAR_DATA), - (uint32_t)&ADCDataDMA, LL_DMA_DIRECTION_PERIPH_TO_MEMORY); + LL_DMA_ConfigAddresses( + DMA1, LL_DMA_CHANNEL_2, + LL_ADC_DMA_GetRegAddr(ADC1, LL_ADC_DMA_REG_REGULAR_DATA), + (uint32_t)&ADCDataDMA, LL_DMA_DIRECTION_PERIPH_TO_MEMORY); - /* Set DMA transfer size */ + /* Set DMA transfer size */ #ifdef USE_ADC_INPUT - LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_2, 4); + LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_2, 4); #else - LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_2, 3); + LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_2, 3); #endif - /* Enable DMA transfer interruption: transfer complete */ - LL_DMA_EnableIT_TC(DMA1, LL_DMA_CHANNEL_2); + /* Enable DMA transfer interruption: transfer complete */ + LL_DMA_EnableIT_TC(DMA1, LL_DMA_CHANNEL_2); - /* Enable DMA transfer interruption: transfer error */ - LL_DMA_EnableIT_TE(DMA1, LL_DMA_CHANNEL_2); + /* Enable DMA transfer interruption: transfer error */ + LL_DMA_EnableIT_TE(DMA1, LL_DMA_CHANNEL_2); - /*## Activation of DMA - * #####################################################*/ - /* Enable the DMA transfer */ - LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_2); + /*## Activation of DMA + * #####################################################*/ + /* Enable the DMA transfer */ + LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_2); } void activateADC(void) { - __IO uint32_t wait_loop_index = 0U; - __IO uint32_t backup_setting_adc_dma_transfer = 0U; - - if (LL_ADC_IsEnabled(ADC1) == 0) { - /* Enable ADC internal voltage regulator */ - LL_ADC_EnableInternalRegulator(ADC1); - wait_loop_index = ((LL_ADC_DELAY_INTERNAL_REGUL_STAB_US * (SystemCoreClock / (100000 * 2))) / 10); - while (wait_loop_index != 0) { - wait_loop_index--; - } - backup_setting_adc_dma_transfer = LL_ADC_REG_GetDMATransfer(ADC1); - LL_ADC_REG_SetDMATransfer(ADC1, LL_ADC_REG_DMA_TRANSFER_NONE); - - /* Run ADC self calibration */ - LL_ADC_StartCalibration(ADC1); - - /* Poll for ADC effectively calibrated */ - - while (LL_ADC_IsCalibrationOnGoing(ADC1) != 0) { - } - /* Restore ADC DMA transfer request after calibration */ - LL_ADC_REG_SetDMATransfer(ADC1, backup_setting_adc_dma_transfer); - - /* Delay between ADC end of calibration and ADC enable. */ - /* Note: Variable divided by 2 to compensate partially */ - /* CPU processing cycles (depends on compilation optimization). */ - wait_loop_index = (ADC_DELAY_CALIB_ENABLE_CPU_CYCLES >> 1); - while (wait_loop_index != 0) { - wait_loop_index--; - } - /* Enable ADC */ - LL_ADC_Enable(ADC1); - while (LL_ADC_IsActiveFlag_ADRDY(ADC1) == 0) { - } - ADC->CCR |= ADC_CCR_TSEN; + __IO uint32_t wait_loop_index = 0U; + __IO uint32_t backup_setting_adc_dma_transfer = 0U; + + if (LL_ADC_IsEnabled(ADC1) == 0) { + /* Enable ADC internal voltage regulator */ + LL_ADC_EnableInternalRegulator(ADC1); + wait_loop_index = ((LL_ADC_DELAY_INTERNAL_REGUL_STAB_US * (SystemCoreClock / (100000 * 2))) / 10); + while (wait_loop_index != 0) { + wait_loop_index--; + } + backup_setting_adc_dma_transfer = LL_ADC_REG_GetDMATransfer(ADC1); + LL_ADC_REG_SetDMATransfer(ADC1, LL_ADC_REG_DMA_TRANSFER_NONE); + + /* Run ADC self calibration */ + LL_ADC_StartCalibration(ADC1); + + /* Poll for ADC effectively calibrated */ + + while (LL_ADC_IsCalibrationOnGoing(ADC1) != 0) { + } + /* Restore ADC DMA transfer request after calibration */ + LL_ADC_REG_SetDMATransfer(ADC1, backup_setting_adc_dma_transfer); + + /* Delay between ADC end of calibration and ADC enable. */ + /* Note: Variable divided by 2 to compensate partially */ + /* CPU processing cycles (depends on compilation optimization). */ + wait_loop_index = (ADC_DELAY_CALIB_ENABLE_CPU_CYCLES >> 1); + while (wait_loop_index != 0) { + wait_loop_index--; + } + /* Enable ADC */ + LL_ADC_Enable(ADC1); + while (LL_ADC_IsActiveFlag_ADRDY(ADC1) == 0) { } + ADC->CCR |= ADC_CCR_TSEN; + } } void ADC_Init(void) { - LL_ADC_REG_InitTypeDef ADC_REG_InitStruct = { 0 }; - LL_ADC_InitTypeDef ADC_InitStruct = { 0 }; - - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - - /* Peripheral clock enable */ - LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_ADC); - - LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA); - /**ADC1 GPIO Configuration - PA4 ------> ADC1_IN4 - PA6 ------> ADC1_IN6 - */ - GPIO_InitStruct.Pin = VOLTAGE_ADC_PIN; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = CURRENT_ADC_PIN; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - - /* ADC1 DMA Init */ - - /* ADC1 Init */ - LL_DMA_SetPeriphRequest(DMA1, LL_DMA_CHANNEL_2, LL_DMAMUX_REQ_ADC1); - - LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_2, - LL_DMA_DIRECTION_PERIPH_TO_MEMORY); - - LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_2, LL_DMA_PRIORITY_HIGH); - - LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_2, LL_DMA_MODE_CIRCULAR); - - LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_2, LL_DMA_PERIPH_NOINCREMENT); - - LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_2, LL_DMA_MEMORY_INCREMENT); - - LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_2, LL_DMA_PDATAALIGN_WORD); - - LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_2, LL_DMA_MDATAALIGN_HALFWORD); - - /* USER CODE BEGIN ADC1_Init 1 */ - - /* USER CODE END ADC1_Init 1 */ - /** Configure Regular Channel - */ - LL_ADC_SetCommonPathInternalCh(__LL_ADC_COMMON_INSTANCE(ADC1), - LL_ADC_CHANNEL_TEMPSENSOR); - /** Configure the global features of the ADC (Clock, Resolution, Data - * Alignment and number of conversion) - */ - ADC_REG_InitStruct.TriggerSource = LL_ADC_REG_TRIG_SOFTWARE; - ADC_REG_InitStruct.SequencerLength = LL_ADC_REG_SEQ_SCAN_ENABLE_3RANKS; - ADC_REG_InitStruct.SequencerDiscont = LL_ADC_REG_SEQ_DISCONT_DISABLE; - ADC_REG_InitStruct.ContinuousMode = LL_ADC_REG_CONV_SINGLE; - ADC_REG_InitStruct.DMATransfer = LL_ADC_REG_DMA_TRANSFER_LIMITED; - ADC_REG_InitStruct.Overrun = LL_ADC_REG_OVR_DATA_PRESERVED; - LL_ADC_REG_Init(ADC1, &ADC_REG_InitStruct); - // LL_ADC_REG_SetTriggerEdge(ADC1, LL_ADC_REG_TRIG_EXT_FALLING); - LL_ADC_SetOverSamplingScope(ADC1, LL_ADC_OVS_DISABLE); - LL_ADC_SetTriggerFrequencyMode(ADC1, LL_ADC_CLOCK_FREQ_MODE_LOW); - LL_ADC_REG_SetSequencerConfigurable(ADC1, LL_ADC_REG_SEQ_CONFIGURABLE); - LL_ADC_SetClock(ADC1, LL_ADC_CLOCK_ASYNC_DIV4); - LL_ADC_SetSamplingTimeCommonChannels(ADC1, LL_ADC_SAMPLINGTIME_COMMON_1, - LL_ADC_SAMPLINGTIME_19CYCLES_5); - LL_ADC_SetSamplingTimeCommonChannels(ADC1, LL_ADC_SAMPLINGTIME_COMMON_2, - LL_ADC_SAMPLINGTIME_160CYCLES_5); - LL_ADC_DisableIT_EOC(ADC1); - LL_ADC_DisableIT_EOS(ADC1); - - ADC_InitStruct.Resolution = LL_ADC_RESOLUTION_12B; - ADC_InitStruct.DataAlignment = LL_ADC_DATA_ALIGN_RIGHT; - ADC_InitStruct.LowPowerMode = LL_ADC_LP_MODE_NONE; - LL_ADC_Init(ADC1, &ADC_InitStruct); - - LL_ADC_REG_SetSequencerRanks(ADC1, LL_ADC_REG_RANK_1, CURRENT_ADC_CHANNEL); - LL_ADC_SetChannelSamplingTime(ADC1, CURRENT_ADC_CHANNEL, - LL_ADC_SAMPLINGTIME_COMMON_1); - - LL_ADC_REG_SetSequencerRanks(ADC1, LL_ADC_REG_RANK_2, VOLTAGE_ADC_CHANNEL); - LL_ADC_SetChannelSamplingTime(ADC1, VOLTAGE_ADC_CHANNEL, - LL_ADC_SAMPLINGTIME_COMMON_1); - - LL_ADC_REG_SetSequencerRanks(ADC1, LL_ADC_REG_RANK_3, - LL_ADC_CHANNEL_TEMPSENSOR); - LL_ADC_SetChannelSamplingTime(ADC1, LL_ADC_CHANNEL_TEMPSENSOR, - LL_ADC_SAMPLINGTIME_COMMON_2); + LL_ADC_REG_InitTypeDef ADC_REG_InitStruct = { 0 }; + LL_ADC_InitTypeDef ADC_InitStruct = { 0 }; + + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + + /* Peripheral clock enable */ + LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_ADC); + + LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA); + /**ADC1 GPIO Configuration + PA4 ------> ADC1_IN4 + PA6 ------> ADC1_IN6 + */ + GPIO_InitStruct.Pin = VOLTAGE_ADC_PIN; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = CURRENT_ADC_PIN; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + /* ADC1 DMA Init */ + + /* ADC1 Init */ + LL_DMA_SetPeriphRequest(DMA1, LL_DMA_CHANNEL_2, LL_DMAMUX_REQ_ADC1); + + LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_2, + LL_DMA_DIRECTION_PERIPH_TO_MEMORY); + + LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_2, LL_DMA_PRIORITY_HIGH); + + LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_2, LL_DMA_MODE_CIRCULAR); + + LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_2, LL_DMA_PERIPH_NOINCREMENT); + + LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_2, LL_DMA_MEMORY_INCREMENT); + + LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_2, LL_DMA_PDATAALIGN_WORD); + + LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_2, LL_DMA_MDATAALIGN_HALFWORD); + + /* USER CODE BEGIN ADC1_Init 1 */ + + /* USER CODE END ADC1_Init 1 */ + /** Configure Regular Channel + */ + LL_ADC_SetCommonPathInternalCh(__LL_ADC_COMMON_INSTANCE(ADC1), + LL_ADC_CHANNEL_TEMPSENSOR); + /** Configure the global features of the ADC (Clock, Resolution, Data + * Alignment and number of conversion) + */ + ADC_REG_InitStruct.TriggerSource = LL_ADC_REG_TRIG_SOFTWARE; + ADC_REG_InitStruct.SequencerLength = LL_ADC_REG_SEQ_SCAN_ENABLE_3RANKS; + ADC_REG_InitStruct.SequencerDiscont = LL_ADC_REG_SEQ_DISCONT_DISABLE; + ADC_REG_InitStruct.ContinuousMode = LL_ADC_REG_CONV_SINGLE; + ADC_REG_InitStruct.DMATransfer = LL_ADC_REG_DMA_TRANSFER_LIMITED; + ADC_REG_InitStruct.Overrun = LL_ADC_REG_OVR_DATA_PRESERVED; + LL_ADC_REG_Init(ADC1, &ADC_REG_InitStruct); + // LL_ADC_REG_SetTriggerEdge(ADC1, LL_ADC_REG_TRIG_EXT_FALLING); + LL_ADC_SetOverSamplingScope(ADC1, LL_ADC_OVS_DISABLE); + LL_ADC_SetTriggerFrequencyMode(ADC1, LL_ADC_CLOCK_FREQ_MODE_LOW); + LL_ADC_REG_SetSequencerConfigurable(ADC1, LL_ADC_REG_SEQ_CONFIGURABLE); + LL_ADC_SetClock(ADC1, LL_ADC_CLOCK_ASYNC_DIV4); + LL_ADC_SetSamplingTimeCommonChannels(ADC1, LL_ADC_SAMPLINGTIME_COMMON_1, + LL_ADC_SAMPLINGTIME_19CYCLES_5); + LL_ADC_SetSamplingTimeCommonChannels(ADC1, LL_ADC_SAMPLINGTIME_COMMON_2, + LL_ADC_SAMPLINGTIME_160CYCLES_5); + LL_ADC_DisableIT_EOC(ADC1); + LL_ADC_DisableIT_EOS(ADC1); + + ADC_InitStruct.Resolution = LL_ADC_RESOLUTION_12B; + ADC_InitStruct.DataAlignment = LL_ADC_DATA_ALIGN_RIGHT; + ADC_InitStruct.LowPowerMode = LL_ADC_LP_MODE_NONE; + LL_ADC_Init(ADC1, &ADC_InitStruct); + + LL_ADC_REG_SetSequencerRanks(ADC1, LL_ADC_REG_RANK_1, CURRENT_ADC_CHANNEL); + LL_ADC_SetChannelSamplingTime(ADC1, CURRENT_ADC_CHANNEL, + LL_ADC_SAMPLINGTIME_COMMON_1); + + LL_ADC_REG_SetSequencerRanks(ADC1, LL_ADC_REG_RANK_2, VOLTAGE_ADC_CHANNEL); + LL_ADC_SetChannelSamplingTime(ADC1, VOLTAGE_ADC_CHANNEL, + LL_ADC_SAMPLINGTIME_COMMON_1); + + LL_ADC_REG_SetSequencerRanks(ADC1, LL_ADC_REG_RANK_3, + LL_ADC_CHANNEL_TEMPSENSOR); + LL_ADC_SetChannelSamplingTime(ADC1, LL_ADC_CHANNEL_TEMPSENSOR, + LL_ADC_SAMPLINGTIME_COMMON_2); } diff --git a/Mcu/g071/Src/IO.c b/Mcu/g071/Src/IO.c index ca45765f..5d15b9ff 100644 --- a/Mcu/g071/Src/IO.c +++ b/Mcu/g071/Src/IO.c @@ -21,81 +21,87 @@ char out_put = 0; void receiveDshotDma() { #ifdef USE_TIMER_3_CHANNEL_1 - RCC->APBRSTR1 |= LL_APB1_GRP1_PERIPH_TIM3; - RCC->APBRSTR1 &= ~LL_APB1_GRP1_PERIPH_TIM3; - IC_TIMER_REGISTER->CCMR1 = 0x41; - IC_TIMER_REGISTER->CCER = 0xa; + RCC->APBRSTR1 |= LL_APB1_GRP1_PERIPH_TIM3; + RCC->APBRSTR1 &= ~LL_APB1_GRP1_PERIPH_TIM3; + IC_TIMER_REGISTER->CCMR1 = 0x41; + IC_TIMER_REGISTER->CCER = 0xa; #endif #ifdef USE_TIMER_16_CHANNEL_1 - LL_APB2_GRP1_ForceReset(LL_APB2_GRP1_PERIPH_TIM16); // de-init timer 2 - LL_APB2_GRP1_ReleaseReset(LL_APB2_GRP1_PERIPH_TIM16); - IC_TIMER_REGISTER->CCMR1 = 0x41; - IC_TIMER_REGISTER->CCER = 0xa; + LL_APB2_GRP1_ForceReset(LL_APB2_GRP1_PERIPH_TIM16); // de-init timer 2 + LL_APB2_GRP1_ReleaseReset(LL_APB2_GRP1_PERIPH_TIM16); + IC_TIMER_REGISTER->CCMR1 = 0x41; + IC_TIMER_REGISTER->CCER = 0xa; #endif - IC_TIMER_REGISTER->PSC = ic_timer_prescaler; - IC_TIMER_REGISTER->ARR = 0xFFFF; - IC_TIMER_REGISTER->EGR |= TIM_EGR_UG; - out_put = 0; - IC_TIMER_REGISTER->CNT = 0; - DMA1_Channel1->CMAR = (uint32_t)&dma_buffer; - DMA1_Channel1->CPAR = (uint32_t)&IC_TIMER_REGISTER->CCR1; - DMA1_Channel1->CNDTR = buffersize; - DMA1_Channel1->CCR = 0x98b; - IC_TIMER_REGISTER->DIER |= TIM_DIER_CC1DE; - IC_TIMER_REGISTER->CCER |= IC_TIMER_CHANNEL; - IC_TIMER_REGISTER->CR1 |= TIM_CR1_CEN; + IC_TIMER_REGISTER->PSC = ic_timer_prescaler; + IC_TIMER_REGISTER->ARR = 0xFFFF; + IC_TIMER_REGISTER->EGR |= TIM_EGR_UG; + out_put = 0; + IC_TIMER_REGISTER->CNT = 0; + DMA1_Channel1->CMAR = (uint32_t)&dma_buffer; + DMA1_Channel1->CPAR = (uint32_t)&IC_TIMER_REGISTER->CCR1; + DMA1_Channel1->CNDTR = buffersize; + DMA1_Channel1->CCR = 0x98b; + IC_TIMER_REGISTER->DIER |= TIM_DIER_CC1DE; + IC_TIMER_REGISTER->CCER |= IC_TIMER_CHANNEL; + IC_TIMER_REGISTER->CR1 |= TIM_CR1_CEN; } void sendDshotDma() { #ifdef USE_TIMER_3_CHANNEL_1 - RCC->APBRSTR1 |= LL_APB1_GRP1_PERIPH_TIM3; - RCC->APBRSTR1 &= ~LL_APB1_GRP1_PERIPH_TIM3; - IC_TIMER_REGISTER->CCMR1 = 0x60; - IC_TIMER_REGISTER->CCER = 0x3; + RCC->APBRSTR1 |= LL_APB1_GRP1_PERIPH_TIM3; + RCC->APBRSTR1 &= ~LL_APB1_GRP1_PERIPH_TIM3; + IC_TIMER_REGISTER->CCMR1 = 0x60; + IC_TIMER_REGISTER->CCER = 0x3; #endif #ifdef USE_TIMER_16_CHANNEL_1 - LL_APB2_GRP1_ForceReset(LL_APB2_GRP1_PERIPH_TIM16); // de-init timer 2 - LL_APB2_GRP1_ReleaseReset(LL_APB2_GRP1_PERIPH_TIM16); - IC_TIMER_REGISTER->CCMR1 = 0x60; - IC_TIMER_REGISTER->CCER = 0x3; + LL_APB2_GRP1_ForceReset(LL_APB2_GRP1_PERIPH_TIM16); // de-init timer 2 + LL_APB2_GRP1_ReleaseReset(LL_APB2_GRP1_PERIPH_TIM16); + IC_TIMER_REGISTER->CCMR1 = 0x60; + IC_TIMER_REGISTER->CCER = 0x3; #endif - IC_TIMER_REGISTER->PSC = output_timer_prescaler; - IC_TIMER_REGISTER->ARR = 92; - out_put = 1; - LL_TIM_GenerateEvent_UPDATE(IC_TIMER_REGISTER); + IC_TIMER_REGISTER->PSC = output_timer_prescaler; + IC_TIMER_REGISTER->ARR = 92; + out_put = 1; + LL_TIM_GenerateEvent_UPDATE(IC_TIMER_REGISTER); - DMA1_Channel1->CMAR = (uint32_t)&gcr; - DMA1_Channel1->CPAR = (uint32_t)&IC_TIMER_REGISTER->CCR1; - DMA1_Channel1->CNDTR = 23 + buffer_padding; - DMA1_Channel1->CCR = 0x99b; - IC_TIMER_REGISTER->DIER |= TIM_DIER_CC1DE; - IC_TIMER_REGISTER->CCER |= IC_TIMER_CHANNEL; - IC_TIMER_REGISTER->BDTR |= TIM_BDTR_MOE; - IC_TIMER_REGISTER->CR1 |= TIM_CR1_CEN; + DMA1_Channel1->CMAR = (uint32_t)&gcr; + DMA1_Channel1->CPAR = (uint32_t)&IC_TIMER_REGISTER->CCR1; + DMA1_Channel1->CNDTR = 23 + buffer_padding; + DMA1_Channel1->CCR = 0x99b; + IC_TIMER_REGISTER->DIER |= TIM_DIER_CC1DE; + IC_TIMER_REGISTER->CCER |= IC_TIMER_CHANNEL; + IC_TIMER_REGISTER->BDTR |= TIM_BDTR_MOE; + IC_TIMER_REGISTER->CR1 |= TIM_CR1_CEN; } -uint8_t getInputPinState() { return (INPUT_PIN_PORT->IDR & INPUT_PIN); } +uint8_t getInputPinState() +{ + return (INPUT_PIN_PORT->IDR & INPUT_PIN); +} void setInputPolarityRising() { - LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, - LL_TIM_IC_POLARITY_RISING); + LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, + LL_TIM_IC_POLARITY_RISING); } void setInputPullDown() { - LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_DOWN); + LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_DOWN); } void setInputPullUp() { - LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_UP); + LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_UP); } -void enableHalfTransferInt() { LL_DMA_EnableIT_HT(DMA1, INPUT_DMA_CHANNEL); } +void enableHalfTransferInt() +{ + LL_DMA_EnableIT_HT(DMA1, INPUT_DMA_CHANNEL); +} void setInputPullNone() { - LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_NO); + LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_NO); } diff --git a/Mcu/g071/Src/WS2812.c b/Mcu/g071/Src/WS2812.c index 6b7d2190..3199749f 100644 --- a/Mcu/g071/Src/WS2812.c +++ b/Mcu/g071/Src/WS2812.c @@ -11,147 +11,147 @@ char dma_busy; uint16_t led_Buffer[28] = { - 0, - 0, - 20, - 20, - 20, - 20, - 20, - 20, - 20, - 20, - 60, - 60, - 60, - 60, - 60, - 60, - 60, - 60, - 20, - 20, - 20, - 20, - 20, - 20, - 20, - 20, - 0, - 0, + 0, + 0, + 20, + 20, + 20, + 20, + 20, + 20, + 20, + 20, + 60, + 60, + 60, + 60, + 60, + 60, + 60, + 60, + 20, + 20, + 20, + 20, + 20, + 20, + 20, + 20, + 0, + 0, }; void send_LED_DMA() { - dma_busy = 1; - TIM16->CNT = 0; - LL_DMA_ConfigAddresses( - DMA1, LL_DMA_CHANNEL_6, (uint32_t)&led_Buffer, (uint32_t)&TIM16->CCR1, - LL_DMA_GetDataTransferDirection(DMA1, LL_DMA_CHANNEL_6)); - LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_6, 28); - LL_DMA_EnableIT_TC(DMA1, LL_DMA_CHANNEL_6); - LL_DMA_EnableIT_TE(DMA1, LL_DMA_CHANNEL_6); - LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_6); - LL_TIM_EnableDMAReq_CC1(TIM16); - LL_TIM_CC_EnableChannel(TIM16, LL_TIM_CHANNEL_CH1); - LL_TIM_EnableAllOutputs(TIM16); - LL_TIM_EnableCounter(TIM16); + dma_busy = 1; + TIM16->CNT = 0; + LL_DMA_ConfigAddresses( + DMA1, LL_DMA_CHANNEL_6, (uint32_t)&led_Buffer, (uint32_t)&TIM16->CCR1, + LL_DMA_GetDataTransferDirection(DMA1, LL_DMA_CHANNEL_6)); + LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_6, 28); + LL_DMA_EnableIT_TC(DMA1, LL_DMA_CHANNEL_6); + LL_DMA_EnableIT_TE(DMA1, LL_DMA_CHANNEL_6); + LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_6); + LL_TIM_EnableDMAReq_CC1(TIM16); + LL_TIM_CC_EnableChannel(TIM16, LL_TIM_CHANNEL_CH1); + LL_TIM_EnableAllOutputs(TIM16); + LL_TIM_EnableCounter(TIM16); } void send_LED_RGB(uint8_t red, uint8_t green, uint8_t blue) { - if (!dma_busy) { - uint32_t twenty_four_bit_color_number = green << 16 | red << 8 | blue; + if (!dma_busy) { + uint32_t twenty_four_bit_color_number = green << 16 | red << 8 | blue; - for (int i = 0; i < 24; i++) { - led_Buffer[i + 2] = (((twenty_four_bit_color_number >> (23 - i)) & 1) * 40) + 20; - } - send_LED_DMA(); + for (int i = 0; i < 24; i++) { + led_Buffer[i + 2] = (((twenty_four_bit_color_number >> (23 - i)) & 1) * 40) + 20; } + send_LED_DMA(); + } } void WS2812_Init(void) { - /* USER CODE BEGIN TIM16_Init 0 */ - NVIC_SetPriority(DMA1_Ch4_7_DMAMUX1_OVR_IRQn, 3); - NVIC_EnableIRQ(DMA1_Ch4_7_DMAMUX1_OVR_IRQn); - /* USER CODE END TIM16_Init 0 */ - - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - LL_TIM_OC_InitTypeDef TIM_OC_InitStruct = { 0 }; - LL_TIM_BDTR_InitTypeDef TIM_BDTRInitStruct = { 0 }; - - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - - /* Peripheral clock enable */ - LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM16); - - /* TIM16 DMA Init */ - - /* TIM16_CH1 Init */ - LL_DMA_SetPeriphRequest(DMA1, LL_DMA_CHANNEL_6, LL_DMAMUX_REQ_TIM16_CH1); - - LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_6, - LL_DMA_DIRECTION_MEMORY_TO_PERIPH); - - LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_6, LL_DMA_PRIORITY_HIGH); - - LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_6, LL_DMA_MODE_NORMAL); - - LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_6, LL_DMA_PERIPH_NOINCREMENT); - - LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_6, LL_DMA_MEMORY_INCREMENT); - - LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_6, LL_DMA_PDATAALIGN_HALFWORD); - - LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_6, LL_DMA_MDATAALIGN_HALFWORD); - - /* USER CODE BEGIN TIM16_Init 1 */ - - /* USER CODE END TIM16_Init 1 */ - TIM_InitStruct.Prescaler = 0; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_DOWN; - TIM_InitStruct.Autoreload = 79; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - TIM_InitStruct.RepetitionCounter = 0; - LL_TIM_Init(TIM16, &TIM_InitStruct); - LL_TIM_EnableARRPreload(TIM16); - LL_TIM_OC_EnablePreload(TIM16, LL_TIM_CHANNEL_CH1); - TIM_OC_InitStruct.OCMode = LL_TIM_OCMODE_PWM1; - TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; - TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; - TIM_OC_InitStruct.CompareValue = 0; - TIM_OC_InitStruct.OCPolarity = LL_TIM_OCPOLARITY_HIGH; - TIM_OC_InitStruct.OCNPolarity = LL_TIM_OCPOLARITY_HIGH; - TIM_OC_InitStruct.OCIdleState = LL_TIM_OCIDLESTATE_LOW; - TIM_OC_InitStruct.OCNIdleState = LL_TIM_OCIDLESTATE_LOW; - LL_TIM_OC_Init(TIM16, LL_TIM_CHANNEL_CH1, &TIM_OC_InitStruct); - LL_TIM_OC_DisableFast(TIM16, LL_TIM_CHANNEL_CH1); - TIM_BDTRInitStruct.OSSRState = LL_TIM_OSSR_DISABLE; - TIM_BDTRInitStruct.OSSIState = LL_TIM_OSSI_DISABLE; - TIM_BDTRInitStruct.LockLevel = LL_TIM_LOCKLEVEL_OFF; - TIM_BDTRInitStruct.DeadTime = 0; - TIM_BDTRInitStruct.BreakState = LL_TIM_BREAK_DISABLE; - TIM_BDTRInitStruct.BreakPolarity = LL_TIM_BREAK_POLARITY_HIGH; - TIM_BDTRInitStruct.BreakFilter = LL_TIM_BREAK_FILTER_FDIV1; - TIM_BDTRInitStruct.AutomaticOutput = LL_TIM_AUTOMATICOUTPUT_DISABLE; - LL_TIM_BDTR_Init(TIM16, &TIM_BDTRInitStruct); - /* USER CODE BEGIN TIM16_Init 2 */ - // NVIC_SetPriority(TIM16_IRQn, 0); - // NVIC_EnableIRQ(TIM16_IRQn); - /* USER CODE END TIM16_Init 2 */ - LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOB); - /**TIM16 GPIO Configuration - PB8 ------> TIM16_CH1 - */ - GPIO_InitStruct.Pin = LL_GPIO_PIN_8; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_2; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - TIM16->CCER |= 1 << 0; + /* USER CODE BEGIN TIM16_Init 0 */ + NVIC_SetPriority(DMA1_Ch4_7_DMAMUX1_OVR_IRQn, 3); + NVIC_EnableIRQ(DMA1_Ch4_7_DMAMUX1_OVR_IRQn); + /* USER CODE END TIM16_Init 0 */ + + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + LL_TIM_OC_InitTypeDef TIM_OC_InitStruct = { 0 }; + LL_TIM_BDTR_InitTypeDef TIM_BDTRInitStruct = { 0 }; + + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + + /* Peripheral clock enable */ + LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM16); + + /* TIM16 DMA Init */ + + /* TIM16_CH1 Init */ + LL_DMA_SetPeriphRequest(DMA1, LL_DMA_CHANNEL_6, LL_DMAMUX_REQ_TIM16_CH1); + + LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_6, + LL_DMA_DIRECTION_MEMORY_TO_PERIPH); + + LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_6, LL_DMA_PRIORITY_HIGH); + + LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_6, LL_DMA_MODE_NORMAL); + + LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_6, LL_DMA_PERIPH_NOINCREMENT); + + LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_6, LL_DMA_MEMORY_INCREMENT); + + LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_6, LL_DMA_PDATAALIGN_HALFWORD); + + LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_6, LL_DMA_MDATAALIGN_HALFWORD); + + /* USER CODE BEGIN TIM16_Init 1 */ + + /* USER CODE END TIM16_Init 1 */ + TIM_InitStruct.Prescaler = 0; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_DOWN; + TIM_InitStruct.Autoreload = 79; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + TIM_InitStruct.RepetitionCounter = 0; + LL_TIM_Init(TIM16, &TIM_InitStruct); + LL_TIM_EnableARRPreload(TIM16); + LL_TIM_OC_EnablePreload(TIM16, LL_TIM_CHANNEL_CH1); + TIM_OC_InitStruct.OCMode = LL_TIM_OCMODE_PWM1; + TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; + TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; + TIM_OC_InitStruct.CompareValue = 0; + TIM_OC_InitStruct.OCPolarity = LL_TIM_OCPOLARITY_HIGH; + TIM_OC_InitStruct.OCNPolarity = LL_TIM_OCPOLARITY_HIGH; + TIM_OC_InitStruct.OCIdleState = LL_TIM_OCIDLESTATE_LOW; + TIM_OC_InitStruct.OCNIdleState = LL_TIM_OCIDLESTATE_LOW; + LL_TIM_OC_Init(TIM16, LL_TIM_CHANNEL_CH1, &TIM_OC_InitStruct); + LL_TIM_OC_DisableFast(TIM16, LL_TIM_CHANNEL_CH1); + TIM_BDTRInitStruct.OSSRState = LL_TIM_OSSR_DISABLE; + TIM_BDTRInitStruct.OSSIState = LL_TIM_OSSI_DISABLE; + TIM_BDTRInitStruct.LockLevel = LL_TIM_LOCKLEVEL_OFF; + TIM_BDTRInitStruct.DeadTime = 0; + TIM_BDTRInitStruct.BreakState = LL_TIM_BREAK_DISABLE; + TIM_BDTRInitStruct.BreakPolarity = LL_TIM_BREAK_POLARITY_HIGH; + TIM_BDTRInitStruct.BreakFilter = LL_TIM_BREAK_FILTER_FDIV1; + TIM_BDTRInitStruct.AutomaticOutput = LL_TIM_AUTOMATICOUTPUT_DISABLE; + LL_TIM_BDTR_Init(TIM16, &TIM_BDTRInitStruct); + /* USER CODE BEGIN TIM16_Init 2 */ + // NVIC_SetPriority(TIM16_IRQn, 0); + // NVIC_EnableIRQ(TIM16_IRQn); + /* USER CODE END TIM16_Init 2 */ + LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOB); + /**TIM16 GPIO Configuration + PB8 ------> TIM16_CH1 + */ + GPIO_InitStruct.Pin = LL_GPIO_PIN_8; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_2; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + TIM16->CCER |= 1 << 0; } // void waitClockCycles(uint16_t cycles){ diff --git a/Mcu/g071/Src/comparator.c b/Mcu/g071/Src/comparator.c index 02b64fab..5d748757 100644 --- a/Mcu/g071/Src/comparator.c +++ b/Mcu/g071/Src/comparator.c @@ -37,56 +37,62 @@ COMP_TypeDef* active_COMP = COMP2; uint32_t current_EXTI_LINE = LL_EXTI_LINE_18; -uint8_t getCompOutputLevel() { return (active_COMP->CSR >> 30 & 1); } +uint8_t getCompOutputLevel() +{ + return (active_COMP->CSR >> 30 & 1); +} void maskPhaseInterrupts() { - EXTI->IMR1 &= ~(1 << 18); - EXTI->RPR1 = EXTI_LINE; - EXTI->FPR1 = EXTI_LINE; + EXTI->IMR1 &= ~(1 << 18); + EXTI->RPR1 = EXTI_LINE; + EXTI->FPR1 = EXTI_LINE; #ifdef N_VARIANT - EXTI->IMR1 &= ~(1 << 17); - LL_EXTI_ClearRisingFlag_0_31(LL_EXTI_LINE_17); - LL_EXTI_ClearFallingFlag_0_31(LL_EXTI_LINE_17); + EXTI->IMR1 &= ~(1 << 17); + LL_EXTI_ClearRisingFlag_0_31(LL_EXTI_LINE_17); + LL_EXTI_ClearFallingFlag_0_31(LL_EXTI_LINE_17); #endif } -void enableCompInterrupts() { EXTI->IMR1 |= current_EXTI_LINE; } +void enableCompInterrupts() +{ + EXTI->IMR1 |= current_EXTI_LINE; +} void changeCompInput() { - if (step == 1 || step == 4) { // c floating + if (step == 1 || step == 4) { // c floating #ifdef N_VARIANT - current_EXTI_LINE = PHASE_C_EXTI_LINE; - active_COMP = PHASE_C_COMP_NUMBER; + current_EXTI_LINE = PHASE_C_EXTI_LINE; + active_COMP = PHASE_C_COMP_NUMBER; #endif - LL_COMP_ConfigInputs(active_COMP, PHASE_C_COMP, LL_COMP_INPUT_PLUS_IO3); - } + LL_COMP_ConfigInputs(active_COMP, PHASE_C_COMP, LL_COMP_INPUT_PLUS_IO3); + } - if (step == 2 || step == 5) { // a floating + if (step == 2 || step == 5) { // a floating #ifdef N_VARIANT - current_EXTI_LINE = PHASE_A_EXTI_LINE; - active_COMP = PHASE_A_COMP_NUMBER; + current_EXTI_LINE = PHASE_A_EXTI_LINE; + active_COMP = PHASE_A_COMP_NUMBER; #endif - LL_COMP_ConfigInputs(active_COMP, PHASE_A_COMP, LL_COMP_INPUT_PLUS_IO3); - } + LL_COMP_ConfigInputs(active_COMP, PHASE_A_COMP, LL_COMP_INPUT_PLUS_IO3); + } - if (step == 3 || step == 6) { // b floating + if (step == 3 || step == 6) { // b floating #ifdef N_VARIANT - current_EXTI_LINE = PHASE_B_EXTI_LINE; - active_COMP = PHASE_B_COMP_NUMBER; + current_EXTI_LINE = PHASE_B_EXTI_LINE; + active_COMP = PHASE_B_COMP_NUMBER; #endif - LL_COMP_ConfigInputs(active_COMP, PHASE_B_COMP, LL_COMP_INPUT_PLUS_IO3); - } - if (rising) { - LL_EXTI_DisableRisingTrig_0_31(LL_EXTI_LINE_18); - LL_EXTI_DisableRisingTrig_0_31(LL_EXTI_LINE_17); - LL_EXTI_EnableFallingTrig_0_31(current_EXTI_LINE); - } else { // falling bemf - LL_EXTI_EnableRisingTrig_0_31(current_EXTI_LINE); - LL_EXTI_DisableFallingTrig_0_31(LL_EXTI_LINE_17); - LL_EXTI_DisableFallingTrig_0_31(LL_EXTI_LINE_18); - } + LL_COMP_ConfigInputs(active_COMP, PHASE_B_COMP, LL_COMP_INPUT_PLUS_IO3); + } + if (rising) { + LL_EXTI_DisableRisingTrig_0_31(LL_EXTI_LINE_18); + LL_EXTI_DisableRisingTrig_0_31(LL_EXTI_LINE_17); + LL_EXTI_EnableFallingTrig_0_31(current_EXTI_LINE); + } else { // falling bemf + LL_EXTI_EnableRisingTrig_0_31(current_EXTI_LINE); + LL_EXTI_DisableFallingTrig_0_31(LL_EXTI_LINE_17); + LL_EXTI_DisableFallingTrig_0_31(LL_EXTI_LINE_18); + } } // void changeCompInput() { diff --git a/Mcu/g071/Src/eeprom.c b/Mcu/g071/Src/eeprom.c index 0ee28486..4b238a74 100644 --- a/Mcu/g071/Src/eeprom.c +++ b/Mcu/g071/Src/eeprom.c @@ -16,62 +16,62 @@ uint32_t FLASH_FKEY2 = 0xCDEF89AB; void save_flash_nolib(uint8_t* data, int length, uint32_t add) { - uint32_t data_to_FLASH[length / 4]; - memset(data_to_FLASH, 0, length / 4); - for (int i = 0; i < length / 4; i++) { - data_to_FLASH[i] = data[i * 4 + 3] << 24 | data[i * 4 + 2] << 16 | data[i * 4 + 1] << 8 | data[i * 4]; // make 16 bit - } - volatile uint32_t data_length = length / 4; + uint32_t data_to_FLASH[length / 4]; + memset(data_to_FLASH, 0, length / 4); + for (int i = 0; i < length / 4; i++) { + data_to_FLASH[i] = data[i * 4 + 3] << 24 | data[i * 4 + 2] << 16 | data[i * 4 + 1] << 8 | data[i * 4]; // make 16 bit + } + volatile uint32_t data_length = length / 4; + + // unlock flash - // unlock flash + while ((FLASH->SR & FLASH_SR_BSY1) != 0) { + /* add time-out*/ + } + if ((FLASH->CR & FLASH_CR_LOCK) != 0) { + FLASH->KEYR = FLASH_FKEY1; + FLASH->KEYR = FLASH_FKEY2; + } + // erase page if address even divisable by 1024 + if ((add % 2048) == 0) { + FLASH->CR |= FLASH_CR_PER; + FLASH->CR |= (add / 2048) << 3; + FLASH->CR |= FLASH_CR_STRT; while ((FLASH->SR & FLASH_SR_BSY1) != 0) { - /* add time-out*/ + /* add time-out */ } - if ((FLASH->CR & FLASH_CR_LOCK) != 0) { - FLASH->KEYR = FLASH_FKEY1; - FLASH->KEYR = FLASH_FKEY2; + if ((FLASH->SR & FLASH_SR_EOP) != 0) { + FLASH->SR = FLASH_SR_EOP; + } else { + /* error */ } + FLASH->CR &= ~FLASH_CR_PER; + } - // erase page if address even divisable by 1024 - if ((add % 2048) == 0) { - FLASH->CR |= FLASH_CR_PER; - FLASH->CR |= (add / 2048) << 3; - FLASH->CR |= FLASH_CR_STRT; - while ((FLASH->SR & FLASH_SR_BSY1) != 0) { - /* add time-out */ - } - if ((FLASH->SR & FLASH_SR_EOP) != 0) { - FLASH->SR = FLASH_SR_EOP; - } else { - /* error */ - } - FLASH->CR &= ~FLASH_CR_PER; + volatile uint32_t write_cnt = 0, index = 0; + while (index < data_length) { + FLASH->CR |= FLASH_CR_PG; /* (1) */ + *(__IO uint32_t*)(add + write_cnt) = data_to_FLASH[index]; + *(__IO uint32_t*)(add + write_cnt + 4) = data_to_FLASH[index + 1]; + while ((FLASH->SR & FLASH_SR_BSY1) != 0) { /* add time-out */ } - - volatile uint32_t write_cnt = 0, index = 0; - while (index < data_length) { - FLASH->CR |= FLASH_CR_PG; /* (1) */ - *(__IO uint32_t*)(add + write_cnt) = data_to_FLASH[index]; - *(__IO uint32_t*)(add + write_cnt + 4) = data_to_FLASH[index + 1]; - while ((FLASH->SR & FLASH_SR_BSY1) != 0) { /* add time-out */ - } - if ((FLASH->SR & FLASH_SR_EOP) != 0) { - FLASH->SR = FLASH_SR_EOP; - } else { - /* error */ - } - FLASH->CR &= ~FLASH_CR_PG; - write_cnt += 8; - index += 2; + if ((FLASH->SR & FLASH_SR_EOP) != 0) { + FLASH->SR = FLASH_SR_EOP; + } else { + /* error */ } - SET_BIT(FLASH->CR, FLASH_CR_LOCK); + FLASH->CR &= ~FLASH_CR_PG; + write_cnt += 8; + index += 2; + } + SET_BIT(FLASH->CR, FLASH_CR_LOCK); } void read_flash_bin(uint8_t* data, uint32_t add, int out_buff_len) { - // volatile uint32_t read_data; - for (int i = 0; i < out_buff_len; i++) { - data[i] = *(uint8_t*)(add + i); - } + // volatile uint32_t read_data; + for (int i = 0; i < out_buff_len; i++) { + data[i] = *(uint8_t*)(add + i); + } } diff --git a/Mcu/g071/Src/peripherals.c b/Mcu/g071/Src/peripherals.c index 27b36d09..cd5cf9d1 100644 --- a/Mcu/g071/Src/peripherals.c +++ b/Mcu/g071/Src/peripherals.c @@ -20,209 +20,209 @@ void initCorePeripherals(void) { - LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_SYSCFG); - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_PWR); - LL_SYSCFG_EnablePinRemap(LL_SYSCFG_PIN_RMP_PA11); - LL_SYSCFG_EnablePinRemap(LL_SYSCFG_PIN_RMP_PA12); - FLASH->ACR |= FLASH_ACR_PRFTEN; //// prefetch buffer enable - SystemClock_Config(); - MX_GPIO_Init(); - MX_DMA_Init(); - MX_COMP2_Init(); - MX_TIM1_Init(); - MX_TIM2_Init(); + LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_SYSCFG); + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_PWR); + LL_SYSCFG_EnablePinRemap(LL_SYSCFG_PIN_RMP_PA11); + LL_SYSCFG_EnablePinRemap(LL_SYSCFG_PIN_RMP_PA12); + FLASH->ACR |= FLASH_ACR_PRFTEN; //// prefetch buffer enable + SystemClock_Config(); + MX_GPIO_Init(); + MX_DMA_Init(); + MX_COMP2_Init(); + MX_TIM1_Init(); + MX_TIM2_Init(); #ifdef USE_TIMER_3_CHANNEL_1 - MX_TIM3_Init(); + MX_TIM3_Init(); #endif #ifdef USE_TIMER_16_CHANNEL_1 - MX_TIM16_Init(); + MX_TIM16_Init(); #endif #ifdef N_VARIANT - MX_COMP1_Init(); + MX_COMP1_Init(); #endif - MX_TIM14_Init(); - MX_TIM17_Init(); - MX_TIM6_Init(); - telem_UART_Init(); + MX_TIM14_Init(); + MX_TIM17_Init(); + MX_TIM6_Init(); + telem_UART_Init(); #ifdef USE_LED_STRIP - WS2812_Init(); + WS2812_Init(); #endif } void initAfterJump() { - SCB->VTOR = 0x08001000; - __enable_irq(); + SCB->VTOR = 0x08001000; + __enable_irq(); } void SystemClock_Config(void) { - LL_FLASH_SetLatency(LL_FLASH_LATENCY_2); - if (LL_FLASH_GetLatency() != LL_FLASH_LATENCY_2) { - // Error_Handler(); - }; - - /* HSI configuration and activation */ - LL_RCC_HSI_Enable(); - while (LL_RCC_HSI_IsReady() != 1) { - }; - - /* LSI configuration and activation */ - LL_RCC_LSI_Enable(); - while (LL_RCC_LSI_IsReady() != 1) { - }; - - /* Main PLL configuration and activation */ - LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSI, LL_RCC_PLLM_DIV_1, 8, - LL_RCC_PLLR_DIV_2); - LL_RCC_PLL_Enable(); - LL_RCC_PLL_EnableDomain_SYS(); - while (LL_RCC_PLL_IsReady() != 1) { - }; - - /* Set AHB prescaler*/ - LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1); - - /* Sysclk activation on the main PLL */ - LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL); - while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) { - }; - - /* Set APB1 prescaler*/ - LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1); - LL_Init1msTick(64000000); - LL_SetSystemCoreClock(64000000); - /* Update CMSIS variable (which can be updated also through - * SystemCoreClockUpdate function) */ - LL_SetSystemCoreClock(64000000); - LL_RCC_SetTIMClockSource(LL_RCC_TIM1_CLKSOURCE_PCLK1); - LL_RCC_SetADCClockSource(LL_RCC_ADC_CLKSOURCE_SYSCLK); + LL_FLASH_SetLatency(LL_FLASH_LATENCY_2); + if (LL_FLASH_GetLatency() != LL_FLASH_LATENCY_2) { + // Error_Handler(); + }; + + /* HSI configuration and activation */ + LL_RCC_HSI_Enable(); + while (LL_RCC_HSI_IsReady() != 1) { + }; + + /* LSI configuration and activation */ + LL_RCC_LSI_Enable(); + while (LL_RCC_LSI_IsReady() != 1) { + }; + + /* Main PLL configuration and activation */ + LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSI, LL_RCC_PLLM_DIV_1, 8, + LL_RCC_PLLR_DIV_2); + LL_RCC_PLL_Enable(); + LL_RCC_PLL_EnableDomain_SYS(); + while (LL_RCC_PLL_IsReady() != 1) { + }; + + /* Set AHB prescaler*/ + LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1); + + /* Sysclk activation on the main PLL */ + LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL); + while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) { + }; + + /* Set APB1 prescaler*/ + LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1); + LL_Init1msTick(64000000); + LL_SetSystemCoreClock(64000000); + /* Update CMSIS variable (which can be updated also through + * SystemCoreClockUpdate function) */ + LL_SetSystemCoreClock(64000000); + LL_RCC_SetTIMClockSource(LL_RCC_TIM1_CLKSOURCE_PCLK1); + LL_RCC_SetADCClockSource(LL_RCC_ADC_CLKSOURCE_SYSCLK); } void MX_COMP1_Init(void) { - /* USER CODE BEGIN COMP2_Init 0 */ - - /* USER CODE END COMP2_Init 0 */ - - LL_COMP_InitTypeDef COMP_InitStruct = { 0 }; - - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - - LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA); - LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOB); - /**COMP2 GPIO Configuration - PA2 ------> COMP2_INM - PA3 ------> COMP2_INP - */ - GPIO_InitStruct.Pin = LL_GPIO_PIN_0; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_1; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - - /* USER CODE BEGIN COMP2_Init 1 */ - LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_SYSCFG); - /* USER CODE END COMP2_Init 1 */ - COMP_InitStruct.InputPlus = LL_COMP_INPUT_PLUS_IO3; - COMP_InitStruct.InputMinus = LL_COMP_INPUT_MINUS_IO3; - COMP_InitStruct.InputHysteresis = LL_COMP_HYSTERESIS_NONE; - COMP_InitStruct.OutputPolarity = LL_COMP_OUTPUTPOL_NONINVERTED; - COMP_InitStruct.OutputBlankingSource = LL_COMP_BLANKINGSRC_TIM1_OC5; - LL_COMP_Init(COMP1, &COMP_InitStruct); - LL_COMP_SetPowerMode(COMP1, LL_COMP_POWERMODE_MEDIUMSPEED); - LL_COMP_SetCommonWindowMode(__LL_COMP_COMMON_INSTANCE(COMP1), - LL_COMP_WINDOWMODE_DISABLE); - LL_COMP_SetCommonWindowOutput(__LL_COMP_COMMON_INSTANCE(COMP1), - LL_COMP_WINDOWOUTPUT_EACH_COMP); - - /* Wait loop initialization and execution */ - /* Note: Variable divided by 2 to compensate partially CPU processing cycles - */ - __IO uint32_t wait_loop_index = 0; - wait_loop_index = (LL_COMP_DELAY_VOLTAGE_SCALER_STAB_US * (SystemCoreClock / (1000000 * 2))); - while (wait_loop_index != 0) { - wait_loop_index--; - } - LL_EXTI_DisableEvent_0_31(LL_EXTI_LINE_17); - LL_EXTI_DisableIT_0_31(LL_EXTI_LINE_17); - /* USER CODE BEGIN COMP2_Init 2 */ - NVIC_SetPriority(ADC1_COMP_IRQn, 0); - NVIC_EnableIRQ(ADC1_COMP_IRQn); - //__NVIC_EnableIRQ; - /* USER CODE END COMP2_Init 2 */ + /* USER CODE BEGIN COMP2_Init 0 */ + + /* USER CODE END COMP2_Init 0 */ + + LL_COMP_InitTypeDef COMP_InitStruct = { 0 }; + + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + + LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA); + LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOB); + /**COMP2 GPIO Configuration + PA2 ------> COMP2_INM + PA3 ------> COMP2_INP + */ + GPIO_InitStruct.Pin = LL_GPIO_PIN_0; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_1; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + /* USER CODE BEGIN COMP2_Init 1 */ + LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_SYSCFG); + /* USER CODE END COMP2_Init 1 */ + COMP_InitStruct.InputPlus = LL_COMP_INPUT_PLUS_IO3; + COMP_InitStruct.InputMinus = LL_COMP_INPUT_MINUS_IO3; + COMP_InitStruct.InputHysteresis = LL_COMP_HYSTERESIS_NONE; + COMP_InitStruct.OutputPolarity = LL_COMP_OUTPUTPOL_NONINVERTED; + COMP_InitStruct.OutputBlankingSource = LL_COMP_BLANKINGSRC_TIM1_OC5; + LL_COMP_Init(COMP1, &COMP_InitStruct); + LL_COMP_SetPowerMode(COMP1, LL_COMP_POWERMODE_MEDIUMSPEED); + LL_COMP_SetCommonWindowMode(__LL_COMP_COMMON_INSTANCE(COMP1), + LL_COMP_WINDOWMODE_DISABLE); + LL_COMP_SetCommonWindowOutput(__LL_COMP_COMMON_INSTANCE(COMP1), + LL_COMP_WINDOWOUTPUT_EACH_COMP); + + /* Wait loop initialization and execution */ + /* Note: Variable divided by 2 to compensate partially CPU processing cycles + */ + __IO uint32_t wait_loop_index = 0; + wait_loop_index = (LL_COMP_DELAY_VOLTAGE_SCALER_STAB_US * (SystemCoreClock / (1000000 * 2))); + while (wait_loop_index != 0) { + wait_loop_index--; + } + LL_EXTI_DisableEvent_0_31(LL_EXTI_LINE_17); + LL_EXTI_DisableIT_0_31(LL_EXTI_LINE_17); + /* USER CODE BEGIN COMP2_Init 2 */ + NVIC_SetPriority(ADC1_COMP_IRQn, 0); + NVIC_EnableIRQ(ADC1_COMP_IRQn); + //__NVIC_EnableIRQ; + /* USER CODE END COMP2_Init 2 */ } void MX_COMP2_Init(void) { - /* USER CODE BEGIN COMP2_Init 0 */ + /* USER CODE BEGIN COMP2_Init 0 */ - /* USER CODE END COMP2_Init 0 */ + /* USER CODE END COMP2_Init 0 */ - LL_COMP_InitTypeDef COMP_InitStruct = { 0 }; + LL_COMP_InitTypeDef COMP_InitStruct = { 0 }; - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA); - LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOB); - /**COMP2 GPIO Configuration - PA2 ------> COMP2_INM - PA3 ------> COMP2_INP - */ - GPIO_InitStruct.Pin = LL_GPIO_PIN_2; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA); + LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOB); + /**COMP2 GPIO Configuration + PA2 ------> COMP2_INM + PA3 ------> COMP2_INP + */ + GPIO_InitStruct.Pin = LL_GPIO_PIN_2; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - GPIO_InitStruct.Pin = LL_GPIO_PIN_3; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + GPIO_InitStruct.Pin = LL_GPIO_PIN_3; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); #ifndef N_VARIANT - GPIO_InitStruct.Pin = LL_GPIO_PIN_3; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + GPIO_InitStruct.Pin = LL_GPIO_PIN_3; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); #endif - GPIO_InitStruct.Pin = LL_GPIO_PIN_7; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - /* USER CODE BEGIN COMP2_Init 1 */ - LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_SYSCFG); - /* USER CODE END COMP2_Init 1 */ - COMP_InitStruct.InputPlus = LL_COMP_INPUT_PLUS_IO3; - COMP_InitStruct.InputMinus = LL_COMP_INPUT_MINUS_IO3; - COMP_InitStruct.InputHysteresis = LL_COMP_HYSTERESIS_NONE; - COMP_InitStruct.OutputPolarity = LL_COMP_OUTPUTPOL_NONINVERTED; - COMP_InitStruct.OutputBlankingSource = LL_COMP_BLANKINGSRC_TIM1_OC5; - LL_COMP_Init(COMP2, &COMP_InitStruct); - LL_COMP_SetPowerMode(COMP2, LL_COMP_POWERMODE_MEDIUMSPEED); - LL_COMP_SetCommonWindowMode(__LL_COMP_COMMON_INSTANCE(COMP2), - LL_COMP_WINDOWMODE_DISABLE); - LL_COMP_SetCommonWindowOutput(__LL_COMP_COMMON_INSTANCE(COMP2), - LL_COMP_WINDOWOUTPUT_EACH_COMP); - - /* Wait loop initialization and execution */ - /* Note: Variable divided by 2 to compensate partially CPU processing cycles - */ - __IO uint32_t wait_loop_index = 0; - wait_loop_index = (LL_COMP_DELAY_VOLTAGE_SCALER_STAB_US * (SystemCoreClock / (1000000 * 2))); - while (wait_loop_index != 0) { - wait_loop_index--; - } - LL_EXTI_DisableEvent_0_31(LL_EXTI_LINE_18); - LL_EXTI_DisableIT_0_31(LL_EXTI_LINE_18); - /* USER CODE BEGIN COMP2_Init 2 */ - NVIC_SetPriority(ADC1_COMP_IRQn, 0); - NVIC_EnableIRQ(ADC1_COMP_IRQn); - //__NVIC_EnableIRQ; - /* USER CODE END COMP2_Init 2 */ + GPIO_InitStruct.Pin = LL_GPIO_PIN_7; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + /* USER CODE BEGIN COMP2_Init 1 */ + LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_SYSCFG); + /* USER CODE END COMP2_Init 1 */ + COMP_InitStruct.InputPlus = LL_COMP_INPUT_PLUS_IO3; + COMP_InitStruct.InputMinus = LL_COMP_INPUT_MINUS_IO3; + COMP_InitStruct.InputHysteresis = LL_COMP_HYSTERESIS_NONE; + COMP_InitStruct.OutputPolarity = LL_COMP_OUTPUTPOL_NONINVERTED; + COMP_InitStruct.OutputBlankingSource = LL_COMP_BLANKINGSRC_TIM1_OC5; + LL_COMP_Init(COMP2, &COMP_InitStruct); + LL_COMP_SetPowerMode(COMP2, LL_COMP_POWERMODE_MEDIUMSPEED); + LL_COMP_SetCommonWindowMode(__LL_COMP_COMMON_INSTANCE(COMP2), + LL_COMP_WINDOWMODE_DISABLE); + LL_COMP_SetCommonWindowOutput(__LL_COMP_COMMON_INSTANCE(COMP2), + LL_COMP_WINDOWOUTPUT_EACH_COMP); + + /* Wait loop initialization and execution */ + /* Note: Variable divided by 2 to compensate partially CPU processing cycles + */ + __IO uint32_t wait_loop_index = 0; + wait_loop_index = (LL_COMP_DELAY_VOLTAGE_SCALER_STAB_US * (SystemCoreClock / (1000000 * 2))); + while (wait_loop_index != 0) { + wait_loop_index--; + } + LL_EXTI_DisableEvent_0_31(LL_EXTI_LINE_18); + LL_EXTI_DisableIT_0_31(LL_EXTI_LINE_18); + /* USER CODE BEGIN COMP2_Init 2 */ + NVIC_SetPriority(ADC1_COMP_IRQn, 0); + NVIC_EnableIRQ(ADC1_COMP_IRQn); + //__NVIC_EnableIRQ; + /* USER CODE END COMP2_Init 2 */ } /** @@ -232,25 +232,25 @@ void MX_COMP2_Init(void) */ void MX_IWDG_Init(void) { - /* USER CODE BEGIN IWDG_Init 0 */ + /* USER CODE BEGIN IWDG_Init 0 */ - /* USER CODE END IWDG_Init 0 */ + /* USER CODE END IWDG_Init 0 */ - /* USER CODE BEGIN IWDG_Init 1 */ + /* USER CODE BEGIN IWDG_Init 1 */ - /* USER CODE END IWDG_Init 1 */ - LL_IWDG_Enable(IWDG); - LL_IWDG_EnableWriteAccess(IWDG); - LL_IWDG_SetPrescaler(IWDG, LL_IWDG_PRESCALER_4); - LL_IWDG_SetReloadCounter(IWDG, 4095); - while (LL_IWDG_IsReady(IWDG) != 1) { - } + /* USER CODE END IWDG_Init 1 */ + LL_IWDG_Enable(IWDG); + LL_IWDG_EnableWriteAccess(IWDG); + LL_IWDG_SetPrescaler(IWDG, LL_IWDG_PRESCALER_4); + LL_IWDG_SetReloadCounter(IWDG, 4095); + while (LL_IWDG_IsReady(IWDG) != 1) { + } - LL_IWDG_SetWindow(IWDG, 4095); - LL_IWDG_ReloadCounter(IWDG); - /* USER CODE BEGIN IWDG_Init 2 */ + LL_IWDG_SetWindow(IWDG, 4095); + LL_IWDG_ReloadCounter(IWDG); + /* USER CODE BEGIN IWDG_Init 2 */ - /* USER CODE END IWDG_Init 2 */ + /* USER CODE END IWDG_Init 2 */ } /** @@ -260,97 +260,97 @@ void MX_IWDG_Init(void) */ void MX_TIM1_Init(void) { - /* USER CODE BEGIN TIM1_Init 0 */ + /* USER CODE BEGIN TIM1_Init 0 */ - /* USER CODE END TIM1_Init 0 */ + /* USER CODE END TIM1_Init 0 */ - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - LL_TIM_OC_InitTypeDef TIM_OC_InitStruct = { 0 }; - LL_TIM_BDTR_InitTypeDef TIM_BDTRInitStruct = { 0 }; + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + LL_TIM_OC_InitTypeDef TIM_OC_InitStruct = { 0 }; + LL_TIM_BDTR_InitTypeDef TIM_BDTRInitStruct = { 0 }; - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - /* Peripheral clock enable */ - LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM1); + /* Peripheral clock enable */ + LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM1); - /* USER CODE BEGIN TIM1_Init 1 */ + /* USER CODE BEGIN TIM1_Init 1 */ - /* USER CODE END TIM1_Init 1 */ - TIM_InitStruct.Prescaler = 0; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = TIM1_AUTORELOAD; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - TIM_InitStruct.RepetitionCounter = 0; - LL_TIM_Init(TIM1, &TIM_InitStruct); - LL_TIM_EnableARRPreload(TIM1); - LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH1); + /* USER CODE END TIM1_Init 1 */ + TIM_InitStruct.Prescaler = 0; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = TIM1_AUTORELOAD; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + TIM_InitStruct.RepetitionCounter = 0; + LL_TIM_Init(TIM1, &TIM_InitStruct); + LL_TIM_EnableARRPreload(TIM1); + LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH1); #ifdef USE_SWAPPED_OUPUT - TIM_OC_InitStruct.OCMode = LL_TIM_OCMODE_PWM2; + TIM_OC_InitStruct.OCMode = LL_TIM_OCMODE_PWM2; #else - TIM_OC_InitStruct.OCMode = LL_TIM_OCMODE_PWM1; + TIM_OC_InitStruct.OCMode = LL_TIM_OCMODE_PWM1; #endif - TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; - TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; - TIM_OC_InitStruct.CompareValue = 0; - TIM_OC_InitStruct.OCPolarity = LL_TIM_OCPOLARITY_HIGH; - TIM_OC_InitStruct.OCNPolarity = LL_TIM_OCPOLARITY_HIGH; - TIM_OC_InitStruct.OCIdleState = LL_TIM_OCIDLESTATE_LOW; - TIM_OC_InitStruct.OCNIdleState = LL_TIM_OCIDLESTATE_LOW; - LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH1, &TIM_OC_InitStruct); - LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH1); - LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH2); - TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; - TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; - LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH2, &TIM_OC_InitStruct); - LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH2); - LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH3); - TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; - TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; - LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH3, &TIM_OC_InitStruct); - LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH3); - - LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH4); - TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; - TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; - LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH4, &TIM_OC_InitStruct); - LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH4); - - LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH5); - TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; - TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; - LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH5, &TIM_OC_InitStruct); - LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH5); - - LL_TIM_SetTriggerOutput(TIM1, LL_TIM_TRGO_RESET); - LL_TIM_SetTriggerOutput2(TIM1, LL_TIM_TRGO2_RESET); - LL_TIM_DisableMasterSlaveMode(TIM1); - TIM_BDTRInitStruct.OSSRState = LL_TIM_OSSR_DISABLE; - TIM_BDTRInitStruct.OSSIState = LL_TIM_OSSI_DISABLE; - TIM_BDTRInitStruct.LockLevel = LL_TIM_LOCKLEVEL_OFF; - TIM_BDTRInitStruct.DeadTime = DEAD_TIME; - TIM_BDTRInitStruct.BreakState = LL_TIM_BREAK_DISABLE; - TIM_BDTRInitStruct.BreakPolarity = LL_TIM_BREAK_POLARITY_HIGH; - TIM_BDTRInitStruct.BreakFilter = LL_TIM_BREAK_FILTER_FDIV1; - TIM_BDTRInitStruct.BreakAFMode = LL_TIM_BREAK_AFMODE_INPUT; - TIM_BDTRInitStruct.Break2State = LL_TIM_BREAK2_DISABLE; - TIM_BDTRInitStruct.Break2Polarity = LL_TIM_BREAK2_POLARITY_HIGH; - TIM_BDTRInitStruct.Break2Filter = LL_TIM_BREAK2_FILTER_FDIV1; - TIM_BDTRInitStruct.Break2AFMode = LL_TIM_BREAK_AFMODE_INPUT; - TIM_BDTRInitStruct.AutomaticOutput = LL_TIM_AUTOMATICOUTPUT_DISABLE; - LL_TIM_BDTR_Init(TIM1, &TIM_BDTRInitStruct); - /* USER CODE BEGIN TIM1_Init 2 */ - - /* USER CODE END TIM1_Init 2 */ - LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA); - LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOB); - /**TIM1 GPIO Configuration - PA7 ------> TIM1_CH1N - PB0 ------> TIM1_CH2N - PB1 ------> TIM1_CH3N - PA8 ------> TIM1_CH1 - PA9 [PA11] ------> TIM1_CH2 - PA10 [PA12] ------> TIM1_CH3 - */ + TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; + TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; + TIM_OC_InitStruct.CompareValue = 0; + TIM_OC_InitStruct.OCPolarity = LL_TIM_OCPOLARITY_HIGH; + TIM_OC_InitStruct.OCNPolarity = LL_TIM_OCPOLARITY_HIGH; + TIM_OC_InitStruct.OCIdleState = LL_TIM_OCIDLESTATE_LOW; + TIM_OC_InitStruct.OCNIdleState = LL_TIM_OCIDLESTATE_LOW; + LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH1, &TIM_OC_InitStruct); + LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH1); + LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH2); + TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; + TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; + LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH2, &TIM_OC_InitStruct); + LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH2); + LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH3); + TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; + TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; + LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH3, &TIM_OC_InitStruct); + LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH3); + + LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH4); + TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; + TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; + LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH4, &TIM_OC_InitStruct); + LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH4); + + LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH5); + TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; + TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; + LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH5, &TIM_OC_InitStruct); + LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH5); + + LL_TIM_SetTriggerOutput(TIM1, LL_TIM_TRGO_RESET); + LL_TIM_SetTriggerOutput2(TIM1, LL_TIM_TRGO2_RESET); + LL_TIM_DisableMasterSlaveMode(TIM1); + TIM_BDTRInitStruct.OSSRState = LL_TIM_OSSR_DISABLE; + TIM_BDTRInitStruct.OSSIState = LL_TIM_OSSI_DISABLE; + TIM_BDTRInitStruct.LockLevel = LL_TIM_LOCKLEVEL_OFF; + TIM_BDTRInitStruct.DeadTime = DEAD_TIME; + TIM_BDTRInitStruct.BreakState = LL_TIM_BREAK_DISABLE; + TIM_BDTRInitStruct.BreakPolarity = LL_TIM_BREAK_POLARITY_HIGH; + TIM_BDTRInitStruct.BreakFilter = LL_TIM_BREAK_FILTER_FDIV1; + TIM_BDTRInitStruct.BreakAFMode = LL_TIM_BREAK_AFMODE_INPUT; + TIM_BDTRInitStruct.Break2State = LL_TIM_BREAK2_DISABLE; + TIM_BDTRInitStruct.Break2Polarity = LL_TIM_BREAK2_POLARITY_HIGH; + TIM_BDTRInitStruct.Break2Filter = LL_TIM_BREAK2_FILTER_FDIV1; + TIM_BDTRInitStruct.Break2AFMode = LL_TIM_BREAK_AFMODE_INPUT; + TIM_BDTRInitStruct.AutomaticOutput = LL_TIM_AUTOMATICOUTPUT_DISABLE; + LL_TIM_BDTR_Init(TIM1, &TIM_BDTRInitStruct); + /* USER CODE BEGIN TIM1_Init 2 */ + + /* USER CODE END TIM1_Init 2 */ + LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA); + LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOB); + /**TIM1 GPIO Configuration + PA7 ------> TIM1_CH1N + PB0 ------> TIM1_CH2N + PB1 ------> TIM1_CH3N + PA8 ------> TIM1_CH1 + PA9 [PA11] ------> TIM1_CH2 + PA10 [PA12] ------> TIM1_CH3 + */ #ifdef PWM_ENABLE_BRIDGE #define PHASE_C_GPIO_LOW PHASE_C_GPIO_ENABLE #define PHASE_B_GPIO_LOW PHASE_B_GPIO_ENABLE @@ -372,54 +372,54 @@ void MX_TIM1_Init(void) #define PWM_OUTPUT_TYPE LL_GPIO_OUTPUT_OPENDRAIN #endif - GPIO_InitStruct.Pin = PHASE_C_GPIO_LOW; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_2; - LL_GPIO_Init(PHASE_C_GPIO_PORT_LOW, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = PHASE_B_GPIO_LOW; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_2; - LL_GPIO_Init(PHASE_B_GPIO_PORT_LOW, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = PHASE_A_GPIO_LOW; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_2; - LL_GPIO_Init(PHASE_A_GPIO_PORT_LOW, &GPIO_InitStruct); - - // high side gate / PWM outputs - GPIO_InitStruct.Pin = PHASE_C_GPIO_HIGH; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = PWM_OUTPUT_TYPE; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_2; - LL_GPIO_Init(PHASE_C_GPIO_PORT_HIGH, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = PHASE_B_GPIO_HIGH; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = PWM_OUTPUT_TYPE; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_2; - LL_GPIO_Init(PHASE_B_GPIO_PORT_HIGH, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = PHASE_A_GPIO_HIGH; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = PWM_OUTPUT_TYPE; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_2; - LL_GPIO_Init(PHASE_A_GPIO_PORT_HIGH, &GPIO_InitStruct); + GPIO_InitStruct.Pin = PHASE_C_GPIO_LOW; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_2; + LL_GPIO_Init(PHASE_C_GPIO_PORT_LOW, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = PHASE_B_GPIO_LOW; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_2; + LL_GPIO_Init(PHASE_B_GPIO_PORT_LOW, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = PHASE_A_GPIO_LOW; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_2; + LL_GPIO_Init(PHASE_A_GPIO_PORT_LOW, &GPIO_InitStruct); + + // high side gate / PWM outputs + GPIO_InitStruct.Pin = PHASE_C_GPIO_HIGH; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = PWM_OUTPUT_TYPE; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_2; + LL_GPIO_Init(PHASE_C_GPIO_PORT_HIGH, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = PHASE_B_GPIO_HIGH; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = PWM_OUTPUT_TYPE; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_2; + LL_GPIO_Init(PHASE_B_GPIO_PORT_HIGH, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = PHASE_A_GPIO_HIGH; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = PWM_OUTPUT_TYPE; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_2; + LL_GPIO_Init(PHASE_A_GPIO_PORT_HIGH, &GPIO_InitStruct); } /** @@ -429,30 +429,30 @@ void MX_TIM1_Init(void) */ void MX_TIM2_Init(void) { - /* USER CODE BEGIN TIM2_Init 0 */ + /* USER CODE BEGIN TIM2_Init 0 */ - /* USER CODE END TIM2_Init 0 */ + /* USER CODE END TIM2_Init 0 */ - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - /* Peripheral clock enable */ - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2); + /* Peripheral clock enable */ + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2); - /* USER CODE BEGIN TIM2_Init 1 */ + /* USER CODE BEGIN TIM2_Init 1 */ - /* USER CODE END TIM2_Init 1 */ - TIM_InitStruct.Prescaler = 31; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 65535; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - LL_TIM_Init(TIM2, &TIM_InitStruct); - LL_TIM_DisableARRPreload(TIM2); - LL_TIM_SetClockSource(TIM2, LL_TIM_CLOCKSOURCE_INTERNAL); - LL_TIM_SetTriggerOutput(TIM2, LL_TIM_TRGO_RESET); - LL_TIM_DisableMasterSlaveMode(TIM2); - /* USER CODE BEGIN TIM2_Init 2 */ + /* USER CODE END TIM2_Init 1 */ + TIM_InitStruct.Prescaler = 31; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 65535; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + LL_TIM_Init(TIM2, &TIM_InitStruct); + LL_TIM_DisableARRPreload(TIM2); + LL_TIM_SetClockSource(TIM2, LL_TIM_CLOCKSOURCE_INTERNAL); + LL_TIM_SetTriggerOutput(TIM2, LL_TIM_TRGO_RESET); + LL_TIM_DisableMasterSlaveMode(TIM2); + /* USER CODE BEGIN TIM2_Init 2 */ - /* USER CODE END TIM2_Init 2 */ + /* USER CODE END TIM2_Init 2 */ } /** @@ -462,136 +462,136 @@ void MX_TIM2_Init(void) */ void MX_TIM3_Init(void) { - /* USER CODE BEGIN TIM3_Init 0 */ + /* USER CODE BEGIN TIM3_Init 0 */ - /* USER CODE END TIM3_Init 0 */ + /* USER CODE END TIM3_Init 0 */ - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - /* Peripheral clock enable */ - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM3); + /* Peripheral clock enable */ + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM3); - LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOB); - LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA); - /**TIM3 GPIO Configuration - PB4 ------> TIM3_CH1 - */ - GPIO_InitStruct.Pin = INPUT_PIN; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_1; - LL_GPIO_Init(INPUT_PIN_PORT, &GPIO_InitStruct); + LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOB); + LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA); + /**TIM3 GPIO Configuration + PB4 ------> TIM3_CH1 + */ + GPIO_InitStruct.Pin = INPUT_PIN; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_1; + LL_GPIO_Init(INPUT_PIN_PORT, &GPIO_InitStruct); - /* TIM3 DMA Init */ + /* TIM3 DMA Init */ - /* TIM3_CH1 Init */ - LL_DMA_SetPeriphRequest(DMA1, LL_DMA_CHANNEL_1, LL_DMAMUX_REQ_TIM3_CH1); + /* TIM3_CH1 Init */ + LL_DMA_SetPeriphRequest(DMA1, LL_DMA_CHANNEL_1, LL_DMAMUX_REQ_TIM3_CH1); - LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_1, - LL_DMA_DIRECTION_PERIPH_TO_MEMORY); + LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_1, + LL_DMA_DIRECTION_PERIPH_TO_MEMORY); - LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PRIORITY_LOW); + LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PRIORITY_LOW); - LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MODE_NORMAL); + LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MODE_NORMAL); - LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PERIPH_NOINCREMENT); + LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PERIPH_NOINCREMENT); - LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MEMORY_INCREMENT); + LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MEMORY_INCREMENT); - LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PDATAALIGN_HALFWORD); + LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PDATAALIGN_HALFWORD); - LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MDATAALIGN_WORD); + LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MDATAALIGN_WORD); - /* TIM3 interrupt Init */ - NVIC_SetPriority(TIM3_IRQn, 0); - NVIC_EnableIRQ(TIM3_IRQn); + /* TIM3 interrupt Init */ + NVIC_SetPriority(TIM3_IRQn, 0); + NVIC_EnableIRQ(TIM3_IRQn); - /* USER CODE BEGIN TIM3_Init 1 */ + /* USER CODE BEGIN TIM3_Init 1 */ - /* USER CODE END TIM3_Init 1 */ - TIM_InitStruct.Prescaler = 0; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 65535; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - LL_TIM_Init(TIM3, &TIM_InitStruct); - LL_TIM_DisableARRPreload(TIM3); - LL_TIM_SetTriggerOutput(TIM3, LL_TIM_TRGO_RESET); - LL_TIM_DisableMasterSlaveMode(TIM3); - LL_TIM_IC_SetActiveInput(TIM3, LL_TIM_CHANNEL_CH1, - LL_TIM_ACTIVEINPUT_DIRECTTI); - LL_TIM_IC_SetPrescaler(TIM3, LL_TIM_CHANNEL_CH1, LL_TIM_ICPSC_DIV1); - LL_TIM_IC_SetFilter(TIM3, LL_TIM_CHANNEL_CH1, LL_TIM_IC_FILTER_FDIV1); - LL_TIM_IC_SetPolarity(TIM3, LL_TIM_CHANNEL_CH1, LL_TIM_IC_POLARITY_BOTHEDGE); - /* USER CODE BEGIN TIM3_Init 2 */ + /* USER CODE END TIM3_Init 1 */ + TIM_InitStruct.Prescaler = 0; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 65535; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + LL_TIM_Init(TIM3, &TIM_InitStruct); + LL_TIM_DisableARRPreload(TIM3); + LL_TIM_SetTriggerOutput(TIM3, LL_TIM_TRGO_RESET); + LL_TIM_DisableMasterSlaveMode(TIM3); + LL_TIM_IC_SetActiveInput(TIM3, LL_TIM_CHANNEL_CH1, + LL_TIM_ACTIVEINPUT_DIRECTTI); + LL_TIM_IC_SetPrescaler(TIM3, LL_TIM_CHANNEL_CH1, LL_TIM_ICPSC_DIV1); + LL_TIM_IC_SetFilter(TIM3, LL_TIM_CHANNEL_CH1, LL_TIM_IC_FILTER_FDIV1); + LL_TIM_IC_SetPolarity(TIM3, LL_TIM_CHANNEL_CH1, LL_TIM_IC_POLARITY_BOTHEDGE); + /* USER CODE BEGIN TIM3_Init 2 */ - /* USER CODE END TIM3_Init 2 */ + /* USER CODE END TIM3_Init 2 */ } void MX_TIM16_Init(void) { - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - /* Peripheral clock enable */ - LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM16); + /* Peripheral clock enable */ + LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM16); - LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA); - /**TIM3 GPIO Configuration - PB4 ------> TIM3_CH1 - */ - GPIO_InitStruct.Pin = LL_GPIO_PIN_6; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_5; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA); + /**TIM3 GPIO Configuration + PB4 ------> TIM3_CH1 + */ + GPIO_InitStruct.Pin = LL_GPIO_PIN_6; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_5; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - LL_DMA_SetPeriphRequest(DMA1, LL_DMA_CHANNEL_1, LL_DMAMUX_REQ_TIM16_CH1); + LL_DMA_SetPeriphRequest(DMA1, LL_DMA_CHANNEL_1, LL_DMAMUX_REQ_TIM16_CH1); - LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_1, - LL_DMA_DIRECTION_PERIPH_TO_MEMORY); + LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_1, + LL_DMA_DIRECTION_PERIPH_TO_MEMORY); - LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PRIORITY_LOW); + LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PRIORITY_LOW); - LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MODE_NORMAL); + LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MODE_NORMAL); - LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PERIPH_NOINCREMENT); + LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PERIPH_NOINCREMENT); - LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MEMORY_INCREMENT); + LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MEMORY_INCREMENT); - LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PDATAALIGN_HALFWORD); + LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PDATAALIGN_HALFWORD); - LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MDATAALIGN_WORD); + LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MDATAALIGN_WORD); - /* TIM3 interrupt Init */ - NVIC_SetPriority(TIM16_IRQn, 0); - NVIC_EnableIRQ(TIM16_IRQn); + /* TIM3 interrupt Init */ + NVIC_SetPriority(TIM16_IRQn, 0); + NVIC_EnableIRQ(TIM16_IRQn); - /* USER CODE BEGIN TIM3_Init 1 */ + /* USER CODE BEGIN TIM3_Init 1 */ - /* USER CODE END TIM3_Init 1 */ - TIM_InitStruct.Prescaler = 0; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 65535; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - LL_TIM_Init(TIM16, &TIM_InitStruct); - LL_TIM_DisableARRPreload(TIM16); - LL_TIM_SetTriggerOutput(TIM16, LL_TIM_TRGO_RESET); - LL_TIM_DisableMasterSlaveMode(TIM16); - LL_TIM_IC_SetActiveInput(TIM16, LL_TIM_CHANNEL_CH1, - LL_TIM_ACTIVEINPUT_DIRECTTI); - LL_TIM_IC_SetPrescaler(TIM16, LL_TIM_CHANNEL_CH1, LL_TIM_ICPSC_DIV1); - LL_TIM_IC_SetFilter(TIM16, LL_TIM_CHANNEL_CH1, LL_TIM_IC_FILTER_FDIV1); - LL_TIM_IC_SetPolarity(TIM16, LL_TIM_CHANNEL_CH1, LL_TIM_IC_POLARITY_BOTHEDGE); - /* USER CODE BEGIN TIM3_Init 2 */ + /* USER CODE END TIM3_Init 1 */ + TIM_InitStruct.Prescaler = 0; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 65535; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + LL_TIM_Init(TIM16, &TIM_InitStruct); + LL_TIM_DisableARRPreload(TIM16); + LL_TIM_SetTriggerOutput(TIM16, LL_TIM_TRGO_RESET); + LL_TIM_DisableMasterSlaveMode(TIM16); + LL_TIM_IC_SetActiveInput(TIM16, LL_TIM_CHANNEL_CH1, + LL_TIM_ACTIVEINPUT_DIRECTTI); + LL_TIM_IC_SetPrescaler(TIM16, LL_TIM_CHANNEL_CH1, LL_TIM_ICPSC_DIV1); + LL_TIM_IC_SetFilter(TIM16, LL_TIM_CHANNEL_CH1, LL_TIM_IC_FILTER_FDIV1); + LL_TIM_IC_SetPolarity(TIM16, LL_TIM_CHANNEL_CH1, LL_TIM_IC_POLARITY_BOTHEDGE); + /* USER CODE BEGIN TIM3_Init 2 */ - /* USER CODE END TIM3_Init 2 */ + /* USER CODE END TIM3_Init 2 */ } /** @@ -601,31 +601,31 @@ void MX_TIM16_Init(void) */ void MX_TIM14_Init(void) { - /* USER CODE BEGIN TIM14_Init 0 */ + /* USER CODE BEGIN TIM14_Init 0 */ - /* USER CODE END TIM14_Init 0 */ + /* USER CODE END TIM14_Init 0 */ - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - /* Peripheral clock enable */ - LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM14); + /* Peripheral clock enable */ + LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM14); - /* TIM14 interrupt Init */ - NVIC_SetPriority(TIM14_IRQn, 0); - NVIC_EnableIRQ(TIM14_IRQn); + /* TIM14 interrupt Init */ + NVIC_SetPriority(TIM14_IRQn, 0); + NVIC_EnableIRQ(TIM14_IRQn); - /* USER CODE BEGIN TIM14_Init 1 */ + /* USER CODE BEGIN TIM14_Init 1 */ - /* USER CODE END TIM14_Init 1 */ - TIM_InitStruct.Prescaler = 31; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 65535; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - LL_TIM_Init(TIM14, &TIM_InitStruct); - LL_TIM_DisableARRPreload(TIM14); - /* USER CODE BEGIN TIM14_Init 2 */ + /* USER CODE END TIM14_Init 1 */ + TIM_InitStruct.Prescaler = 31; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 65535; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + LL_TIM_Init(TIM14, &TIM_InitStruct); + LL_TIM_DisableARRPreload(TIM14); + /* USER CODE BEGIN TIM14_Init 2 */ - /* USER CODE END TIM14_Init 2 */ + /* USER CODE END TIM14_Init 2 */ } /** @@ -635,28 +635,28 @@ void MX_TIM14_Init(void) */ void MX_TIM17_Init(void) { - /* USER CODE BEGIN TIM17_Init 0 */ + /* USER CODE BEGIN TIM17_Init 0 */ - /* USER CODE END TIM17_Init 0 */ + /* USER CODE END TIM17_Init 0 */ - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - /* Peripheral clock enable */ - LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM17); + /* Peripheral clock enable */ + LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM17); - /* USER CODE BEGIN TIM17_Init 1 */ + /* USER CODE BEGIN TIM17_Init 1 */ - /* USER CODE END TIM17_Init 1 */ - TIM_InitStruct.Prescaler = 63; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 65535; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - TIM_InitStruct.RepetitionCounter = 0; - LL_TIM_Init(TIM17, &TIM_InitStruct); - LL_TIM_EnableARRPreload(TIM17); - /* USER CODE BEGIN TIM17_Init 2 */ + /* USER CODE END TIM17_Init 1 */ + TIM_InitStruct.Prescaler = 63; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 65535; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + TIM_InitStruct.RepetitionCounter = 0; + LL_TIM_Init(TIM17, &TIM_InitStruct); + LL_TIM_EnableARRPreload(TIM17); + /* USER CODE BEGIN TIM17_Init 2 */ - /* USER CODE END TIM17_Init 2 */ + /* USER CODE END TIM17_Init 2 */ } /** @@ -664,135 +664,150 @@ void MX_TIM17_Init(void) */ void MX_DMA_Init(void) { - /* Init with LL driver */ - /* DMA controller clock enable */ - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_DMA1); - - /* DMA interrupt init */ - /* DMA1_Channel1_IRQn interrupt configuration */ - NVIC_SetPriority(DMA1_Channel1_IRQn, 1); - NVIC_EnableIRQ(DMA1_Channel1_IRQn); - /* DMA1_Channel2_3_IRQn interrupt configuration */ - NVIC_SetPriority(DMA1_Channel2_3_IRQn, 1); - NVIC_EnableIRQ(DMA1_Channel2_3_IRQn); + /* Init with LL driver */ + /* DMA controller clock enable */ + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_DMA1); + + /* DMA interrupt init */ + /* DMA1_Channel1_IRQn interrupt configuration */ + NVIC_SetPriority(DMA1_Channel1_IRQn, 1); + NVIC_EnableIRQ(DMA1_Channel1_IRQn); + /* DMA1_Channel2_3_IRQn interrupt configuration */ + NVIC_SetPriority(DMA1_Channel2_3_IRQn, 1); + NVIC_EnableIRQ(DMA1_Channel2_3_IRQn); } void MX_TIM6_Init(void) { - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM6); + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM6); - /* TIM6 interrupt Init */ - NVIC_SetPriority(TIM6_DAC_LPTIM1_IRQn, 2); - NVIC_EnableIRQ(TIM6_DAC_LPTIM1_IRQn); + /* TIM6 interrupt Init */ + NVIC_SetPriority(TIM6_DAC_LPTIM1_IRQn, 2); + NVIC_EnableIRQ(TIM6_DAC_LPTIM1_IRQn); - TIM_InitStruct.Prescaler = 63; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 1000000 / LOOP_FREQUENCY_HZ; - LL_TIM_Init(TIM6, &TIM_InitStruct); - LL_TIM_DisableARRPreload(TIM6); - LL_TIM_SetTriggerOutput(TIM6, LL_TIM_TRGO_RESET); - LL_TIM_DisableMasterSlaveMode(TIM6); + TIM_InitStruct.Prescaler = 63; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 1000000 / LOOP_FREQUENCY_HZ; + LL_TIM_Init(TIM6, &TIM_InitStruct); + LL_TIM_DisableARRPreload(TIM6); + LL_TIM_SetTriggerOutput(TIM6, LL_TIM_TRGO_RESET); + LL_TIM_DisableMasterSlaveMode(TIM6); } void MX_GPIO_Init(void) { - /* GPIO Ports Clock Enable */ - LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA); - LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOB); + /* GPIO Ports Clock Enable */ + LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA); + LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOB); } -void reloadWatchDogCounter() { LL_IWDG_ReloadCounter(IWDG); } +void reloadWatchDogCounter() +{ + LL_IWDG_ReloadCounter(IWDG); +} -void setPWMCompare1(uint16_t compareone) { TIM1->CCR1 = compareone; } -void setPWMCompare2(uint16_t comparetwo) { TIM1->CCR2 = comparetwo; } -void setPWMCompare3(uint16_t comparethree) { TIM1->CCR3 = comparethree; } +void setPWMCompare1(uint16_t compareone) +{ + TIM1->CCR1 = compareone; +} +void setPWMCompare2(uint16_t comparetwo) +{ + TIM1->CCR2 = comparetwo; +} +void setPWMCompare3(uint16_t comparethree) +{ + TIM1->CCR3 = comparethree; +} -void generatePwmTimerEvent() { LL_TIM_GenerateEvent_UPDATE(TIM1); } +void generatePwmTimerEvent() +{ + LL_TIM_GenerateEvent_UPDATE(TIM1); +} void resetInputCaptureTimer() { - IC_TIMER_REGISTER->PSC = 0; - IC_TIMER_REGISTER->CNT = 0; + IC_TIMER_REGISTER->PSC = 0; + IC_TIMER_REGISTER->CNT = 0; } void enableCorePeripherals() { - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH1); - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH2); - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH3); - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH1N); - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH2N); - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH3N); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH1); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH2); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH3); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH1N); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH2N); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH3N); #ifdef MCU_G071 - LL_TIM_CC_EnableChannel( - TIM1, LL_TIM_CHANNEL_CH5); // timer used for comparator blanking + LL_TIM_CC_EnableChannel( + TIM1, LL_TIM_CHANNEL_CH5); // timer used for comparator blanking #endif - LL_TIM_CC_EnableChannel(TIM1, - LL_TIM_CHANNEL_CH4); // timer used for timing adc read - TIM1->CCR4 = 100; // set in 10khz loop to match pwm cycle timed to end of pwm on + LL_TIM_CC_EnableChannel(TIM1, + LL_TIM_CHANNEL_CH4); // timer used for timing adc read + TIM1->CCR4 = 100; // set in 10khz loop to match pwm cycle timed to end of pwm on - /* Enable counter */ - LL_TIM_EnableCounter(TIM1); - LL_TIM_EnableAllOutputs(TIM1); - /* Force update generation */ - LL_TIM_GenerateEvent_UPDATE(TIM1); + /* Enable counter */ + LL_TIM_EnableCounter(TIM1); + LL_TIM_EnableAllOutputs(TIM1); + /* Force update generation */ + LL_TIM_GenerateEvent_UPDATE(TIM1); #ifdef USE_ADC_INPUT #else - LL_TIM_CC_EnableChannel(IC_TIMER_REGISTER, - IC_TIMER_CHANNEL); // input capture and output compare - LL_TIM_EnableCounter(IC_TIMER_REGISTER); + LL_TIM_CC_EnableChannel(IC_TIMER_REGISTER, + IC_TIMER_CHANNEL); // input capture and output compare + LL_TIM_EnableCounter(IC_TIMER_REGISTER); #endif #ifdef USE_RGB_LED - LED_GPIO_init(); - GPIOB->BRR = LL_GPIO_PIN_8; // turn on red - GPIOB->BSRR = LL_GPIO_PIN_5; - GPIOB->BSRR = LL_GPIO_PIN_3; // + LED_GPIO_init(); + GPIOB->BRR = LL_GPIO_PIN_8; // turn on red + GPIOB->BSRR = LL_GPIO_PIN_5; + GPIOB->BSRR = LL_GPIO_PIN_3; // #endif #ifndef BRUSHED_MODE - LL_TIM_EnableCounter(COM_TIMER); // commutation_timer priority 0 - LL_TIM_GenerateEvent_UPDATE(COM_TIMER); - LL_TIM_EnableIT_UPDATE(COM_TIMER); - COM_TIMER->DIER &= ~((0x1UL << (0U))); // disable for now. + LL_TIM_EnableCounter(COM_TIMER); // commutation_timer priority 0 + LL_TIM_GenerateEvent_UPDATE(COM_TIMER); + LL_TIM_EnableIT_UPDATE(COM_TIMER); + COM_TIMER->DIER &= ~((0x1UL << (0U))); // disable for now. #endif - LL_TIM_EnableCounter(UTILITY_TIMER); - LL_TIM_GenerateEvent_UPDATE(UTILITY_TIMER); - // - LL_TIM_EnableCounter(INTERVAL_TIMER); - LL_TIM_GenerateEvent_UPDATE(INTERVAL_TIMER); - - LL_TIM_EnableCounter(TEN_KHZ_TIMER); // 10khz timer - LL_TIM_GenerateEvent_UPDATE(TEN_KHZ_TIMER); - TEN_KHZ_TIMER->DIER |= (0x1UL << (0U)); // enable interrupt - // RCC->APB2ENR &= ~(1 << 22); // turn debug off + LL_TIM_EnableCounter(UTILITY_TIMER); + LL_TIM_GenerateEvent_UPDATE(UTILITY_TIMER); + // + LL_TIM_EnableCounter(INTERVAL_TIMER); + LL_TIM_GenerateEvent_UPDATE(INTERVAL_TIMER); + + LL_TIM_EnableCounter(TEN_KHZ_TIMER); // 10khz timer + LL_TIM_GenerateEvent_UPDATE(TEN_KHZ_TIMER); + TEN_KHZ_TIMER->DIER |= (0x1UL << (0U)); // enable interrupt + // RCC->APB2ENR &= ~(1 << 22); // turn debug off #ifdef USE_ADC - ADC_Init(); - enableADC_DMA(); - activateADC(); + ADC_Init(); + enableADC_DMA(); + activateADC(); #endif #ifndef MCU_F031 - __IO uint32_t wait_loop_index = 0; - /* Enable comparator */ - LL_COMP_Enable(MAIN_COMP); + __IO uint32_t wait_loop_index = 0; + /* Enable comparator */ + LL_COMP_Enable(MAIN_COMP); #ifdef N_VARIANT // needs comp 1 and 2 - LL_COMP_Enable(COMP1); + LL_COMP_Enable(COMP1); #endif - wait_loop_index = ((LL_COMP_DELAY_STARTUP_US * (SystemCoreClock / (100000 * 2))) / 10); - while (wait_loop_index != 0) { - wait_loop_index--; - } + wait_loop_index = ((LL_COMP_DELAY_STARTUP_US * (SystemCoreClock / (100000 * 2))) / 10); + while (wait_loop_index != 0) { + wait_loop_index--; + } #endif - NVIC_SetPriority(EXTI4_15_IRQn, 2); - NVIC_EnableIRQ(EXTI4_15_IRQn); - EXTI->IMR1 |= (1 << 15); - #ifdef USE_PULSE_OUT - LL_GPIO_SetPinMode(RPM_PULSE_PORT, RPM_PULSE_PIN, LL_GPIO_MODE_OUTPUT); - #endif + NVIC_SetPriority(EXTI4_15_IRQn, 2); + NVIC_EnableIRQ(EXTI4_15_IRQn); + EXTI->IMR1 |= (1 << 15); +#ifdef USE_PULSE_OUT + LL_GPIO_SetPinMode(RPM_PULSE_PORT, RPM_PULSE_PIN, LL_GPIO_MODE_OUTPUT); +#endif } diff --git a/Mcu/g071/Src/phaseouts.c b/Mcu/g071/Src/phaseouts.c index fdcf49e0..1bc452d2 100644 --- a/Mcu/g071/Src/phaseouts.c +++ b/Mcu/g071/Src/phaseouts.c @@ -31,66 +31,67 @@ extern char prop_brake_active; #endif void proportionalBrake() -{ // alternate all channels between braking (ABC LOW) - // and coasting (ABC float) put lower channel into - // alternate mode and turn upper OFF for each - // channel - // turn all HIGH channels off for ABC - - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; - - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; - - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; - - // set low channel to PWM, duty cycle will now control braking - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); +{ + // alternate all channels between braking (ABC LOW) + // and coasting (ABC float) put lower channel into + // alternate mode and turn upper OFF for each + // channel + // turn all HIGH channels off for ABC + + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; + + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; + + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; + + // set low channel to PWM, duty cycle will now control braking + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); } void phaseBPWM() { - if (!eepromBuffer.comp_pwm) { // for future - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); // low - } - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, - LL_GPIO_MODE_ALTERNATE); // high + if (!eepromBuffer.comp_pwm) { // for future + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); // low + } + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, + LL_GPIO_MODE_ALTERNATE); // high } void phaseBFLOAT() { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; } void phaseBLOW() { - // low mosfet on - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_B_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; + // low mosfet on + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_B_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; } //////////////////////////////PHASE @@ -98,37 +99,37 @@ void phaseBLOW() void phaseCPWM() { - if (!eepromBuffer.comp_pwm) { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); - } - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); + } + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, + LL_GPIO_MODE_ALTERNATE); } void phaseCFLOAT() { - // floating - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; + // floating + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; } void phaseCLOW() { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_C_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_C_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; } ///////////////////////////////////////////////PHASE 3 @@ -136,36 +137,36 @@ void phaseCLOW() void phaseAPWM() { - if (!eepromBuffer.comp_pwm) { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); - } - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); + } + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, + LL_GPIO_MODE_ALTERNATE); } void phaseAFLOAT() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; } void phaseALOW() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_A_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_A_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; } #else @@ -173,38 +174,38 @@ void phaseALOW() //////////////////////////////////PHASE 1////////////////////// void phaseBPWM() { - if (!eepromBuffer.comp_pwm) { // for future - // LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, - // PHASE_B_GPIO_LOW, LL_GPIO_MODE_OUTPUT); - // PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); // high pwm + if (!eepromBuffer.comp_pwm) { // for future + // LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, + // PHASE_B_GPIO_LOW, LL_GPIO_MODE_OUTPUT); + // PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); // high pwm } void phaseBFLOAT() { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable off - PHASE_B_GPIO_PORT_ENABLE->BRR = PHASE_B_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); // pwm off - PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable off + PHASE_B_GPIO_PORT_ENABLE->BRR = PHASE_B_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); // pwm off + PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; } void phaseBLOW() { - // low mosfet on - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); // pwm off - PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; + // low mosfet on + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); // pwm off + PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; } //////////////////////////////PHASE @@ -212,38 +213,38 @@ void phaseBLOW() void phaseCPWM() { - if (!eepromBuffer.comp_pwm) { - // LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - // LL_GPIO_MODE_OUTPUT); PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = - // PHASE_C_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + // LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + // LL_GPIO_MODE_OUTPUT); PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = + // PHASE_C_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); } void phaseCFLOAT() { - // floating - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable off - PHASE_C_GPIO_PORT_ENABLE->BRR = PHASE_C_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; + // floating + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable off + PHASE_C_GPIO_PORT_ENABLE->BRR = PHASE_C_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; } void phaseCLOW() { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; } ///////////////////////////////////////////////PHASE 3 @@ -251,113 +252,115 @@ void phaseCLOW() void phaseAPWM() { - if (!eepromBuffer.comp_pwm) { - // LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - // LL_GPIO_MODE_OUTPUT); PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = - // PHASE_A_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + // LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + // LL_GPIO_MODE_OUTPUT); PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = + // PHASE_A_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); } void phaseAFLOAT() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BRR = PHASE_A_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BRR = PHASE_A_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; } void phaseALOW() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; } #endif void allOff() { - phaseAFLOAT(); - phaseBFLOAT(); - phaseCFLOAT(); + phaseAFLOAT(); + phaseBFLOAT(); + phaseCFLOAT(); } void comStep(int newStep) { - switch (newStep) { - case 1: // A-B - phaseCFLOAT(); - phaseBLOW(); - phaseAPWM(); - break; - - case 2: // C-B - phaseAFLOAT(); - phaseBLOW(); - phaseCPWM(); - break; - - case 3: // C-A - phaseBFLOAT(); - phaseALOW(); - phaseCPWM(); - break; - - case 4: // B-A - phaseCFLOAT(); - phaseALOW(); - phaseBPWM(); - break; - - case 5: // B-C - phaseAFLOAT(); - phaseCLOW(); - phaseBPWM(); - break; - - case 6: // A-C - phaseBFLOAT(); - phaseCLOW(); - phaseAPWM(); - break; - } -} + switch (newStep) { + case 1: // A-B + phaseCFLOAT(); + phaseBLOW(); + phaseAPWM(); + break; -void fullBrake() -{ // full braking shorting all low sides - phaseALOW(); + case 2: // C-B + phaseAFLOAT(); phaseBLOW(); + phaseCPWM(); + break; + + case 3: // C-A + phaseBFLOAT(); + phaseALOW(); + phaseCPWM(); + break; + + case 4: // B-A + phaseCFLOAT(); + phaseALOW(); + phaseBPWM(); + break; + + case 5: // B-C + phaseAFLOAT(); + phaseCLOW(); + phaseBPWM(); + break; + + case 6: // A-C + phaseBFLOAT(); phaseCLOW(); + phaseAPWM(); + break; + } +} + +void fullBrake() +{ + // full braking shorting all low sides + phaseALOW(); + phaseBLOW(); + phaseCLOW(); } void allpwm() -{ // for stepper_sine - phaseAPWM(); - phaseBPWM(); - phaseCPWM(); +{ + // for stepper_sine + phaseAPWM(); + phaseBPWM(); + phaseCPWM(); } void twoChannelForward() { - phaseAPWM(); - phaseBLOW(); - phaseCPWM(); + phaseAPWM(); + phaseBLOW(); + phaseCPWM(); } void twoChannelReverse() { - phaseALOW(); - phaseBPWM(); - phaseCLOW(); + phaseALOW(); + phaseBPWM(); + phaseCLOW(); } diff --git a/Mcu/g071/Src/serial_telemetry.c b/Mcu/g071/Src/serial_telemetry.c index a509ce00..33abc1b0 100644 --- a/Mcu/g071/Src/serial_telemetry.c +++ b/Mcu/g071/Src/serial_telemetry.c @@ -13,116 +13,119 @@ uint8_t nbDataToTransmit = sizeof(aTxBuffer); void telem_UART_Init() { - LL_USART_InitTypeDef USART_InitStruct = { 0 }; + LL_USART_InitTypeDef USART_InitStruct = { 0 }; - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - /* Peripheral clock enable */ - LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_USART1); + /* Peripheral clock enable */ + LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_USART1); - LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOB); + LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOB); - GPIO_InitStruct.Pin = LL_GPIO_PIN_6; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_OPENDRAIN; - GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; - GPIO_InitStruct.Alternate = LL_GPIO_AF_0; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + GPIO_InitStruct.Pin = LL_GPIO_PIN_6; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_OPENDRAIN; + GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; + GPIO_InitStruct.Alternate = LL_GPIO_AF_0; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); - NVIC_SetPriority(USART1_IRQn, 3); - NVIC_EnableIRQ(USART1_IRQn); + NVIC_SetPriority(USART1_IRQn, 3); + NVIC_EnableIRQ(USART1_IRQn); - LL_DMA_SetPeriphRequest(DMA1, LL_DMA_CHANNEL_3, LL_DMAMUX_REQ_USART1_TX); + LL_DMA_SetPeriphRequest(DMA1, LL_DMA_CHANNEL_3, LL_DMAMUX_REQ_USART1_TX); - LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_3, - LL_DMA_DIRECTION_MEMORY_TO_PERIPH); + LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_3, + LL_DMA_DIRECTION_MEMORY_TO_PERIPH); - LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_3, LL_DMA_PRIORITY_LOW); + LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_3, LL_DMA_PRIORITY_LOW); - LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_3, LL_DMA_MODE_NORMAL); + LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_3, LL_DMA_MODE_NORMAL); - LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_3, LL_DMA_PERIPH_NOINCREMENT); + LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_3, LL_DMA_PERIPH_NOINCREMENT); - LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_3, LL_DMA_MEMORY_INCREMENT); + LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_3, LL_DMA_MEMORY_INCREMENT); - LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_3, LL_DMA_PDATAALIGN_BYTE); + LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_3, LL_DMA_PDATAALIGN_BYTE); - LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_3, LL_DMA_MDATAALIGN_BYTE); + LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_3, LL_DMA_MDATAALIGN_BYTE); - USART_InitStruct.PrescalerValue = LL_USART_PRESCALER_DIV1; - USART_InitStruct.BaudRate = 115200; - USART_InitStruct.DataWidth = LL_USART_DATAWIDTH_8B; - USART_InitStruct.StopBits = LL_USART_STOPBITS_1; - USART_InitStruct.Parity = LL_USART_PARITY_NONE; - USART_InitStruct.TransferDirection = LL_USART_DIRECTION_TX_RX; - USART_InitStruct.OverSampling = LL_USART_OVERSAMPLING_16; - LL_USART_Init(USART1, &USART_InitStruct); - LL_USART_SetTXFIFOThreshold(USART1, LL_USART_FIFOTHRESHOLD_1_8); - LL_USART_SetRXFIFOThreshold(USART1, LL_USART_FIFOTHRESHOLD_1_8); - LL_USART_DisableFIFO(USART1); - LL_USART_ConfigHalfDuplexMode(USART1); + USART_InitStruct.PrescalerValue = LL_USART_PRESCALER_DIV1; + USART_InitStruct.BaudRate = 115200; + USART_InitStruct.DataWidth = LL_USART_DATAWIDTH_8B; + USART_InitStruct.StopBits = LL_USART_STOPBITS_1; + USART_InitStruct.Parity = LL_USART_PARITY_NONE; + USART_InitStruct.TransferDirection = LL_USART_DIRECTION_TX_RX; + USART_InitStruct.OverSampling = LL_USART_OVERSAMPLING_16; + LL_USART_Init(USART1, &USART_InitStruct); + LL_USART_SetTXFIFOThreshold(USART1, LL_USART_FIFOTHRESHOLD_1_8); + LL_USART_SetRXFIFOThreshold(USART1, LL_USART_FIFOTHRESHOLD_1_8); + LL_USART_DisableFIFO(USART1); + LL_USART_ConfigHalfDuplexMode(USART1); - LL_USART_Enable(USART1); - while ((!(LL_USART_IsActiveFlag_TEACK(USART1))) || (!(LL_USART_IsActiveFlag_REACK(USART1)))) { - } + LL_USART_Enable(USART1); + while ((!(LL_USART_IsActiveFlag_TEACK(USART1))) || (!(LL_USART_IsActiveFlag_REACK(USART1)))) { + } - LL_DMA_ConfigAddresses( - DMA1, LL_DMA_CHANNEL_3, (uint32_t)aTxBuffer, - LL_USART_DMA_GetRegAddr(USART1, LL_USART_DMA_REG_DATA_TRANSMIT), - LL_DMA_GetDataTransferDirection(DMA1, LL_DMA_CHANNEL_3)); - LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_3, nbDataToTransmit); + LL_DMA_ConfigAddresses( + DMA1, LL_DMA_CHANNEL_3, (uint32_t)aTxBuffer, + LL_USART_DMA_GetRegAddr(USART1, LL_USART_DMA_REG_DATA_TRANSMIT), + LL_DMA_GetDataTransferDirection(DMA1, LL_DMA_CHANNEL_3)); + LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_3, nbDataToTransmit); - /* (5) Enable DMA transfer complete/error interrupts */ - LL_DMA_EnableIT_TC(DMA1, LL_DMA_CHANNEL_3); - LL_DMA_EnableIT_TE(DMA1, LL_DMA_CHANNEL_3); + /* (5) Enable DMA transfer complete/error interrupts */ + LL_DMA_EnableIT_TC(DMA1, LL_DMA_CHANNEL_3); + LL_DMA_EnableIT_TE(DMA1, LL_DMA_CHANNEL_3); } void send_telem_DMA() -{ // set data length and enable channel to start transfer - LL_USART_SetTransferDirection(USART1, LL_USART_DIRECTION_TX); - // GPIOB->OTYPER &= 0 << 6; - LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_3, nbDataToTransmit); - LL_USART_EnableDMAReq_TX(USART1); - - LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_3); - LL_USART_SetTransferDirection(USART1, LL_USART_DIRECTION_RX); +{ + // set data length and enable channel to start transfer + LL_USART_SetTransferDirection(USART1, LL_USART_DIRECTION_TX); + // GPIOB->OTYPER &= 0 << 6; + LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_3, nbDataToTransmit); + LL_USART_EnableDMAReq_TX(USART1); + + LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_3); + LL_USART_SetTransferDirection(USART1, LL_USART_DIRECTION_RX); } uint8_t update_crc8(uint8_t crc, uint8_t crc_seed) { - uint8_t crc_u, i; - crc_u = crc; - crc_u ^= crc_seed; - for (i = 0; i < 8; i++) - crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); - return (crc_u); + uint8_t crc_u, i; + crc_u = crc; + crc_u ^= crc_seed; + for (i = 0; i < 8; i++) { + crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); + } + return (crc_u); } uint8_t get_crc8(uint8_t* Buf, uint8_t BufLen) { - uint8_t crc = 0, i; - for (i = 0; i < BufLen; i++) - crc = update_crc8(Buf[i], crc); - return (crc); + uint8_t crc = 0, i; + for (i = 0; i < BufLen; i++) { + crc = update_crc8(Buf[i], crc); + } + return (crc); } void makeTelemPackage(uint8_t temp, uint16_t voltage, uint16_t current, - uint16_t consumption, uint16_t e_rpm) + uint16_t consumption, uint16_t e_rpm) { - aTxBuffer[0] = temp; // temperature + aTxBuffer[0] = temp; // temperature - aTxBuffer[1] = (voltage >> 8) & 0xFF; // voltage hB - aTxBuffer[2] = voltage & 0xFF; // voltage lowB + aTxBuffer[1] = (voltage >> 8) & 0xFF; // voltage hB + aTxBuffer[2] = voltage & 0xFF; // voltage lowB - aTxBuffer[3] = (current >> 8) & 0xFF; // current - aTxBuffer[4] = current & 0xFF; // divide by 10 for Amps + aTxBuffer[3] = (current >> 8) & 0xFF; // current + aTxBuffer[4] = current & 0xFF; // divide by 10 for Amps - aTxBuffer[5] = (consumption >> 8) & 0xFF; // consumption - aTxBuffer[6] = consumption & 0xFF; // in mah + aTxBuffer[5] = (consumption >> 8) & 0xFF; // consumption + aTxBuffer[6] = consumption & 0xFF; // in mah - aTxBuffer[7] = (e_rpm >> 8) & 0xFF; // - aTxBuffer[8] = e_rpm & 0xFF; // eRpM *100 + aTxBuffer[7] = (e_rpm >> 8) & 0xFF; // + aTxBuffer[8] = e_rpm & 0xFF; // eRpM *100 - aTxBuffer[9] = get_crc8(aTxBuffer, 9); + aTxBuffer[9] = get_crc8(aTxBuffer, 9); } diff --git a/Mcu/g071/Src/stm32g0xx_it.c b/Mcu/g071/Src/stm32g0xx_it.c index 9799763e..cd2dc919 100644 --- a/Mcu/g071/Src/stm32g0xx_it.c +++ b/Mcu/g071/Src/stm32g0xx_it.c @@ -96,12 +96,12 @@ extern uint32_t commutation_interval; */ void NMI_Handler(void) { - /* USER CODE BEGIN NonMaskableInt_IRQn 0 */ + /* USER CODE BEGIN NonMaskableInt_IRQn 0 */ - /* USER CODE END NonMaskableInt_IRQn 0 */ - /* USER CODE BEGIN NonMaskableInt_IRQn 1 */ + /* USER CODE END NonMaskableInt_IRQn 0 */ + /* USER CODE BEGIN NonMaskableInt_IRQn 1 */ - /* USER CODE END NonMaskableInt_IRQn 1 */ + /* USER CODE END NonMaskableInt_IRQn 1 */ } /** @@ -109,13 +109,13 @@ void NMI_Handler(void) */ void HardFault_Handler(void) { - /* USER CODE BEGIN HardFault_IRQn 0 */ + /* USER CODE BEGIN HardFault_IRQn 0 */ - /* USER CODE END HardFault_IRQn 0 */ - while (1) { - /* USER CODE BEGIN W1_HardFault_IRQn 0 */ - /* USER CODE END W1_HardFault_IRQn 0 */ - } + /* USER CODE END HardFault_IRQn 0 */ + while (1) { + /* USER CODE BEGIN W1_HardFault_IRQn 0 */ + /* USER CODE END W1_HardFault_IRQn 0 */ + } } /** @@ -123,12 +123,12 @@ void HardFault_Handler(void) */ void SVC_Handler(void) { - /* USER CODE BEGIN SVC_IRQn 0 */ + /* USER CODE BEGIN SVC_IRQn 0 */ - /* USER CODE END SVC_IRQn 0 */ - /* USER CODE BEGIN SVC_IRQn 1 */ + /* USER CODE END SVC_IRQn 0 */ + /* USER CODE BEGIN SVC_IRQn 1 */ - /* USER CODE END SVC_IRQn 1 */ + /* USER CODE END SVC_IRQn 1 */ } /** @@ -136,12 +136,12 @@ void SVC_Handler(void) */ void PendSV_Handler(void) { - /* USER CODE BEGIN PendSV_IRQn 0 */ + /* USER CODE BEGIN PendSV_IRQn 0 */ - /* USER CODE END PendSV_IRQn 0 */ - /* USER CODE BEGIN PendSV_IRQn 1 */ + /* USER CODE END PendSV_IRQn 0 */ + /* USER CODE BEGIN PendSV_IRQn 1 */ - /* USER CODE END PendSV_IRQn 1 */ + /* USER CODE END PendSV_IRQn 1 */ } /** @@ -149,13 +149,13 @@ void PendSV_Handler(void) */ void SysTick_Handler(void) { - /* USER CODE BEGIN SysTick_IRQn 0 */ + /* USER CODE BEGIN SysTick_IRQn 0 */ - /* USER CODE END SysTick_IRQn 0 */ + /* USER CODE END SysTick_IRQn 0 */ - /* USER CODE BEGIN SysTick_IRQn 1 */ + /* USER CODE BEGIN SysTick_IRQn 1 */ - /* USER CODE END SysTick_IRQn 1 */ + /* USER CODE END SysTick_IRQn 1 */ } /******************************************************************************/ @@ -170,36 +170,36 @@ void SysTick_Handler(void) */ void DMA1_Channel1_IRQHandler(void) { - if (armed && dshot_telemetry) { - DMA1->IFCR |= DMA_IFCR_CGIF1; - DMA1_Channel1->CCR = 0x00; - if (out_put) { - receiveDshotDma(); - compute_dshot_flag = 2; - } else { - sendDshotDma(); - compute_dshot_flag = 1; - } - EXTI->SWIER1 |= LL_EXTI_LINE_15; - - return; - } - /* USER CODE BEGIN DMA1_Channel1_IRQn 0 */ - if (LL_DMA_IsActiveFlag_HT1(DMA1)) { - if (servoPwm) { - LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, - LL_TIM_IC_POLARITY_FALLING); - LL_DMA_ClearFlag_HT1(DMA1); - } + if (armed && dshot_telemetry) { + DMA1->IFCR |= DMA_IFCR_CGIF1; + DMA1_Channel1->CCR = 0x00; + if (out_put) { + receiveDshotDma(); + compute_dshot_flag = 2; + } else { + sendDshotDma(); + compute_dshot_flag = 1; } - if (LL_DMA_IsActiveFlag_TC1(DMA1) == 1) { - DMA1->IFCR |= DMA_IFCR_CGIF1; - DMA1_Channel1->CCR = 0x00; - transfercomplete(); - EXTI->SWIER1 |= LL_EXTI_LINE_15; - } else if (LL_DMA_IsActiveFlag_TE1(DMA1) == 1) { - LL_DMA_ClearFlag_GI1(DMA1); + EXTI->SWIER1 |= LL_EXTI_LINE_15; + + return; + } + /* USER CODE BEGIN DMA1_Channel1_IRQn 0 */ + if (LL_DMA_IsActiveFlag_HT1(DMA1)) { + if (servoPwm) { + LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, + LL_TIM_IC_POLARITY_FALLING); + LL_DMA_ClearFlag_HT1(DMA1); } + } + if (LL_DMA_IsActiveFlag_TC1(DMA1) == 1) { + DMA1->IFCR |= DMA_IFCR_CGIF1; + DMA1_Channel1->CCR = 0x00; + transfercomplete(); + EXTI->SWIER1 |= LL_EXTI_LINE_15; + } else if (LL_DMA_IsActiveFlag_TE1(DMA1) == 1) { + LL_DMA_ClearFlag_GI1(DMA1); + } } /** @@ -207,25 +207,25 @@ void DMA1_Channel1_IRQHandler(void) */ void DMA1_Channel2_3_IRQHandler(void) { - if (LL_DMA_IsActiveFlag_TC2(DMA1) == 1) { - LL_DMA_ClearFlag_GI2(DMA1); - ADC_DMA_Callback(); - } - if (LL_DMA_IsActiveFlag_TE2(DMA1) == 1) { - LL_DMA_ClearFlag_TE2(DMA1); - } - - if (LL_DMA_IsActiveFlag_TC3(DMA1)) { - send_telemetry = 0; - LL_DMA_ClearFlag_GI3(DMA1); - LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_3); - /* Call function Transmission complete Callback */ - } else if (LL_DMA_IsActiveFlag_TE3(DMA1)) { - LL_DMA_ClearFlag_GI3(DMA1); - LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_3); - /* Call Error function */ - // USART_TransferError_Callback(); - } + if (LL_DMA_IsActiveFlag_TC2(DMA1) == 1) { + LL_DMA_ClearFlag_GI2(DMA1); + ADC_DMA_Callback(); + } + if (LL_DMA_IsActiveFlag_TE2(DMA1) == 1) { + LL_DMA_ClearFlag_TE2(DMA1); + } + + if (LL_DMA_IsActiveFlag_TC3(DMA1)) { + send_telemetry = 0; + LL_DMA_ClearFlag_GI3(DMA1); + LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_3); + /* Call function Transmission complete Callback */ + } else if (LL_DMA_IsActiveFlag_TE3(DMA1)) { + LL_DMA_ClearFlag_GI3(DMA1); + LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_3); + /* Call Error function */ + // USART_TransferError_Callback(); + } } /** @@ -234,35 +234,35 @@ void DMA1_Channel2_3_IRQHandler(void) */ void ADC1_COMP_IRQHandler(void) { - if (LL_EXTI_IsActiveFallingFlag_0_31(LL_EXTI_LINE_18)) { - LL_EXTI_ClearFallingFlag_0_31(LL_EXTI_LINE_18); - if((INTERVAL_TIMER->CNT) > (commutation_interval >> 1)){ - interruptRoutine(); - } - return; + if (LL_EXTI_IsActiveFallingFlag_0_31(LL_EXTI_LINE_18)) { + LL_EXTI_ClearFallingFlag_0_31(LL_EXTI_LINE_18); + if ((INTERVAL_TIMER->CNT) > (commutation_interval >> 1)) { + interruptRoutine(); } + return; + } - if (LL_EXTI_IsActiveRisingFlag_0_31(LL_EXTI_LINE_18)) { - LL_EXTI_ClearRisingFlag_0_31(LL_EXTI_LINE_18); - if((INTERVAL_TIMER->CNT) > (commutation_interval >> 1)){ - interruptRoutine(); - } - return; - } - if (LL_EXTI_IsActiveFallingFlag_0_31(LL_EXTI_LINE_17)) { - LL_EXTI_ClearFallingFlag_0_31(LL_EXTI_LINE_17); - if((INTERVAL_TIMER->CNT) > (commutation_interval >> 1)){ - interruptRoutine(); + if (LL_EXTI_IsActiveRisingFlag_0_31(LL_EXTI_LINE_18)) { + LL_EXTI_ClearRisingFlag_0_31(LL_EXTI_LINE_18); + if ((INTERVAL_TIMER->CNT) > (commutation_interval >> 1)) { + interruptRoutine(); } - return; + return; + } + if (LL_EXTI_IsActiveFallingFlag_0_31(LL_EXTI_LINE_17)) { + LL_EXTI_ClearFallingFlag_0_31(LL_EXTI_LINE_17); + if ((INTERVAL_TIMER->CNT) > (commutation_interval >> 1)) { + interruptRoutine(); } - if (LL_EXTI_IsActiveRisingFlag_0_31(LL_EXTI_LINE_17)) { - LL_EXTI_ClearRisingFlag_0_31(LL_EXTI_LINE_17); - if((INTERVAL_TIMER->CNT) > (commutation_interval >> 1)){ - interruptRoutine(); - } - return; + return; + } + if (LL_EXTI_IsActiveRisingFlag_0_31(LL_EXTI_LINE_17)) { + LL_EXTI_ClearRisingFlag_0_31(LL_EXTI_LINE_17); + if ((INTERVAL_TIMER->CNT) > (commutation_interval >> 1)) { + interruptRoutine(); } + return; + } } /** @@ -270,50 +270,50 @@ void ADC1_COMP_IRQHandler(void) */ void TIM3_IRQHandler(void) { - /* USER CODE BEGIN TIM3_IRQn 0 */ - if (LL_TIM_IsActiveFlag_CC1(TIM3) == 1) { - LL_TIM_ClearFlag_CC1(TIM3); - } - - if (LL_TIM_IsActiveFlag_UPDATE(TIM3) == 1) { - LL_TIM_ClearFlag_UPDATE(TIM3); - // update_interupt++; - } - /* USER CODE END TIM3_IRQn 0 */ - /* USER CODE BEGIN TIM3_IRQn 1 */ - - /* USER CODE END TIM3_IRQn 1 */ + /* USER CODE BEGIN TIM3_IRQn 0 */ + if (LL_TIM_IsActiveFlag_CC1(TIM3) == 1) { + LL_TIM_ClearFlag_CC1(TIM3); + } + + if (LL_TIM_IsActiveFlag_UPDATE(TIM3) == 1) { + LL_TIM_ClearFlag_UPDATE(TIM3); + // update_interupt++; + } + /* USER CODE END TIM3_IRQn 0 */ + /* USER CODE BEGIN TIM3_IRQn 1 */ + + /* USER CODE END TIM3_IRQn 1 */ } void TIM16_IRQHandler(void) { - /* USER CODE BEGIN TIM3_IRQn 0 */ - if (LL_TIM_IsActiveFlag_CC1(TIM16) == 1) { - LL_TIM_ClearFlag_CC1(TIM16); - } - - if (LL_TIM_IsActiveFlag_UPDATE(TIM16) == 1) { - LL_TIM_ClearFlag_UPDATE(TIM16); - // update_interupt++; - } - /* USER CODE END TIM3_IRQn 0 */ - /* USER CODE BEGIN TIM3_IRQn 1 */ - - /* USER CODE END TIM3_IRQn 1 */ + /* USER CODE BEGIN TIM3_IRQn 0 */ + if (LL_TIM_IsActiveFlag_CC1(TIM16) == 1) { + LL_TIM_ClearFlag_CC1(TIM16); + } + + if (LL_TIM_IsActiveFlag_UPDATE(TIM16) == 1) { + LL_TIM_ClearFlag_UPDATE(TIM16); + // update_interupt++; + } + /* USER CODE END TIM3_IRQn 0 */ + /* USER CODE BEGIN TIM3_IRQn 1 */ + + /* USER CODE END TIM3_IRQn 1 */ } void TIM6_DAC_LPTIM1_IRQHandler(void) { - /* USER CODE BEGIN TIM6_DAC_LPTIM1_IRQn 0 */ - if (LL_TIM_IsActiveFlag_UPDATE(TIM6) == 1) { - LL_TIM_ClearFlag_UPDATE(TIM6); - tenKhzRoutine(); - } - /* USER CODE END TIM6_DAC_LPTIM1_IRQn 0 */ + /* USER CODE BEGIN TIM6_DAC_LPTIM1_IRQn 0 */ + if (LL_TIM_IsActiveFlag_UPDATE(TIM6) == 1) { + LL_TIM_ClearFlag_UPDATE(TIM6); + tenKhzRoutine(); + } + /* USER CODE END TIM6_DAC_LPTIM1_IRQn 0 */ - /* USER CODE BEGIN TIM6_DAC_LPTIM1_IRQn 1 */ + /* USER CODE BEGIN TIM6_DAC_LPTIM1_IRQn 1 */ - /* USER CODE END TIM6_DAC_LPTIM1_IRQn 1 */ + /* USER CODE END TIM6_DAC_LPTIM1_IRQn 1 */ } /** @@ -321,36 +321,36 @@ void TIM6_DAC_LPTIM1_IRQHandler(void) */ void TIM14_IRQHandler(void) { - interrupt_time = UTILITY_TIMER->CNT; - PeriodElapsedCallback(); - LL_TIM_ClearFlag_UPDATE(TIM14); - interrupt_time = ((uint16_t)UTILITY_TIMER->CNT) - interrupt_time; + interrupt_time = UTILITY_TIMER->CNT; + PeriodElapsedCallback(); + LL_TIM_ClearFlag_UPDATE(TIM14); + interrupt_time = ((uint16_t)UTILITY_TIMER->CNT) - interrupt_time; } /* USER CODE BEGIN 1 */ void DMA1_Ch4_7_DMAMUX1_OVR_IRQHandler(void) { - /* USER CODE BEGIN DMA1_Ch4_7_DMAMUX1_OVR_IRQn 0 */ - if (LL_DMA_IsActiveFlag_HT6(DMA1)) { - } - if (LL_DMA_IsActiveFlag_TC6(DMA1) == 1) { - LL_DMA_ClearFlag_GI6(DMA1); - - LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_6); - LL_TIM_DisableAllOutputs(TIM16); - LL_TIM_DisableCounter(TIM16); - dma_busy = 0; - } else if (LL_DMA_IsActiveFlag_TE6(DMA1) == 1) { - LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_6); - LL_TIM_DisableAllOutputs(TIM16); - LL_TIM_DisableCounter(TIM16); - dma_busy = 0; - LL_DMA_ClearFlag_GI6(DMA1); - } + /* USER CODE BEGIN DMA1_Ch4_7_DMAMUX1_OVR_IRQn 0 */ + if (LL_DMA_IsActiveFlag_HT6(DMA1)) { + } + if (LL_DMA_IsActiveFlag_TC6(DMA1) == 1) { + LL_DMA_ClearFlag_GI6(DMA1); + + LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_6); + LL_TIM_DisableAllOutputs(TIM16); + LL_TIM_DisableCounter(TIM16); + dma_busy = 0; + } else if (LL_DMA_IsActiveFlag_TE6(DMA1) == 1) { + LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_6); + LL_TIM_DisableAllOutputs(TIM16); + LL_TIM_DisableCounter(TIM16); + dma_busy = 0; + LL_DMA_ClearFlag_GI6(DMA1); + } } void EXTI4_15_IRQHandler(void) { - LL_EXTI_ClearRisingFlag_0_31(LL_EXTI_LINE_15); - processDshot(); + LL_EXTI_ClearRisingFlag_0_31(LL_EXTI_LINE_15); + processDshot(); } diff --git a/Mcu/g071/Src/system_stm32g0xx.c b/Mcu/g071/Src/system_stm32g0xx.c index d6aac35c..b8e02ef8 100644 --- a/Mcu/g071/Src/system_stm32g0xx.c +++ b/Mcu/g071/Src/system_stm32g0xx.c @@ -144,7 +144,8 @@ uint32_t SystemCoreClock = 16000000UL; const uint32_t AHBPrescTable[16UL] = { 0UL, 0UL, 0UL, 0UL, 0UL, 0UL, 0UL, 0UL, - 1UL, 2UL, 3UL, 4UL, 6UL, 7UL, 8UL, 9UL }; + 1UL, 2UL, 3UL, 4UL, 6UL, 7UL, 8UL, 9UL + }; const uint32_t APBPrescTable[8UL] = { 0UL, 0UL, 0UL, 0UL, 1UL, 2UL, 3UL, 4UL }; /** @@ -170,12 +171,12 @@ const uint32_t APBPrescTable[8UL] = { 0UL, 0UL, 0UL, 0UL, 1UL, 2UL, 3UL, 4UL }; */ void SystemInit(void) { - /* Configure the Vector Table location add offset address - * ------------------*/ + /* Configure the Vector Table location add offset address + * ------------------*/ #ifdef VECT_TAB_SRAM - SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */ + SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */ #else - SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */ + SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */ #endif } @@ -225,60 +226,58 @@ void SystemInit(void) */ void SystemCoreClockUpdate(void) { - uint32_t tmp; - uint32_t pllvco; - uint32_t pllr; - uint32_t pllsource; - uint32_t pllm; - uint32_t hsidiv; - - /* Get SYSCLK source - * -------------------------------------------------------*/ - switch (RCC->CFGR & RCC_CFGR_SWS) { - case RCC_CFGR_SWS_HSE: /* HSE used as system clock */ - SystemCoreClock = HSE_VALUE; - break; - - case RCC_CFGR_SWS_LSI: /* LSI used as system clock */ - SystemCoreClock = LSI_VALUE; - break; - - case RCC_CFGR_SWS_LSE: /* LSE used as system clock */ - SystemCoreClock = LSE_VALUE; - break; - - case RCC_CFGR_SWS_PLL: /* PLL used as system clock */ - /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLLM) * PLLN - SYSCLK = PLL_VCO / PLLR - */ - pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC); - pllm = ((RCC->PLLCFGR & RCC_PLLCFGR_PLLM) >> RCC_PLLCFGR_PLLM_Pos) + 1UL; - - if (pllsource == 0x03UL) /* HSE used as PLL clock source */ - { - pllvco = (HSE_VALUE / pllm); - } else /* HSI used as PLL clock source */ - { - pllvco = (HSI_VALUE / pllm); - } - pllvco = pllvco * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> RCC_PLLCFGR_PLLN_Pos); - pllr = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLR) >> RCC_PLLCFGR_PLLR_Pos) + 1UL); - - SystemCoreClock = pllvco / pllr; - break; - - case RCC_CFGR_SWS_HSI: /* HSI used as system clock */ - default: /* HSI used as system clock */ - hsidiv = (1UL << ((READ_BIT(RCC->CR, RCC_CR_HSIDIV)) >> RCC_CR_HSIDIV_Pos)); - SystemCoreClock = (HSI_VALUE / hsidiv); - break; + uint32_t tmp; + uint32_t pllvco; + uint32_t pllr; + uint32_t pllsource; + uint32_t pllm; + uint32_t hsidiv; + + /* Get SYSCLK source + * -------------------------------------------------------*/ + switch (RCC->CFGR & RCC_CFGR_SWS) { + case RCC_CFGR_SWS_HSE: /* HSE used as system clock */ + SystemCoreClock = HSE_VALUE; + break; + + case RCC_CFGR_SWS_LSI: /* LSI used as system clock */ + SystemCoreClock = LSI_VALUE; + break; + + case RCC_CFGR_SWS_LSE: /* LSE used as system clock */ + SystemCoreClock = LSE_VALUE; + break; + + case RCC_CFGR_SWS_PLL: /* PLL used as system clock */ + /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLLM) * PLLN + SYSCLK = PLL_VCO / PLLR + */ + pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC); + pllm = ((RCC->PLLCFGR & RCC_PLLCFGR_PLLM) >> RCC_PLLCFGR_PLLM_Pos) + 1UL; + + if (pllsource == 0x03UL) { /* HSE used as PLL clock source */ + pllvco = (HSE_VALUE / pllm); + } else { /* HSI used as PLL clock source */ + pllvco = (HSI_VALUE / pllm); } - /* Compute HCLK clock frequency - * --------------------------------------------*/ - /* Get HCLK prescaler */ - tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> RCC_CFGR_HPRE_Pos)]; - /* HCLK clock frequency */ - SystemCoreClock >>= tmp; + pllvco = pllvco * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> RCC_PLLCFGR_PLLN_Pos); + pllr = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLR) >> RCC_PLLCFGR_PLLR_Pos) + 1UL); + + SystemCoreClock = pllvco / pllr; + break; + + case RCC_CFGR_SWS_HSI: /* HSI used as system clock */ + default: /* HSI used as system clock */ + hsidiv = (1UL << ((READ_BIT(RCC->CR, RCC_CR_HSIDIV)) >> RCC_CR_HSIDIV_Pos)); + SystemCoreClock = (HSI_VALUE / hsidiv); + break; + } + /* Compute HCLK clock frequency + * --------------------------------------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> RCC_CFGR_HPRE_Pos)]; + /* HCLK clock frequency */ + SystemCoreClock >>= tmp; } /** diff --git a/Mcu/g431/Inc/serial_telemetry.h b/Mcu/g431/Inc/serial_telemetry.h index fad4ee7a..77656728 100644 --- a/Mcu/g431/Inc/serial_telemetry.h +++ b/Mcu/g431/Inc/serial_telemetry.h @@ -11,7 +11,7 @@ #define SERIAL_TELEMETRY_H_ void makeTelemPackage(uint8_t temp, uint16_t voltage, uint16_t current, - uint16_t consumption, uint16_t e_rpm); + uint16_t consumption, uint16_t e_rpm); void telem_UART_Init(void); void send_telem_DMA(); diff --git a/Mcu/g431/Src/ADC.c b/Mcu/g431/Src/ADC.c index 23938f53..61c4d63f 100644 --- a/Mcu/g431/Src/ADC.c +++ b/Mcu/g431/Src/ADC.c @@ -4,112 +4,109 @@ * Created on: May 20, 2020 * Author: Alka */ - #include "ADC.h" +#include "ADC.h" - #ifdef USE_ADC_INPUT - uint16_t ADCDataDMA[4]; - #else - uint16_t ADCDataDMA[3]; - #endif +#ifdef USE_ADC_INPUT +uint16_t ADCDataDMA[4]; +#else +uint16_t ADCDataDMA[3]; +#endif - extern uint16_t ADC_raw_temp; - extern uint16_t ADC_raw_volts; - extern uint16_t ADC_raw_current; - extern uint16_t ADC_raw_input; +extern uint16_t ADC_raw_temp; +extern uint16_t ADC_raw_volts; +extern uint16_t ADC_raw_current; +extern uint16_t ADC_raw_input; #define ADC_DELAY_CALIB_ENABLE_CPU_CYCLES \ (LL_ADC_DELAY_CALIB_ENABLE_ADC_CYCLES * 64) - void ADC_DMA_Callback() -{ // read dma buffer and set extern variables +void ADC_DMA_Callback() +{ + // read dma buffer and set extern variables - #ifdef USE_ADC_INPUT - ADC_raw_temp = ADCDataDMA[3]; - ADC_raw_volts = ADCDataDMA[1] / 2; - ADC_raw_current = ADCDataDMA[2]; - ADC_raw_input = ADCDataDMA[0]; +#ifdef USE_ADC_INPUT + ADC_raw_temp = ADCDataDMA[3]; + ADC_raw_volts = ADCDataDMA[1] / 2; + ADC_raw_current = ADCDataDMA[2]; + ADC_raw_input = ADCDataDMA[0]; - #else - ADC_raw_temp = ADCDataDMA[0]; - ADC_raw_volts = ADCDataDMA[1]; +#else + ADC_raw_temp = ADCDataDMA[0]; + ADC_raw_volts = ADCDataDMA[1]; // ADC_raw_current = ADCDataDMA[0]; - #endif - } +#endif +} - void enableADC_DMA() -{ // enables channel +void enableADC_DMA() +{ + // enables channel - NVIC_SetPriority(DMA1_Channel1_IRQn, 3); - NVIC_EnableIRQ(DMA1_Channel1_IRQn); + NVIC_SetPriority(DMA1_Channel1_IRQn, 3); + NVIC_EnableIRQ(DMA1_Channel1_IRQn); - LL_DMA_ConfigAddresses( - DMA1, LL_DMA_CHANNEL_2, - LL_ADC_DMA_GetRegAddr(ADC1, LL_ADC_DMA_REG_REGULAR_DATA), - (uint32_t)&ADCDataDMA, LL_DMA_DIRECTION_PERIPH_TO_MEMORY); + LL_DMA_ConfigAddresses( + DMA1, LL_DMA_CHANNEL_2, + LL_ADC_DMA_GetRegAddr(ADC1, LL_ADC_DMA_REG_REGULAR_DATA), + (uint32_t)&ADCDataDMA, LL_DMA_DIRECTION_PERIPH_TO_MEMORY); - /* Set DMA transfer size */ - #ifdef USE_ADC_INPUT - LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_2, 4); - #else - LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_2, 2); + /* Set DMA transfer size */ +#ifdef USE_ADC_INPUT + LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_2, 4); +#else + LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_2, 2); - #endif - /* Enable DMA transfer interruption: transfer complete */ - // LL_DMA_EnableIT_TC(DMA1, LL_DMA_CHANNEL_2); +#endif + /* Enable DMA transfer interruption: transfer complete */ + // LL_DMA_EnableIT_TC(DMA1, LL_DMA_CHANNEL_2); - /* Enable DMA transfer interruption: transfer error */ -// LL_DMA_EnableIT_TE(DMA1, LL_DMA_CHANNEL_2); + /* Enable DMA transfer interruption: transfer error */ + // LL_DMA_EnableIT_TE(DMA1, LL_DMA_CHANNEL_2); - /*## Activation of DMA - * #####################################################*/ - /* Enable the DMA transfer */ - LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_2); + /*## Activation of DMA + * #####################################################*/ + /* Enable the DMA transfer */ + LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_2); } void activateADC(void) { __IO uint32_t wait_loop_index = 0U; - #if (USE_TIMEOUT == 1) +#if (USE_TIMEOUT == 1) uint32_t Timeout = 0U; /* Variable used for timeout management */ - #endif /* USE_TIMEOUT */ - if (LL_ADC_IsEnabled(ADC1) == 0) - { +#endif /* USE_TIMEOUT */ + if (LL_ADC_IsEnabled(ADC1) == 0) { /* Disable ADC deep power down (enabled by default after reset state) */ LL_ADC_DisableDeepPowerDown(ADC1); - + /* Enable ADC internal voltage regulator */ LL_ADC_EnableInternalRegulator(ADC1); wait_loop_index = ((LL_ADC_DELAY_INTERNAL_REGUL_STAB_US * (SystemCoreClock / (100000 * 2))) / 10); - while(wait_loop_index != 0) - { + while (wait_loop_index != 0) { wait_loop_index--; } - + /* Run ADC self calibration */ LL_ADC_StartCalibration(ADC1, LL_ADC_SINGLE_ENDED); - + /* Poll for ADC effectively calibrated */ - #if (USE_TIMEOUT == 1) +#if (USE_TIMEOUT == 1) Timeout = ADC_CALIBRATION_TIMEOUT_MS; - #endif /* USE_TIMEOUT */ - - while (LL_ADC_IsCalibrationOnGoing(ADC1) != 0) - { +#endif /* USE_TIMEOUT */ + + while (LL_ADC_IsCalibrationOnGoing(ADC1) != 0) { } wait_loop_index = (ADC_DELAY_CALIB_ENABLE_CPU_CYCLES >> 1); - while(wait_loop_index != 0) - { + while (wait_loop_index != 0) { wait_loop_index--; } - + /* Enable ADC */ LL_ADC_Enable(ADC1); - - while (LL_ADC_IsActiveFlag_ADRDY(ADC1) == 0) - { + + while (LL_ADC_IsActiveFlag_ADRDY(ADC1) == 0) { } - } - + } + } void ADC_Init(void) @@ -193,8 +190,7 @@ void ADC_Init(void) /* is only a few CPU processing cycles. */ uint32_t wait_loop_index; wait_loop_index = ((LL_ADC_DELAY_INTERNAL_REGUL_STAB_US * (SystemCoreClock / (100000 * 2))) / 10); - while(wait_loop_index != 0) - { + while (wait_loop_index != 0) { wait_loop_index--; } diff --git a/Mcu/g431/Src/IO.c b/Mcu/g431/Src/IO.c index 45ac7ca5..c527ecfa 100644 --- a/Mcu/g431/Src/IO.c +++ b/Mcu/g431/Src/IO.c @@ -21,80 +21,86 @@ char out_put = 0; void receiveDshotDma() { #ifdef USE_TIMER_3_CHANNEL_1 - RCC->APBRSTR1 |= LL_APB1_GRP1_PERIPH_TIM3; - RCC->APBRSTR1 &= ~LL_APB1_GRP1_PERIPH_TIM3; - IC_TIMER_REGISTER->CCMR1 = 0x41; - IC_TIMER_REGISTER->CCER = 0xa; + RCC->APBRSTR1 |= LL_APB1_GRP1_PERIPH_TIM3; + RCC->APBRSTR1 &= ~LL_APB1_GRP1_PERIPH_TIM3; + IC_TIMER_REGISTER->CCMR1 = 0x41; + IC_TIMER_REGISTER->CCER = 0xa; #else - LL_APB2_GRP1_ForceReset(LL_APB2_GRP1_PERIPH_TIM15); // de-init timer 2 - LL_APB2_GRP1_ReleaseReset(LL_APB2_GRP1_PERIPH_TIM15); - IC_TIMER_REGISTER->CCMR1 = 0x41; - IC_TIMER_REGISTER->CCER = 0xa; + LL_APB2_GRP1_ForceReset(LL_APB2_GRP1_PERIPH_TIM15); // de-init timer 2 + LL_APB2_GRP1_ReleaseReset(LL_APB2_GRP1_PERIPH_TIM15); + IC_TIMER_REGISTER->CCMR1 = 0x41; + IC_TIMER_REGISTER->CCER = 0xa; #endif - IC_TIMER_REGISTER->PSC = ic_timer_prescaler; - IC_TIMER_REGISTER->ARR = 0xFFFF; - IC_TIMER_REGISTER->EGR |= TIM_EGR_UG; - out_put = 0; - IC_TIMER_REGISTER->CNT = 0; - DMA1_Channel1->CMAR = (uint32_t)&dma_buffer; - DMA1_Channel1->CPAR = (uint32_t)&IC_TIMER_REGISTER->CCR1; - DMA1_Channel1->CNDTR = buffersize; - DMA1_Channel1->CCR = 0x98b; - IC_TIMER_REGISTER->DIER |= TIM_DIER_CC1DE; - IC_TIMER_REGISTER->CCER |= IC_TIMER_CHANNEL; - IC_TIMER_REGISTER->CR1 |= TIM_CR1_CEN; + IC_TIMER_REGISTER->PSC = ic_timer_prescaler; + IC_TIMER_REGISTER->ARR = 0xFFFF; + IC_TIMER_REGISTER->EGR |= TIM_EGR_UG; + out_put = 0; + IC_TIMER_REGISTER->CNT = 0; + DMA1_Channel1->CMAR = (uint32_t)&dma_buffer; + DMA1_Channel1->CPAR = (uint32_t)&IC_TIMER_REGISTER->CCR1; + DMA1_Channel1->CNDTR = buffersize; + DMA1_Channel1->CCR = 0x98b; + IC_TIMER_REGISTER->DIER |= TIM_DIER_CC1DE; + IC_TIMER_REGISTER->CCER |= IC_TIMER_CHANNEL; + IC_TIMER_REGISTER->CR1 |= TIM_CR1_CEN; } void sendDshotDma() { #ifdef USE_TIMER_3_CHANNEL_1 - RCC->APBRSTR1 |= LL_APB1_GRP1_PERIPH_TIM3; - RCC->APBRSTR1 &= ~LL_APB1_GRP1_PERIPH_TIM3; - IC_TIMER_REGISTER->CCMR1 = 0x60; - IC_TIMER_REGISTER->CCER = 0x3; + RCC->APBRSTR1 |= LL_APB1_GRP1_PERIPH_TIM3; + RCC->APBRSTR1 &= ~LL_APB1_GRP1_PERIPH_TIM3; + IC_TIMER_REGISTER->CCMR1 = 0x60; + IC_TIMER_REGISTER->CCER = 0x3; #endif - LL_APB2_GRP1_ForceReset(LL_APB2_GRP1_PERIPH_TIM15); // de-init timer 2 - LL_APB2_GRP1_ReleaseReset(LL_APB2_GRP1_PERIPH_TIM15); - IC_TIMER_REGISTER->CCMR1 = 0x60; - IC_TIMER_REGISTER->CCER = 0x3; + LL_APB2_GRP1_ForceReset(LL_APB2_GRP1_PERIPH_TIM15); // de-init timer 2 + LL_APB2_GRP1_ReleaseReset(LL_APB2_GRP1_PERIPH_TIM15); + IC_TIMER_REGISTER->CCMR1 = 0x60; + IC_TIMER_REGISTER->CCER = 0x3; - IC_TIMER_REGISTER->PSC = output_timer_prescaler; - IC_TIMER_REGISTER->ARR = 92; - out_put = 1; - LL_TIM_GenerateEvent_UPDATE(IC_TIMER_REGISTER); + IC_TIMER_REGISTER->PSC = output_timer_prescaler; + IC_TIMER_REGISTER->ARR = 92; + out_put = 1; + LL_TIM_GenerateEvent_UPDATE(IC_TIMER_REGISTER); - DMA1_Channel1->CMAR = (uint32_t)&gcr; - DMA1_Channel1->CPAR = (uint32_t)&IC_TIMER_REGISTER->CCR1; - DMA1_Channel1->CNDTR = 23 + buffer_padding; - DMA1_Channel1->CCR = 0x99b; - IC_TIMER_REGISTER->DIER |= TIM_DIER_CC1DE; - IC_TIMER_REGISTER->CCER |= IC_TIMER_CHANNEL; - IC_TIMER_REGISTER->BDTR |= TIM_BDTR_MOE; - IC_TIMER_REGISTER->CR1 |= TIM_CR1_CEN; + DMA1_Channel1->CMAR = (uint32_t)&gcr; + DMA1_Channel1->CPAR = (uint32_t)&IC_TIMER_REGISTER->CCR1; + DMA1_Channel1->CNDTR = 23 + buffer_padding; + DMA1_Channel1->CCR = 0x99b; + IC_TIMER_REGISTER->DIER |= TIM_DIER_CC1DE; + IC_TIMER_REGISTER->CCER |= IC_TIMER_CHANNEL; + IC_TIMER_REGISTER->BDTR |= TIM_BDTR_MOE; + IC_TIMER_REGISTER->CR1 |= TIM_CR1_CEN; } -uint8_t getInputPinState() { return (INPUT_PIN_PORT->IDR & INPUT_PIN); } +uint8_t getInputPinState() +{ + return (INPUT_PIN_PORT->IDR & INPUT_PIN); +} void setInputPolarityRising() { - LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, - LL_TIM_IC_POLARITY_RISING); + LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, + LL_TIM_IC_POLARITY_RISING); } void setInputPullDown() { - LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_DOWN); + LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_DOWN); } void setInputPullUp() { - LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_UP); + LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_UP); } -void enableHalfTransferInt() { LL_DMA_EnableIT_HT(DMA1, INPUT_DMA_CHANNEL); } +void enableHalfTransferInt() +{ + LL_DMA_EnableIT_HT(DMA1, INPUT_DMA_CHANNEL); +} void setInputPullNone() { - LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_NO); + LL_GPIO_SetPinPull(INPUT_PIN_PORT, INPUT_PIN, LL_GPIO_PULL_NO); } diff --git a/Mcu/g431/Src/comparator.c b/Mcu/g431/Src/comparator.c index 35c9b085..e159d3d0 100644 --- a/Mcu/g431/Src/comparator.c +++ b/Mcu/g431/Src/comparator.c @@ -40,53 +40,56 @@ uint32_t current_EXTI_LINE = LL_EXTI_LINE_22; uint8_t getCompOutputLevel() { // return (active_COMP->CSR >> 30 & 1); - return LL_COMP_ReadOutputLevel(active_COMP); + return LL_COMP_ReadOutputLevel(active_COMP); } void maskPhaseInterrupts() { - EXTI->IMR1 &= ~(1 << 21); - EXTI->IMR1 &= ~(1 << 22); - EXTI->PR1 = LL_EXTI_LINE_22; - EXTI->PR1 = LL_EXTI_LINE_21; + EXTI->IMR1 &= ~(1 << 21); + EXTI->IMR1 &= ~(1 << 22); + EXTI->PR1 = LL_EXTI_LINE_22; + EXTI->PR1 = LL_EXTI_LINE_21; } -void enableCompInterrupts() { EXTI->IMR1 |= current_EXTI_LINE; } +void enableCompInterrupts() +{ + EXTI->IMR1 |= current_EXTI_LINE; +} void changeCompInput() { - if (step == 1 || step == 4) { // c floating + if (step == 1 || step == 4) { // c floating - current_EXTI_LINE = PHASE_C_EXTI_LINE; - active_COMP = PHASE_C_COMP_NUMBER; + current_EXTI_LINE = PHASE_C_EXTI_LINE; + active_COMP = PHASE_C_COMP_NUMBER; - LL_COMP_ConfigInputs(active_COMP, PHASE_C_COMP, PHASE_C_INPUT_PLUS); - } + LL_COMP_ConfigInputs(active_COMP, PHASE_C_COMP, PHASE_C_INPUT_PLUS); + } - if (step == 2 || step == 5) { // a floating + if (step == 2 || step == 5) { // a floating - current_EXTI_LINE = PHASE_A_EXTI_LINE; - active_COMP = PHASE_A_COMP_NUMBER; + current_EXTI_LINE = PHASE_A_EXTI_LINE; + active_COMP = PHASE_A_COMP_NUMBER; - LL_COMP_ConfigInputs(active_COMP, PHASE_A_COMP, PHASE_A_INPUT_PLUS); - } + LL_COMP_ConfigInputs(active_COMP, PHASE_A_COMP, PHASE_A_INPUT_PLUS); + } - if (step == 3 || step == 6) { // b floating + if (step == 3 || step == 6) { // b floating - current_EXTI_LINE = PHASE_B_EXTI_LINE; - active_COMP = PHASE_B_COMP_NUMBER; + current_EXTI_LINE = PHASE_B_EXTI_LINE; + active_COMP = PHASE_B_COMP_NUMBER; - LL_COMP_ConfigInputs(active_COMP, PHASE_B_COMP, PHASE_B_INPUT_PLUS); - } - if (rising) { - LL_EXTI_DisableRisingTrig_0_31(LL_EXTI_LINE_22); - LL_EXTI_DisableRisingTrig_0_31(LL_EXTI_LINE_21); - LL_EXTI_EnableFallingTrig_0_31(current_EXTI_LINE); - } else { // falling bemf - LL_EXTI_EnableRisingTrig_0_31(current_EXTI_LINE); - LL_EXTI_DisableFallingTrig_0_31(LL_EXTI_LINE_21); - LL_EXTI_DisableFallingTrig_0_31(LL_EXTI_LINE_22); - } + LL_COMP_ConfigInputs(active_COMP, PHASE_B_COMP, PHASE_B_INPUT_PLUS); + } + if (rising) { + LL_EXTI_DisableRisingTrig_0_31(LL_EXTI_LINE_22); + LL_EXTI_DisableRisingTrig_0_31(LL_EXTI_LINE_21); + LL_EXTI_EnableFallingTrig_0_31(current_EXTI_LINE); + } else { // falling bemf + LL_EXTI_EnableRisingTrig_0_31(current_EXTI_LINE); + LL_EXTI_DisableFallingTrig_0_31(LL_EXTI_LINE_21); + LL_EXTI_DisableFallingTrig_0_31(LL_EXTI_LINE_22); + } } // void changeCompInput() { diff --git a/Mcu/g431/Src/eeprom.c b/Mcu/g431/Src/eeprom.c index 79afeaee..3d9c8413 100644 --- a/Mcu/g431/Src/eeprom.c +++ b/Mcu/g431/Src/eeprom.c @@ -16,62 +16,62 @@ uint32_t FLASH_FKEY2 = 0xCDEF89AB; void save_flash_nolib(uint8_t* data, int length, uint32_t add) { - uint32_t data_to_FLASH[length / 4]; - memset(data_to_FLASH, 0, length / 4); - for (int i = 0; i < length / 4; i++) { - data_to_FLASH[i] = data[i * 4 + 3] << 24 | data[i * 4 + 2] << 16 | data[i * 4 + 1] << 8 | data[i * 4]; // make 16 bit - } - volatile uint32_t data_length = length / 4; + uint32_t data_to_FLASH[length / 4]; + memset(data_to_FLASH, 0, length / 4); + for (int i = 0; i < length / 4; i++) { + data_to_FLASH[i] = data[i * 4 + 3] << 24 | data[i * 4 + 2] << 16 | data[i * 4 + 1] << 8 | data[i * 4]; // make 16 bit + } + volatile uint32_t data_length = length / 4; + + // unlock flash - // unlock flash + while ((FLASH->SR & FLASH_SR_BSY) != 0) { + /* add time-out*/ + } + if ((FLASH->CR & FLASH_CR_LOCK) != 0) { + FLASH->KEYR = FLASH_FKEY1; + FLASH->KEYR = FLASH_FKEY2; + } + // erase page if address even divisable by 1024 + if ((add % 2048) == 0) { + FLASH->CR |= FLASH_CR_PER; + FLASH->CR |= (add / 2048) << 3; + FLASH->CR |= FLASH_CR_STRT; while ((FLASH->SR & FLASH_SR_BSY) != 0) { - /* add time-out*/ + /* add time-out */ } - if ((FLASH->CR & FLASH_CR_LOCK) != 0) { - FLASH->KEYR = FLASH_FKEY1; - FLASH->KEYR = FLASH_FKEY2; + if ((FLASH->SR & FLASH_SR_EOP) != 0) { + FLASH->SR = FLASH_SR_EOP; + } else { + /* error */ } + FLASH->CR &= ~FLASH_CR_PER; + } - // erase page if address even divisable by 1024 - if ((add % 2048) == 0) { - FLASH->CR |= FLASH_CR_PER; - FLASH->CR |= (add / 2048) << 3; - FLASH->CR |= FLASH_CR_STRT; - while ((FLASH->SR & FLASH_SR_BSY) != 0) { - /* add time-out */ - } - if ((FLASH->SR & FLASH_SR_EOP) != 0) { - FLASH->SR = FLASH_SR_EOP; - } else { - /* error */ - } - FLASH->CR &= ~FLASH_CR_PER; + volatile uint32_t write_cnt = 0, index = 0; + while (index < data_length) { + FLASH->CR |= FLASH_CR_PG; /* (1) */ + *(__IO uint32_t*)(add + write_cnt) = data_to_FLASH[index]; + *(__IO uint32_t*)(add + write_cnt + 4) = data_to_FLASH[index + 1]; + while ((FLASH->SR & FLASH_SR_BSY) != 0) { /* add time-out */ } - - volatile uint32_t write_cnt = 0, index = 0; - while (index < data_length) { - FLASH->CR |= FLASH_CR_PG; /* (1) */ - *(__IO uint32_t*)(add + write_cnt) = data_to_FLASH[index]; - *(__IO uint32_t*)(add + write_cnt + 4) = data_to_FLASH[index + 1]; - while ((FLASH->SR & FLASH_SR_BSY) != 0) { /* add time-out */ - } - if ((FLASH->SR & FLASH_SR_EOP) != 0) { - FLASH->SR = FLASH_SR_EOP; - } else { - /* error */ - } - FLASH->CR &= ~FLASH_CR_PG; - write_cnt += 8; - index += 2; + if ((FLASH->SR & FLASH_SR_EOP) != 0) { + FLASH->SR = FLASH_SR_EOP; + } else { + /* error */ } - SET_BIT(FLASH->CR, FLASH_CR_LOCK); + FLASH->CR &= ~FLASH_CR_PG; + write_cnt += 8; + index += 2; + } + SET_BIT(FLASH->CR, FLASH_CR_LOCK); } void read_flash_bin(uint8_t* data, uint32_t add, int out_buff_len) { - // volatile uint32_t read_data; - for (int i = 0; i < out_buff_len; i++) { - data[i] = *(uint8_t*)(add + i); - } + // volatile uint32_t read_data; + for (int i = 0; i < out_buff_len; i++) { + data[i] = *(uint8_t*)(add + i); + } } diff --git a/Mcu/g431/Src/peripherals.c b/Mcu/g431/Src/peripherals.c index 24f5b29b..9cea7674 100644 --- a/Mcu/g431/Src/peripherals.c +++ b/Mcu/g431/Src/peripherals.c @@ -20,140 +20,140 @@ void initCorePeripherals(void) { - LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_SYSCFG); - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_PWR); - FLASH->ACR |= FLASH_ACR_PRFTEN; //// prefetch buffer enable - // NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_4); - LL_PWR_DisableUCPDDeadBattery(); - SystemClock_Config(); - MX_GPIO_Init(); - MX_DMA_Init(); - MX_COMP2_Init(); - MX_TIM1_Init(); - MX_TIM2_Init(); - MX_TIM16_Init(); - MX_COMP1_Init(); - MX_TIM17_Init(); - MX_TIM6_Init(); - MX_TIM15_Init(); -// telem_UART_Init(); + LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_SYSCFG); + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_PWR); + FLASH->ACR |= FLASH_ACR_PRFTEN; //// prefetch buffer enable + // NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_4); + LL_PWR_DisableUCPDDeadBattery(); + SystemClock_Config(); + MX_GPIO_Init(); + MX_DMA_Init(); + MX_COMP2_Init(); + MX_TIM1_Init(); + MX_TIM2_Init(); + MX_TIM16_Init(); + MX_COMP1_Init(); + MX_TIM17_Init(); + MX_TIM6_Init(); + MX_TIM15_Init(); + // telem_UART_Init(); #ifdef USE_LED_STRIP - WS2812_Init(); + WS2812_Init(); #endif } void initAfterJump() { - SCB->VTOR = 0x08001000; - __enable_irq(); + SCB->VTOR = 0x08001000; + __enable_irq(); } void SystemClock_Config(void) { - LL_FLASH_SetLatency(LL_FLASH_LATENCY_4); - while (LL_FLASH_GetLatency() != LL_FLASH_LATENCY_4) { - } - LL_PWR_SetRegulVoltageScaling(LL_PWR_REGU_VOLTAGE_SCALE1); - LL_RCC_HSI_Enable(); - /* Wait till HSI is ready */ - while (LL_RCC_HSI_IsReady() != 1) { - } - - LL_RCC_HSI_SetCalibTrimming(64); - LL_RCC_LSI_Enable(); - /* Wait till LSI is ready */ - while (LL_RCC_LSI_IsReady() != 1) { - } - - LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSI, LL_RCC_PLLM_DIV_4, 75, - LL_RCC_PLLR_DIV_2); - LL_RCC_PLL_EnableDomain_SYS(); - LL_RCC_PLL_Enable(); - /* Wait till PLL is ready */ - while (LL_RCC_PLL_IsReady() != 1) { - } - - LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL); - LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_2); - /* Wait till System clock is ready */ - while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) { - } - - /* Insure 1us transition state at intermediate medium speed clock*/ - for (__IO uint32_t i = (170 >> 1); i != 0; i--) - ; - - /* Set AHB prescaler*/ - LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1); - LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1); - LL_RCC_SetAPB2Prescaler(LL_RCC_APB2_DIV_1); - - LL_Init1msTick(150000000); - - LL_SetSystemCoreClock(150000000); + LL_FLASH_SetLatency(LL_FLASH_LATENCY_4); + while (LL_FLASH_GetLatency() != LL_FLASH_LATENCY_4) { + } + LL_PWR_SetRegulVoltageScaling(LL_PWR_REGU_VOLTAGE_SCALE1); + LL_RCC_HSI_Enable(); + /* Wait till HSI is ready */ + while (LL_RCC_HSI_IsReady() != 1) { + } + + LL_RCC_HSI_SetCalibTrimming(64); + LL_RCC_LSI_Enable(); + /* Wait till LSI is ready */ + while (LL_RCC_LSI_IsReady() != 1) { + } + + LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSI, LL_RCC_PLLM_DIV_4, 75, + LL_RCC_PLLR_DIV_2); + LL_RCC_PLL_EnableDomain_SYS(); + LL_RCC_PLL_Enable(); + /* Wait till PLL is ready */ + while (LL_RCC_PLL_IsReady() != 1) { + } + + LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL); + LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_2); + /* Wait till System clock is ready */ + while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) { + } + + /* Insure 1us transition state at intermediate medium speed clock*/ + for (__IO uint32_t i = (170 >> 1); i != 0; i--) + ; + + /* Set AHB prescaler*/ + LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1); + LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1); + LL_RCC_SetAPB2Prescaler(LL_RCC_APB2_DIV_1); + + LL_Init1msTick(150000000); + + LL_SetSystemCoreClock(150000000); } void MX_COMP1_Init(void) { - LL_COMP_InitTypeDef COMP_InitStruct = { 0 }; - - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - - LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOA); - GPIO_InitStruct.Pin = LL_GPIO_PIN_1; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - GPIO_InitStruct.Pin = LL_GPIO_PIN_4; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - COMP_InitStruct.InputPlus = LL_COMP_INPUT_PLUS_IO1; - COMP_InitStruct.InputMinus = LL_COMP_INPUT_MINUS_IO1; - COMP_InitStruct.InputHysteresis = LL_COMP_HYSTERESIS_NONE; - COMP_InitStruct.OutputPolarity = LL_COMP_OUTPUTPOL_NONINVERTED; - COMP_InitStruct.OutputBlankingSource = LL_COMP_BLANKINGSRC_NONE; - LL_COMP_Init(COMP1, &COMP_InitStruct); - __IO uint32_t wait_loop_index = 0; - wait_loop_index = (LL_COMP_DELAY_VOLTAGE_SCALER_STAB_US * (SystemCoreClock / (1000000 * 2))); - while (wait_loop_index != 0) { - wait_loop_index--; - } - LL_EXTI_DisableEvent_0_31(LL_EXTI_LINE_21); - LL_EXTI_DisableIT_0_31(LL_EXTI_LINE_21); + LL_COMP_InitTypeDef COMP_InitStruct = { 0 }; + + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + + LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOA); + GPIO_InitStruct.Pin = LL_GPIO_PIN_1; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + GPIO_InitStruct.Pin = LL_GPIO_PIN_4; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + COMP_InitStruct.InputPlus = LL_COMP_INPUT_PLUS_IO1; + COMP_InitStruct.InputMinus = LL_COMP_INPUT_MINUS_IO1; + COMP_InitStruct.InputHysteresis = LL_COMP_HYSTERESIS_NONE; + COMP_InitStruct.OutputPolarity = LL_COMP_OUTPUTPOL_NONINVERTED; + COMP_InitStruct.OutputBlankingSource = LL_COMP_BLANKINGSRC_NONE; + LL_COMP_Init(COMP1, &COMP_InitStruct); + __IO uint32_t wait_loop_index = 0; + wait_loop_index = (LL_COMP_DELAY_VOLTAGE_SCALER_STAB_US * (SystemCoreClock / (1000000 * 2))); + while (wait_loop_index != 0) { + wait_loop_index--; + } + LL_EXTI_DisableEvent_0_31(LL_EXTI_LINE_21); + LL_EXTI_DisableIT_0_31(LL_EXTI_LINE_21); } void MX_COMP2_Init(void) { - LL_COMP_InitTypeDef COMP_InitStruct = { 0 }; - - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - - LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOA); - GPIO_InitStruct.Pin = LL_GPIO_PIN_3; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_5; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - COMP_InitStruct.InputPlus = LL_COMP_INPUT_PLUS_IO2; - COMP_InitStruct.InputMinus = LL_COMP_INPUT_MINUS_IO1; - COMP_InitStruct.InputHysteresis = LL_COMP_HYSTERESIS_NONE; - COMP_InitStruct.OutputPolarity = LL_COMP_OUTPUTPOL_NONINVERTED; - COMP_InitStruct.OutputBlankingSource = LL_COMP_BLANKINGSRC_NONE; - LL_COMP_Init(COMP2, &COMP_InitStruct); - __IO uint32_t wait_loop_index = 0; - wait_loop_index = (LL_COMP_DELAY_VOLTAGE_SCALER_STAB_US * (SystemCoreClock / (1000000 * 2))); - while (wait_loop_index != 0) { - wait_loop_index--; - } - LL_EXTI_DisableEvent_0_31(LL_EXTI_LINE_22); - LL_EXTI_DisableIT_0_31(LL_EXTI_LINE_22); - NVIC_SetPriority(COMP1_2_3_IRQn, 0); - NVIC_EnableIRQ(COMP1_2_3_IRQn); + LL_COMP_InitTypeDef COMP_InitStruct = { 0 }; + + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + + LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOA); + GPIO_InitStruct.Pin = LL_GPIO_PIN_3; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_5; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + COMP_InitStruct.InputPlus = LL_COMP_INPUT_PLUS_IO2; + COMP_InitStruct.InputMinus = LL_COMP_INPUT_MINUS_IO1; + COMP_InitStruct.InputHysteresis = LL_COMP_HYSTERESIS_NONE; + COMP_InitStruct.OutputPolarity = LL_COMP_OUTPUTPOL_NONINVERTED; + COMP_InitStruct.OutputBlankingSource = LL_COMP_BLANKINGSRC_NONE; + LL_COMP_Init(COMP2, &COMP_InitStruct); + __IO uint32_t wait_loop_index = 0; + wait_loop_index = (LL_COMP_DELAY_VOLTAGE_SCALER_STAB_US * (SystemCoreClock / (1000000 * 2))); + while (wait_loop_index != 0) { + wait_loop_index--; + } + LL_EXTI_DisableEvent_0_31(LL_EXTI_LINE_22); + LL_EXTI_DisableIT_0_31(LL_EXTI_LINE_22); + NVIC_SetPriority(COMP1_2_3_IRQn, 0); + NVIC_EnableIRQ(COMP1_2_3_IRQn); } /** @@ -163,14 +163,14 @@ void MX_COMP2_Init(void) */ void MX_IWDG_Init(void) { - LL_IWDG_Enable(IWDG); - LL_IWDG_EnableWriteAccess(IWDG); - LL_IWDG_SetPrescaler(IWDG, LL_IWDG_PRESCALER_4); - LL_IWDG_SetReloadCounter(IWDG, 4095); - while (LL_IWDG_IsReady(IWDG) != 1) { - } - LL_IWDG_SetWindow(IWDG, 4095); - LL_IWDG_ReloadCounter(IWDG); + LL_IWDG_Enable(IWDG); + LL_IWDG_EnableWriteAccess(IWDG); + LL_IWDG_SetPrescaler(IWDG, LL_IWDG_PRESCALER_4); + LL_IWDG_SetReloadCounter(IWDG, 4095); + while (LL_IWDG_IsReady(IWDG) != 1) { + } + LL_IWDG_SetWindow(IWDG, 4095); + LL_IWDG_ReloadCounter(IWDG); } /** @@ -180,131 +180,131 @@ void MX_IWDG_Init(void) */ void MX_TIM1_Init(void) { - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - LL_TIM_OC_InitTypeDef TIM_OC_InitStruct = { 0 }; - LL_TIM_BDTR_InitTypeDef TIM_BDTRInitStruct = { 0 }; - - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - - /* Peripheral clock enable */ - LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM1); - TIM_InitStruct.Prescaler = 0; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = TIM1_AUTORELOAD; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - TIM_InitStruct.RepetitionCounter = 0; - LL_TIM_Init(TIM1, &TIM_InitStruct); - LL_TIM_EnableARRPreload(TIM1); - LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH1); - TIM_OC_InitStruct.OCMode = LL_TIM_OCMODE_PWM1; - TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; - TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; - TIM_OC_InitStruct.CompareValue = 0; - TIM_OC_InitStruct.OCPolarity = LL_TIM_OCPOLARITY_HIGH; - TIM_OC_InitStruct.OCNPolarity = LL_TIM_OCPOLARITY_HIGH; - TIM_OC_InitStruct.OCIdleState = LL_TIM_OCIDLESTATE_LOW; - TIM_OC_InitStruct.OCNIdleState = LL_TIM_OCIDLESTATE_LOW; - LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH1, &TIM_OC_InitStruct); - LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH1); - LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH2); - LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH2, &TIM_OC_InitStruct); - LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH2); - LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH3); - LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH3, &TIM_OC_InitStruct); - LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH3); - LL_TIM_SetTriggerOutput(TIM1, LL_TIM_TRGO_RESET); - LL_TIM_SetTriggerOutput2(TIM1, LL_TIM_TRGO2_RESET); - LL_TIM_DisableMasterSlaveMode(TIM1); - TIM_BDTRInitStruct.OSSRState = LL_TIM_OSSR_DISABLE; - TIM_BDTRInitStruct.OSSIState = LL_TIM_OSSI_DISABLE; - TIM_BDTRInitStruct.LockLevel = LL_TIM_LOCKLEVEL_OFF; - TIM_BDTRInitStruct.DeadTime = DEAD_TIME; - TIM_BDTRInitStruct.BreakState = LL_TIM_BREAK_DISABLE; - TIM_BDTRInitStruct.BreakPolarity = LL_TIM_BREAK_POLARITY_HIGH; - TIM_BDTRInitStruct.BreakFilter = LL_TIM_BREAK_FILTER_FDIV1; - TIM_BDTRInitStruct.BreakAFMode = LL_TIM_BREAK_AFMODE_INPUT; - TIM_BDTRInitStruct.Break2State = LL_TIM_BREAK2_DISABLE; - TIM_BDTRInitStruct.Break2Polarity = LL_TIM_BREAK2_POLARITY_HIGH; - TIM_BDTRInitStruct.Break2Filter = LL_TIM_BREAK2_FILTER_FDIV1; - TIM_BDTRInitStruct.Break2AFMode = LL_TIM_BREAK_AFMODE_INPUT; - TIM_BDTRInitStruct.AutomaticOutput = LL_TIM_AUTOMATICOUTPUT_DISABLE; - LL_TIM_BDTR_Init(TIM1, &TIM_BDTRInitStruct); - /* USER CODE BEGIN TIM1_Init 2 */ - - /* USER CODE END TIM1_Init 2 */ - LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOF); - LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOA); - LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOB); - /**TIM1 GPIO Configuration - PF0-OSC_IN ------> TIM1_CH3N - PA7 ------> TIM1_CH1N - PB0 ------> TIM1_CH2N - PA8 ------> TIM1_CH1 - PA9 ------> TIM1_CH2 - PA10 ------> TIM1_CH3 - */ - GPIO_InitStruct.Pin = PHASE_A_GPIO_LOW; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_6; - LL_GPIO_Init(PHASE_A_GPIO_PORT_LOW, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_7; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_6; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_0; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_6; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_8; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_6; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_9; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_6; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_10; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_6; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + LL_TIM_OC_InitTypeDef TIM_OC_InitStruct = { 0 }; + LL_TIM_BDTR_InitTypeDef TIM_BDTRInitStruct = { 0 }; + + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + + /* Peripheral clock enable */ + LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM1); + TIM_InitStruct.Prescaler = 0; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = TIM1_AUTORELOAD; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + TIM_InitStruct.RepetitionCounter = 0; + LL_TIM_Init(TIM1, &TIM_InitStruct); + LL_TIM_EnableARRPreload(TIM1); + LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH1); + TIM_OC_InitStruct.OCMode = LL_TIM_OCMODE_PWM1; + TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE; + TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE; + TIM_OC_InitStruct.CompareValue = 0; + TIM_OC_InitStruct.OCPolarity = LL_TIM_OCPOLARITY_HIGH; + TIM_OC_InitStruct.OCNPolarity = LL_TIM_OCPOLARITY_HIGH; + TIM_OC_InitStruct.OCIdleState = LL_TIM_OCIDLESTATE_LOW; + TIM_OC_InitStruct.OCNIdleState = LL_TIM_OCIDLESTATE_LOW; + LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH1, &TIM_OC_InitStruct); + LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH1); + LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH2); + LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH2, &TIM_OC_InitStruct); + LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH2); + LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH3); + LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH3, &TIM_OC_InitStruct); + LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH3); + LL_TIM_SetTriggerOutput(TIM1, LL_TIM_TRGO_RESET); + LL_TIM_SetTriggerOutput2(TIM1, LL_TIM_TRGO2_RESET); + LL_TIM_DisableMasterSlaveMode(TIM1); + TIM_BDTRInitStruct.OSSRState = LL_TIM_OSSR_DISABLE; + TIM_BDTRInitStruct.OSSIState = LL_TIM_OSSI_DISABLE; + TIM_BDTRInitStruct.LockLevel = LL_TIM_LOCKLEVEL_OFF; + TIM_BDTRInitStruct.DeadTime = DEAD_TIME; + TIM_BDTRInitStruct.BreakState = LL_TIM_BREAK_DISABLE; + TIM_BDTRInitStruct.BreakPolarity = LL_TIM_BREAK_POLARITY_HIGH; + TIM_BDTRInitStruct.BreakFilter = LL_TIM_BREAK_FILTER_FDIV1; + TIM_BDTRInitStruct.BreakAFMode = LL_TIM_BREAK_AFMODE_INPUT; + TIM_BDTRInitStruct.Break2State = LL_TIM_BREAK2_DISABLE; + TIM_BDTRInitStruct.Break2Polarity = LL_TIM_BREAK2_POLARITY_HIGH; + TIM_BDTRInitStruct.Break2Filter = LL_TIM_BREAK2_FILTER_FDIV1; + TIM_BDTRInitStruct.Break2AFMode = LL_TIM_BREAK_AFMODE_INPUT; + TIM_BDTRInitStruct.AutomaticOutput = LL_TIM_AUTOMATICOUTPUT_DISABLE; + LL_TIM_BDTR_Init(TIM1, &TIM_BDTRInitStruct); + /* USER CODE BEGIN TIM1_Init 2 */ + + /* USER CODE END TIM1_Init 2 */ + LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOF); + LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOA); + LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOB); + /**TIM1 GPIO Configuration + PF0-OSC_IN ------> TIM1_CH3N + PA7 ------> TIM1_CH1N + PB0 ------> TIM1_CH2N + PA8 ------> TIM1_CH1 + PA9 ------> TIM1_CH2 + PA10 ------> TIM1_CH3 + */ + GPIO_InitStruct.Pin = PHASE_A_GPIO_LOW; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_6; + LL_GPIO_Init(PHASE_A_GPIO_PORT_LOW, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_7; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_6; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_0; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_6; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_8; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_6; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_9; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_6; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_10; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_6; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); } void MX_TIM2_Init(void) { - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2); - TIM_InitStruct.Prescaler = 74; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 65535; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - LL_TIM_Init(TIM2, &TIM_InitStruct); - LL_TIM_DisableARRPreload(TIM2); - LL_TIM_SetClockSource(TIM2, LL_TIM_CLOCKSOURCE_INTERNAL); - LL_TIM_SetTriggerOutput(TIM2, LL_TIM_TRGO_RESET); - LL_TIM_DisableMasterSlaveMode(TIM2); + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2); + TIM_InitStruct.Prescaler = 74; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 65535; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + LL_TIM_Init(TIM2, &TIM_InitStruct); + LL_TIM_DisableARRPreload(TIM2); + LL_TIM_SetClockSource(TIM2, LL_TIM_CLOCKSOURCE_INTERNAL); + LL_TIM_SetTriggerOutput(TIM2, LL_TIM_TRGO_RESET); + LL_TIM_DisableMasterSlaveMode(TIM2); } /** @@ -314,65 +314,65 @@ void MX_TIM2_Init(void) */ void MX_TIM15_Init(void) { - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - - /* Peripheral clock enable */ - LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM15); - - LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOA); - /**TIM15 GPIO Configuration - PA2 ------> TIM15_CH1 - */ - GPIO_InitStruct.Pin = LL_GPIO_PIN_2; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Alternate = LL_GPIO_AF_9; - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - - LL_DMA_SetPeriphRequest(DMA1, LL_DMA_CHANNEL_1, LL_DMAMUX_REQ_TIM15_CH1); - LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_1, - LL_DMA_DIRECTION_PERIPH_TO_MEMORY); - LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PRIORITY_LOW); - LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MODE_NORMAL); - LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PERIPH_NOINCREMENT); - LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MEMORY_INCREMENT); - LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PDATAALIGN_HALFWORD); - LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MDATAALIGN_WORD); - - TIM_InitStruct.Prescaler = 10; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 65535; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - TIM_InitStruct.RepetitionCounter = 0; - LL_TIM_Init(TIM15, &TIM_InitStruct); - LL_TIM_DisableARRPreload(TIM15); - LL_TIM_SetTriggerOutput(TIM15, LL_TIM_TRGO_RESET); - LL_TIM_DisableMasterSlaveMode(TIM15); - LL_TIM_IC_SetActiveInput(TIM15, LL_TIM_CHANNEL_CH1, - LL_TIM_ACTIVEINPUT_DIRECTTI); - LL_TIM_IC_SetPrescaler(TIM15, LL_TIM_CHANNEL_CH1, LL_TIM_ICPSC_DIV1); - LL_TIM_IC_SetFilter(TIM15, LL_TIM_CHANNEL_CH1, LL_TIM_IC_FILTER_FDIV1); - LL_TIM_IC_SetPolarity(TIM15, LL_TIM_CHANNEL_CH1, LL_TIM_IC_POLARITY_BOTHEDGE); + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + + /* Peripheral clock enable */ + LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM15); + + LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOA); + /**TIM15 GPIO Configuration + PA2 ------> TIM15_CH1 + */ + GPIO_InitStruct.Pin = LL_GPIO_PIN_2; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Alternate = LL_GPIO_AF_9; + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + LL_DMA_SetPeriphRequest(DMA1, LL_DMA_CHANNEL_1, LL_DMAMUX_REQ_TIM15_CH1); + LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_1, + LL_DMA_DIRECTION_PERIPH_TO_MEMORY); + LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PRIORITY_LOW); + LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MODE_NORMAL); + LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PERIPH_NOINCREMENT); + LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MEMORY_INCREMENT); + LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_1, LL_DMA_PDATAALIGN_HALFWORD); + LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_1, LL_DMA_MDATAALIGN_WORD); + + TIM_InitStruct.Prescaler = 10; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 65535; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + TIM_InitStruct.RepetitionCounter = 0; + LL_TIM_Init(TIM15, &TIM_InitStruct); + LL_TIM_DisableARRPreload(TIM15); + LL_TIM_SetTriggerOutput(TIM15, LL_TIM_TRGO_RESET); + LL_TIM_DisableMasterSlaveMode(TIM15); + LL_TIM_IC_SetActiveInput(TIM15, LL_TIM_CHANNEL_CH1, + LL_TIM_ACTIVEINPUT_DIRECTTI); + LL_TIM_IC_SetPrescaler(TIM15, LL_TIM_CHANNEL_CH1, LL_TIM_ICPSC_DIV1); + LL_TIM_IC_SetFilter(TIM15, LL_TIM_CHANNEL_CH1, LL_TIM_IC_FILTER_FDIV1); + LL_TIM_IC_SetPolarity(TIM15, LL_TIM_CHANNEL_CH1, LL_TIM_IC_POLARITY_BOTHEDGE); } void MX_TIM16_Init(void) { - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM16); - NVIC_SetPriority(TIM1_UP_TIM16_IRQn, - NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 0, 0)); - NVIC_EnableIRQ(TIM1_UP_TIM16_IRQn); - TIM_InitStruct.Prescaler = 74; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 65535; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - TIM_InitStruct.RepetitionCounter = 0; - LL_TIM_Init(TIM16, &TIM_InitStruct); - LL_TIM_DisableARRPreload(TIM16); + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM16); + NVIC_SetPriority(TIM1_UP_TIM16_IRQn, + NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 0, 0)); + NVIC_EnableIRQ(TIM1_UP_TIM16_IRQn); + TIM_InitStruct.Prescaler = 74; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 65535; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + TIM_InitStruct.RepetitionCounter = 0; + LL_TIM_Init(TIM16, &TIM_InitStruct); + LL_TIM_DisableARRPreload(TIM16); } /** @@ -382,15 +382,15 @@ void MX_TIM16_Init(void) */ void MX_TIM17_Init(void) { - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM17); - TIM_InitStruct.Prescaler = 159; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 65535; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - TIM_InitStruct.RepetitionCounter = 0; - LL_TIM_Init(TIM17, &TIM_InitStruct); - LL_TIM_EnableARRPreload(TIM17); + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM17); + TIM_InitStruct.Prescaler = 159; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 65535; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + TIM_InitStruct.RepetitionCounter = 0; + LL_TIM_Init(TIM17, &TIM_InitStruct); + LL_TIM_EnableARRPreload(TIM17); } /** @@ -399,100 +399,112 @@ void MX_TIM17_Init(void) void MX_DMA_Init(void) { - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_DMAMUX1); - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_DMA1); + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_DMAMUX1); + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_DMA1); - NVIC_SetPriority(DMA1_Channel1_IRQn, 1); - NVIC_EnableIRQ(DMA1_Channel1_IRQn); + NVIC_SetPriority(DMA1_Channel1_IRQn, 1); + NVIC_EnableIRQ(DMA1_Channel1_IRQn); } void MX_TIM6_Init(void) { - LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM6); - NVIC_SetPriority(TIM6_DAC_IRQn, - NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 0, 0)); - NVIC_EnableIRQ(TIM6_DAC_IRQn); - TIM_InitStruct.Prescaler = 149; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 1000000 / LOOP_FREQUENCY_HZ; - LL_TIM_Init(TIM6, &TIM_InitStruct); - LL_TIM_DisableARRPreload(TIM6); - LL_TIM_SetTriggerOutput(TIM6, LL_TIM_TRGO_RESET); - LL_TIM_DisableMasterSlaveMode(TIM6); + LL_TIM_InitTypeDef TIM_InitStruct = { 0 }; + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM6); + NVIC_SetPriority(TIM6_DAC_IRQn, + NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 0, 0)); + NVIC_EnableIRQ(TIM6_DAC_IRQn); + TIM_InitStruct.Prescaler = 149; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 1000000 / LOOP_FREQUENCY_HZ; + LL_TIM_Init(TIM6, &TIM_InitStruct); + LL_TIM_DisableARRPreload(TIM6); + LL_TIM_SetTriggerOutput(TIM6, LL_TIM_TRGO_RESET); + LL_TIM_DisableMasterSlaveMode(TIM6); } void MX_GPIO_Init(void) { - /* GPIO Ports Clock Enable */ - LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOF); - LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOA); - LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOB); + /* GPIO Ports Clock Enable */ + LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOF); + LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOA); + LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOB); } -void setPWMCompare1(uint16_t compareone) { TIM1->CCR1 = compareone; } -void setPWMCompare2(uint16_t comparetwo) { TIM1->CCR2 = comparetwo; } -void setPWMCompare3(uint16_t comparethree) { TIM1->CCR3 = comparethree; } +void setPWMCompare1(uint16_t compareone) +{ + TIM1->CCR1 = compareone; +} +void setPWMCompare2(uint16_t comparetwo) +{ + TIM1->CCR2 = comparetwo; +} +void setPWMCompare3(uint16_t comparethree) +{ + TIM1->CCR3 = comparethree; +} -void generatePwmTimerEvent() { LL_TIM_GenerateEvent_UPDATE(TIM1); } +void generatePwmTimerEvent() +{ + LL_TIM_GenerateEvent_UPDATE(TIM1); +} void resetInputCaptureTimer() { - IC_TIMER_REGISTER->PSC = 0; - IC_TIMER_REGISTER->CNT = 0; + IC_TIMER_REGISTER->PSC = 0; + IC_TIMER_REGISTER->CNT = 0; } void enableCorePeripherals() { - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH1); - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH2); - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH3); - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH1N); - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH2N); - LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH3N); - - /* Enable counter */ - LL_TIM_EnableCounter(TIM1); - LL_TIM_EnableAllOutputs(TIM1); - /* Force update generation */ - LL_TIM_GenerateEvent_UPDATE(TIM1); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH1); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH2); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH3); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH1N); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH2N); + LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH3N); + + /* Enable counter */ + LL_TIM_EnableCounter(TIM1); + LL_TIM_EnableAllOutputs(TIM1); + /* Force update generation */ + LL_TIM_GenerateEvent_UPDATE(TIM1); #ifdef USE_ADC_INPUT #else - LL_TIM_CC_EnableChannel(IC_TIMER_REGISTER, - IC_TIMER_CHANNEL); // input capture and output compare - LL_TIM_EnableCounter(IC_TIMER_REGISTER); + LL_TIM_CC_EnableChannel(IC_TIMER_REGISTER, + IC_TIMER_CHANNEL); // input capture and output compare + LL_TIM_EnableCounter(IC_TIMER_REGISTER); #endif #ifdef USE_RGB_LED - LED_GPIO_init(); - GPIOB->BRR = LL_GPIO_PIN_8; // turn on red - GPIOB->BSRR = LL_GPIO_PIN_5; - GPIOB->BSRR = LL_GPIO_PIN_3; // + LED_GPIO_init(); + GPIOB->BRR = LL_GPIO_PIN_8; // turn on red + GPIOB->BSRR = LL_GPIO_PIN_5; + GPIOB->BSRR = LL_GPIO_PIN_3; // #endif #ifndef BRUSHED_MODE - LL_TIM_EnableCounter(COM_TIMER); // commutation_timer priority 0 - LL_TIM_GenerateEvent_UPDATE(COM_TIMER); - LL_TIM_EnableIT_UPDATE(COM_TIMER); - COM_TIMER->DIER &= ~((0x1UL << (0U))); // disable for now. + LL_TIM_EnableCounter(COM_TIMER); // commutation_timer priority 0 + LL_TIM_GenerateEvent_UPDATE(COM_TIMER); + LL_TIM_EnableIT_UPDATE(COM_TIMER); + COM_TIMER->DIER &= ~((0x1UL << (0U))); // disable for now. #endif - LL_TIM_EnableCounter(UTILITY_TIMER); - LL_TIM_GenerateEvent_UPDATE(UTILITY_TIMER); - // - LL_TIM_EnableCounter(INTERVAL_TIMER); - LL_TIM_GenerateEvent_UPDATE(INTERVAL_TIMER); - - LL_TIM_EnableCounter(TEN_KHZ_TIMER); // 20khz timer - LL_TIM_GenerateEvent_UPDATE(TEN_KHZ_TIMER); - TEN_KHZ_TIMER->DIER |= (0x1UL << (0U)); // enable interrupt + LL_TIM_EnableCounter(UTILITY_TIMER); + LL_TIM_GenerateEvent_UPDATE(UTILITY_TIMER); + // + LL_TIM_EnableCounter(INTERVAL_TIMER); + LL_TIM_GenerateEvent_UPDATE(INTERVAL_TIMER); + + LL_TIM_EnableCounter(TEN_KHZ_TIMER); // 20khz timer + LL_TIM_GenerateEvent_UPDATE(TEN_KHZ_TIMER); + TEN_KHZ_TIMER->DIER |= (0x1UL << (0U)); // enable interrupt #ifdef USE_ADC - ADC_Init(); - enableADC_DMA(); - activateADC(); + ADC_Init(); + enableADC_DMA(); + activateADC(); #endif - LL_COMP_Enable(COMP2); - LL_COMP_Enable(COMP1); - NVIC_SetPriority(EXTI15_10_IRQn, 2); - NVIC_EnableIRQ(EXTI15_10_IRQn); - EXTI->IMR1 |= (1 << 15); + LL_COMP_Enable(COMP2); + LL_COMP_Enable(COMP1); + NVIC_SetPriority(EXTI15_10_IRQn, 2); + NVIC_EnableIRQ(EXTI15_10_IRQn); + EXTI->IMR1 |= (1 << 15); } diff --git a/Mcu/g431/Src/phaseouts.c b/Mcu/g431/Src/phaseouts.c index 3b93047b..0ab430b7 100644 --- a/Mcu/g431/Src/phaseouts.c +++ b/Mcu/g431/Src/phaseouts.c @@ -31,66 +31,67 @@ extern char prop_brake_active; #endif void proportionalBrake() -{ // alternate all channels between braking (ABC LOW) - // and coasting (ABC float) put lower channel into - // alternate mode and turn upper OFF for each - // channel - // turn all HIGH channels off for ABC - - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; - - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; - - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; - - // set low channel to PWM, duty cycle will now control braking - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); +{ + // alternate all channels between braking (ABC LOW) + // and coasting (ABC float) put lower channel into + // alternate mode and turn upper OFF for each + // channel + // turn all HIGH channels off for ABC + + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; + + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; + + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; + + // set low channel to PWM, duty cycle will now control braking + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); } void phaseBPWM() { - if (!eepromBuffer.comp_pwm) { // for future - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); // low - } - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, - LL_GPIO_MODE_ALTERNATE); // high + if (!eepromBuffer.comp_pwm) { // for future + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); // low + } + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, + LL_GPIO_MODE_ALTERNATE); // high } void phaseBFLOAT() { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; } void phaseBLOW() { - // low mosfet on - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_B_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; + // low mosfet on + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, PHASE_B_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_B_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_HIGH, PHASE_B_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_B_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_B_GPIO_HIGH; } //////////////////////////////PHASE @@ -98,37 +99,37 @@ void phaseBLOW() void phaseCPWM() { - if (!eepromBuffer.comp_pwm) { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); - } - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); + } + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, + LL_GPIO_MODE_ALTERNATE); } void phaseCFLOAT() { - // floating - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; + // floating + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_C_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; } void phaseCLOW() { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_C_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_C_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_HIGH, PHASE_C_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_C_GPIO_HIGH; } ///////////////////////////////////////////////PHASE 3 @@ -136,36 +137,36 @@ void phaseCLOW() void phaseAPWM() { - if (!eepromBuffer.comp_pwm) { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - LL_GPIO_MODE_ALTERNATE); - } - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + LL_GPIO_MODE_ALTERNATE); + } + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, + LL_GPIO_MODE_ALTERNATE); } void phaseAFLOAT() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_A_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; } void phaseALOW() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_A_GPIO_LOW; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_LOW->LOW_BITREG_ON = PHASE_A_GPIO_LOW; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_HIGH, PHASE_A_GPIO_HIGH, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_HIGH->HIGH_BITREG_OFF = PHASE_A_GPIO_HIGH; } #else @@ -173,38 +174,38 @@ void phaseALOW() //////////////////////////////////PHASE 1////////////////////// void phaseBPWM() { - if (!eepromBuffer.comp_pwm) { // for future - // LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, - // PHASE_B_GPIO_LOW, LL_GPIO_MODE_OUTPUT); - // PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); // high pwm + if (!eepromBuffer.comp_pwm) { // for future + // LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_LOW, + // PHASE_B_GPIO_LOW, LL_GPIO_MODE_OUTPUT); + // PHASE_B_GPIO_PORT_LOW->LOW_BITREG_OFF = PHASE_B_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); // high pwm } void phaseBFLOAT() { - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable off - PHASE_B_GPIO_PORT_ENABLE->BRR = PHASE_B_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); // pwm off - PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable off + PHASE_B_GPIO_PORT_ENABLE->BRR = PHASE_B_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); // pwm off + PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; } void phaseBLOW() { - // low mosfet on - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); // pwm off - PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; + // low mosfet on + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_ENABLE, PHASE_B_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_B_GPIO_PORT_ENABLE->BSRR = PHASE_B_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_B_GPIO_PORT_PWM, PHASE_B_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); // pwm off + PHASE_B_GPIO_PORT_PWM->BRR = PHASE_B_GPIO_PWM; } //////////////////////////////PHASE @@ -212,38 +213,38 @@ void phaseBLOW() void phaseCPWM() { - if (!eepromBuffer.comp_pwm) { - // LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, - // LL_GPIO_MODE_OUTPUT); PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = - // PHASE_C_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + // LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_LOW, PHASE_C_GPIO_LOW, + // LL_GPIO_MODE_OUTPUT); PHASE_C_GPIO_PORT_LOW->LOW_BITREG_OFF = + // PHASE_C_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); } void phaseCFLOAT() { - // floating - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable off - PHASE_C_GPIO_PORT_ENABLE->BRR = PHASE_C_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; + // floating + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable off + PHASE_C_GPIO_PORT_ENABLE->BRR = PHASE_C_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; } void phaseCLOW() { - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_ENABLE, PHASE_C_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_C_GPIO_PORT_ENABLE->BSRR = PHASE_C_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_C_GPIO_PORT_PWM, PHASE_C_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_C_GPIO_PORT_PWM->BRR = PHASE_C_GPIO_PWM; } ///////////////////////////////////////////////PHASE 3 @@ -251,116 +252,118 @@ void phaseCLOW() void phaseAPWM() { - if (!eepromBuffer.comp_pwm) { - // LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, - // LL_GPIO_MODE_OUTPUT); PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = - // PHASE_A_GPIO_LOW; - } else { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; - } - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_ALTERNATE); + if (!eepromBuffer.comp_pwm) { + // LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_LOW, PHASE_A_GPIO_LOW, + // LL_GPIO_MODE_OUTPUT); PHASE_A_GPIO_PORT_LOW->LOW_BITREG_OFF = + // PHASE_A_GPIO_LOW; + } else { + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; + } + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_ALTERNATE); } void phaseAFLOAT() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BRR = PHASE_A_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BRR = PHASE_A_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; } void phaseALOW() { - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, - LL_GPIO_MODE_OUTPUT); // enable on - PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; - LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, - LL_GPIO_MODE_OUTPUT); - PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_ENABLE, PHASE_A_GPIO_ENABLE, + LL_GPIO_MODE_OUTPUT); // enable on + PHASE_A_GPIO_PORT_ENABLE->BSRR = PHASE_A_GPIO_ENABLE; + LL_GPIO_SetPinMode(PHASE_A_GPIO_PORT_PWM, PHASE_A_GPIO_PWM, + LL_GPIO_MODE_OUTPUT); + PHASE_A_GPIO_PORT_PWM->BRR = PHASE_A_GPIO_PWM; } #endif void allOff() { - phaseAFLOAT(); - phaseBFLOAT(); - phaseCFLOAT(); + phaseAFLOAT(); + phaseBFLOAT(); + phaseCFLOAT(); } void comStep(int newStep) { - // TIM14->CNT = 0; - switch (newStep) { - case 1: // A-B - phaseAPWM(); - phaseBLOW(); - phaseCFLOAT(); - break; - - case 2: // C-B - phaseAFLOAT(); - phaseBLOW(); - phaseCPWM(); - break; - - case 3: // C-A - phaseALOW(); - phaseBFLOAT(); - phaseCPWM(); - break; - - case 4: // B-A - phaseALOW(); - phaseBPWM(); - phaseCFLOAT(); - break; - - case 5: // B-C - phaseAFLOAT(); - phaseBPWM(); - phaseCLOW(); - break; - - case 6: // A-C - phaseAPWM(); - phaseBFLOAT(); - phaseCLOW(); - break; - } - - // stop_time = TIM14->CNT; -} + // TIM14->CNT = 0; + switch (newStep) { + case 1: // A-B + phaseAPWM(); + phaseBLOW(); + phaseCFLOAT(); + break; -void fullBrake() -{ // full braking shorting all low sides - phaseALOW(); + case 2: // C-B + phaseAFLOAT(); phaseBLOW(); + phaseCPWM(); + break; + + case 3: // C-A + phaseALOW(); + phaseBFLOAT(); + phaseCPWM(); + break; + + case 4: // B-A + phaseALOW(); + phaseBPWM(); + phaseCFLOAT(); + break; + + case 5: // B-C + phaseAFLOAT(); + phaseBPWM(); + phaseCLOW(); + break; + + case 6: // A-C + phaseAPWM(); + phaseBFLOAT(); phaseCLOW(); + break; + } + + // stop_time = TIM14->CNT; +} + +void fullBrake() +{ + // full braking shorting all low sides + phaseALOW(); + phaseBLOW(); + phaseCLOW(); } void allpwm() -{ // for stepper_sine - phaseAPWM(); - phaseBPWM(); - phaseCPWM(); +{ + // for stepper_sine + phaseAPWM(); + phaseBPWM(); + phaseCPWM(); } void twoChannelForward() { - phaseAPWM(); - phaseBLOW(); - phaseCPWM(); + phaseAPWM(); + phaseBLOW(); + phaseCPWM(); } void twoChannelReverse() { - phaseALOW(); - phaseBPWM(); - phaseCLOW(); + phaseALOW(); + phaseBPWM(); + phaseCLOW(); } diff --git a/Mcu/g431/Src/serial_telemetry.c b/Mcu/g431/Src/serial_telemetry.c index 721f9bcd..778bcca2 100644 --- a/Mcu/g431/Src/serial_telemetry.c +++ b/Mcu/g431/Src/serial_telemetry.c @@ -5,126 +5,129 @@ * Author: Alka */ - #include "serial_telemetry.h" +#include "serial_telemetry.h" - uint8_t aTxBuffer[10]; - uint8_t nbDataToTransmit = sizeof(aTxBuffer); +uint8_t aTxBuffer[10]; +uint8_t nbDataToTransmit = sizeof(aTxBuffer); - void telem_UART_Init() +void telem_UART_Init() { - LL_USART_InitTypeDef USART_InitStruct = { 0 }; - - LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; - LL_RCC_SetUSARTClockSource(LL_RCC_USART1_CLKSOURCE_PCLK2); - LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_USART1); - LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOB); - - GPIO_InitStruct.Pin = LL_GPIO_PIN_6; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_OPENDRAIN; - GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; - GPIO_InitStruct.Alternate = LL_GPIO_AF_0; - LL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - NVIC_SetPriority(USART1_IRQn, 3); - NVIC_EnableIRQ(USART1_IRQn); - - LL_DMA_SetPeriphRequest(DMA1, LL_DMA_CHANNEL_3, LL_DMAMUX_REQ_USART1_TX); - - LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_3, - LL_DMA_DIRECTION_MEMORY_TO_PERIPH); - - LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_3, - LL_DMA_PRIORITY_LOW); - - LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_3, LL_DMA_MODE_NORMAL); - - LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_3, - LL_DMA_PERIPH_NOINCREMENT); - - LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_3, LL_DMA_MEMORY_INCREMENT); - - LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_3, LL_DMA_PDATAALIGN_BYTE); - - LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_3, LL_DMA_MDATAALIGN_BYTE); - - USART_InitStruct.PrescalerValue = LL_USART_PRESCALER_DIV1; - USART_InitStruct.BaudRate = 115200; - USART_InitStruct.DataWidth = LL_USART_DATAWIDTH_8B; - USART_InitStruct.StopBits = LL_USART_STOPBITS_1; - USART_InitStruct.Parity = LL_USART_PARITY_NONE; - USART_InitStruct.TransferDirection = LL_USART_DIRECTION_TX_RX; - USART_InitStruct.OverSampling = LL_USART_OVERSAMPLING_16; - LL_USART_Init(USART1, &USART_InitStruct); - LL_USART_SetTXFIFOThreshold(USART1, LL_USART_FIFOTHRESHOLD_1_8); - LL_USART_SetRXFIFOThreshold(USART1, LL_USART_FIFOTHRESHOLD_1_8); - LL_USART_DisableFIFO(USART1); - LL_USART_ConfigHalfDuplexMode(USART1); - - LL_USART_Enable(USART1); - while ((!(LL_USART_IsActiveFlag_TEACK(USART1))) - || (!(LL_USART_IsActiveFlag_REACK(USART1)))) { - } - - LL_DMA_ConfigAddresses( - DMA1, LL_DMA_CHANNEL_3, (uint32_t)aTxBuffer, - LL_USART_DMA_GetRegAddr(USART1, LL_USART_DMA_REG_DATA_TRANSMIT), - LL_DMA_GetDataTransferDirection(DMA1, LL_DMA_CHANNEL_3)); - LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_3, nbDataToTransmit); - - /* (5) Enable DMA transfer complete/error interrupts */ - LL_DMA_EnableIT_TC(DMA1, LL_DMA_CHANNEL_3); - LL_DMA_EnableIT_TE(DMA1, LL_DMA_CHANNEL_3); + LL_USART_InitTypeDef USART_InitStruct = { 0 }; + + LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 }; + LL_RCC_SetUSARTClockSource(LL_RCC_USART1_CLKSOURCE_PCLK2); + LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_USART1); + LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOB); + + GPIO_InitStruct.Pin = LL_GPIO_PIN_6; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_OPENDRAIN; + GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; + GPIO_InitStruct.Alternate = LL_GPIO_AF_0; + LL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + NVIC_SetPriority(USART1_IRQn, 3); + NVIC_EnableIRQ(USART1_IRQn); + + LL_DMA_SetPeriphRequest(DMA1, LL_DMA_CHANNEL_3, LL_DMAMUX_REQ_USART1_TX); + + LL_DMA_SetDataTransferDirection(DMA1, LL_DMA_CHANNEL_3, + LL_DMA_DIRECTION_MEMORY_TO_PERIPH); + + LL_DMA_SetChannelPriorityLevel(DMA1, LL_DMA_CHANNEL_3, + LL_DMA_PRIORITY_LOW); + + LL_DMA_SetMode(DMA1, LL_DMA_CHANNEL_3, LL_DMA_MODE_NORMAL); + + LL_DMA_SetPeriphIncMode(DMA1, LL_DMA_CHANNEL_3, + LL_DMA_PERIPH_NOINCREMENT); + + LL_DMA_SetMemoryIncMode(DMA1, LL_DMA_CHANNEL_3, LL_DMA_MEMORY_INCREMENT); + + LL_DMA_SetPeriphSize(DMA1, LL_DMA_CHANNEL_3, LL_DMA_PDATAALIGN_BYTE); + + LL_DMA_SetMemorySize(DMA1, LL_DMA_CHANNEL_3, LL_DMA_MDATAALIGN_BYTE); + + USART_InitStruct.PrescalerValue = LL_USART_PRESCALER_DIV1; + USART_InitStruct.BaudRate = 115200; + USART_InitStruct.DataWidth = LL_USART_DATAWIDTH_8B; + USART_InitStruct.StopBits = LL_USART_STOPBITS_1; + USART_InitStruct.Parity = LL_USART_PARITY_NONE; + USART_InitStruct.TransferDirection = LL_USART_DIRECTION_TX_RX; + USART_InitStruct.OverSampling = LL_USART_OVERSAMPLING_16; + LL_USART_Init(USART1, &USART_InitStruct); + LL_USART_SetTXFIFOThreshold(USART1, LL_USART_FIFOTHRESHOLD_1_8); + LL_USART_SetRXFIFOThreshold(USART1, LL_USART_FIFOTHRESHOLD_1_8); + LL_USART_DisableFIFO(USART1); + LL_USART_ConfigHalfDuplexMode(USART1); + + LL_USART_Enable(USART1); + while ((!(LL_USART_IsActiveFlag_TEACK(USART1))) + || (!(LL_USART_IsActiveFlag_REACK(USART1)))) { + } + + LL_DMA_ConfigAddresses( + DMA1, LL_DMA_CHANNEL_3, (uint32_t)aTxBuffer, + LL_USART_DMA_GetRegAddr(USART1, LL_USART_DMA_REG_DATA_TRANSMIT), + LL_DMA_GetDataTransferDirection(DMA1, LL_DMA_CHANNEL_3)); + LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_3, nbDataToTransmit); + + /* (5) Enable DMA transfer complete/error interrupts */ + LL_DMA_EnableIT_TC(DMA1, LL_DMA_CHANNEL_3); + LL_DMA_EnableIT_TE(DMA1, LL_DMA_CHANNEL_3); } - void send_telem_DMA() -{ // set data length and enable channel to start transfer - LL_USART_SetTransferDirection(USART1, LL_USART_DIRECTION_TX); - // GPIOB->OTYPER &= 0 << 6; - LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_3, nbDataToTransmit); - LL_USART_EnableDMAReq_TX(USART1); - - LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_3); - LL_USART_SetTransferDirection(USART1, LL_USART_DIRECTION_RX); +void send_telem_DMA() +{ + // set data length and enable channel to start transfer + LL_USART_SetTransferDirection(USART1, LL_USART_DIRECTION_TX); + // GPIOB->OTYPER &= 0 << 6; + LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_3, nbDataToTransmit); + LL_USART_EnableDMAReq_TX(USART1); + + LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_3); + LL_USART_SetTransferDirection(USART1, LL_USART_DIRECTION_RX); } - uint8_t - update_crc8(uint8_t crc, uint8_t crc_seed) +uint8_t +update_crc8(uint8_t crc, uint8_t crc_seed) { - uint8_t crc_u, i; - crc_u = crc; - crc_u ^= crc_seed; - for (i = 0; i < 8; i++) - crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); - return (crc_u); - } - - uint8_t - get_crc8(uint8_t* Buf, uint8_t BufLen) + uint8_t crc_u, i; + crc_u = crc; + crc_u ^= crc_seed; + for (i = 0; i < 8; i++) { + crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); + } + return (crc_u); +} + +uint8_t +get_crc8(uint8_t* Buf, uint8_t BufLen) { - uint8_t crc = 0, i; - for (i = 0; i < BufLen; i++) - crc = update_crc8(Buf[i], crc); - return (crc); - } - - void makeTelemPackage(uint8_t temp, uint16_t voltage, uint16_t current, - uint16_t consumption, uint16_t e_rpm) + uint8_t crc = 0, i; + for (i = 0; i < BufLen; i++) { + crc = update_crc8(Buf[i], crc); + } + return (crc); +} + +void makeTelemPackage(uint8_t temp, uint16_t voltage, uint16_t current, + uint16_t consumption, uint16_t e_rpm) { - aTxBuffer[0] = temp; // temperature + aTxBuffer[0] = temp; // temperature - aTxBuffer[1] = (voltage >> 8) & 0xFF; // voltage hB - aTxBuffer[2] = voltage & 0xFF; // voltage lowB + aTxBuffer[1] = (voltage >> 8) & 0xFF; // voltage hB + aTxBuffer[2] = voltage & 0xFF; // voltage lowB - aTxBuffer[3] = (current >> 8) & 0xFF; // current - aTxBuffer[4] = current & 0xFF; // divide by 10 for Amps + aTxBuffer[3] = (current >> 8) & 0xFF; // current + aTxBuffer[4] = current & 0xFF; // divide by 10 for Amps - aTxBuffer[5] = (consumption >> 8) & 0xFF; // consumption - aTxBuffer[6] = consumption & 0xFF; // in mah + aTxBuffer[5] = (consumption >> 8) & 0xFF; // consumption + aTxBuffer[6] = consumption & 0xFF; // in mah - aTxBuffer[7] = (e_rpm >> 8) & 0xFF; // - aTxBuffer[8] = e_rpm & 0xFF; // eRpM *100 + aTxBuffer[7] = (e_rpm >> 8) & 0xFF; // + aTxBuffer[8] = e_rpm & 0xFF; // eRpM *100 - aTxBuffer[9] = get_crc8(aTxBuffer, 9); + aTxBuffer[9] = get_crc8(aTxBuffer, 9); } diff --git a/Mcu/g431/Src/stm32g4xx_it.c b/Mcu/g431/Src/stm32g4xx_it.c index f735e287..d2396a67 100644 --- a/Mcu/g431/Src/stm32g4xx_it.c +++ b/Mcu/g431/Src/stm32g4xx_it.c @@ -25,32 +25,32 @@ int interrupt = 0; void NMI_Handler(void) { - while (1) { - } + while (1) { + } } void HardFault_Handler(void) { - while (1) { - } + while (1) { + } } void MemManage_Handler(void) { - while (1) { - } + while (1) { + } } void BusFault_Handler(void) { - while (1) { - } + while (1) { + } } void UsageFault_Handler(void) { - while (1) { - } + while (1) { + } } void SVC_Handler(void) { } @@ -63,90 +63,90 @@ void SysTick_Handler(void) { } void DMA1_Channel1_IRQHandler(void) { - if (armed && dshot_telemetry) { - DMA1->IFCR |= DMA_IFCR_CGIF1; - DMA1_Channel1->CCR = 0x00; - if (out_put) { - receiveDshotDma(); - compute_dshot_flag = 2; - } else { - sendDshotDma(); - compute_dshot_flag = 1; - } - EXTI->SWIER1 |= LL_EXTI_LINE_15; - - return; - } - /* USER CODE BEGIN DMA1_Channel1_IRQn 0 */ - if (LL_DMA_IsActiveFlag_HT1(DMA1)) { - if (servoPwm) { - LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, - LL_TIM_IC_POLARITY_FALLING); - LL_DMA_ClearFlag_HT1(DMA1); - } + if (armed && dshot_telemetry) { + DMA1->IFCR |= DMA_IFCR_CGIF1; + DMA1_Channel1->CCR = 0x00; + if (out_put) { + receiveDshotDma(); + compute_dshot_flag = 2; + } else { + sendDshotDma(); + compute_dshot_flag = 1; } - if (LL_DMA_IsActiveFlag_TC1(DMA1) == 1) { - DMA1->IFCR |= DMA_IFCR_CGIF1; - DMA1_Channel1->CCR = 0x00; - transfercomplete(); - EXTI->SWIER1 |= LL_EXTI_LINE_15; - } else if (LL_DMA_IsActiveFlag_TE1(DMA1) == 1) { - LL_DMA_ClearFlag_GI1(DMA1); + EXTI->SWIER1 |= LL_EXTI_LINE_15; + + return; + } + /* USER CODE BEGIN DMA1_Channel1_IRQn 0 */ + if (LL_DMA_IsActiveFlag_HT1(DMA1)) { + if (servoPwm) { + LL_TIM_IC_SetPolarity(IC_TIMER_REGISTER, IC_TIMER_CHANNEL, + LL_TIM_IC_POLARITY_FALLING); + LL_DMA_ClearFlag_HT1(DMA1); } + } + if (LL_DMA_IsActiveFlag_TC1(DMA1) == 1) { + DMA1->IFCR |= DMA_IFCR_CGIF1; + DMA1_Channel1->CCR = 0x00; + transfercomplete(); + EXTI->SWIER1 |= LL_EXTI_LINE_15; + } else if (LL_DMA_IsActiveFlag_TE1(DMA1) == 1) { + LL_DMA_ClearFlag_GI1(DMA1); + } } void COMP1_2_3_IRQHandler(void) { - if(INTERVAL_TIMER->CNT > (commutation_interval>>1)){ + if (INTERVAL_TIMER->CNT > (commutation_interval>>1)) { interrupt++; if (LL_EXTI_IsActiveFlag_0_31(LL_EXTI_LINE_22)) { - LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_22); - interruptRoutine(); - return; + LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_22); + interruptRoutine(); + return; } if (LL_EXTI_IsActiveFlag_0_31(LL_EXTI_LINE_21)) { - LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_21); - interruptRoutine(); - return; + LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_21); + interruptRoutine(); + return; } - }else{ - LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_21); - LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_22); - } + } else { + LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_21); + LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_22); + } } void TIM6_DAC_IRQHandler(void) { - if (LL_TIM_IsActiveFlag_UPDATE(TIM6) == 1) { - LL_TIM_ClearFlag_UPDATE(TIM6); - tenKhzRoutine(); - } + if (LL_TIM_IsActiveFlag_UPDATE(TIM6) == 1) { + LL_TIM_ClearFlag_UPDATE(TIM6); + tenKhzRoutine(); + } } void TIM1_UP_TIM16_IRQHandler(void) { - PeriodElapsedCallback(); - LL_TIM_ClearFlag_UPDATE(TIM16); + PeriodElapsedCallback(); + LL_TIM_ClearFlag_UPDATE(TIM16); } void EXTI15_10_IRQHandler(void) { - LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_15); - processDshot(); + LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_15); + processDshot(); } void DMA1_Channel3_IRQHandler(void) { - if (LL_DMA_IsActiveFlag_TC3(DMA1)) { - send_telemetry = 0; - LL_DMA_ClearFlag_GI3(DMA1); - LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_3); - /* Call function Transmission complete Callback */ - } else if (LL_DMA_IsActiveFlag_TE3(DMA1)) { - LL_DMA_ClearFlag_GI3(DMA1); - LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_3); - /* Call Error function */ - // USART_TransferError_Callback(); - } + if (LL_DMA_IsActiveFlag_TC3(DMA1)) { + send_telemetry = 0; + LL_DMA_ClearFlag_GI3(DMA1); + LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_3); + /* Call function Transmission complete Callback */ + } else if (LL_DMA_IsActiveFlag_TE3(DMA1)) { + LL_DMA_ClearFlag_GI3(DMA1); + LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_3); + /* Call Error function */ + // USART_TransferError_Callback(); + } } diff --git a/Mcu/g431/Src/system_stm32g4xx.c b/Mcu/g431/Src/system_stm32g4xx.c index b4d2dfdd..2f2ea015 100644 --- a/Mcu/g431/Src/system_stm32g4xx.c +++ b/Mcu/g431/Src/system_stm32g4xx.c @@ -156,7 +156,8 @@ uint32_t SystemCoreClock = HSI_VALUE; const uint8_t AHBPrescTable[16] = { 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 1U, 2U, 3U, 4U, 6U, 7U, 8U, 9U }; + 1U, 2U, 3U, 4U, 6U, 7U, 8U, 9U + }; const uint8_t APBPrescTable[8] = { 0U, 0U, 0U, 0U, 1U, 2U, 3U, 4U }; /** @@ -183,14 +184,14 @@ const uint8_t APBPrescTable[8] = { 0U, 0U, 0U, 0U, 1U, 2U, 3U, 4U }; void SystemInit(void) { -/* FPU settings ------------------------------------------------------------*/ + /* FPU settings ------------------------------------------------------------*/ #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) - SCB->CPACR |= ((3UL << (10 * 2)) | (3UL << (11 * 2))); /* set CP10 and CP11 Full Access */ + SCB->CPACR |= ((3UL << (10 * 2)) | (3UL << (11 * 2))); /* set CP10 and CP11 Full Access */ #endif - /* Configure the Vector Table location add offset address ------------------*/ + /* Configure the Vector Table location add offset address ------------------*/ #if defined(USER_VECT_TAB_ADDRESS) - SCB->VTOR = VECT_TAB_BASE_ADDRESS | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */ + SCB->VTOR = VECT_TAB_BASE_ADDRESS | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */ #endif /* USER_VECT_TAB_ADDRESS */ } @@ -234,44 +235,42 @@ void SystemInit(void) */ void SystemCoreClockUpdate(void) { - uint32_t tmp, pllvco, pllr, pllsource, pllm; + uint32_t tmp, pllvco, pllr, pllsource, pllm; - /* Get SYSCLK source -------------------------------------------------------*/ - switch (RCC->CFGR & RCC_CFGR_SWS) { - case 0x04: /* HSI used as system clock source */ - SystemCoreClock = HSI_VALUE; - break; + /* Get SYSCLK source -------------------------------------------------------*/ + switch (RCC->CFGR & RCC_CFGR_SWS) { + case 0x04: /* HSI used as system clock source */ + SystemCoreClock = HSI_VALUE; + break; - case 0x08: /* HSE used as system clock source */ - SystemCoreClock = HSE_VALUE; - break; + case 0x08: /* HSE used as system clock source */ + SystemCoreClock = HSE_VALUE; + break; - case 0x0C: /* PLL used as system clock source */ - /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLLM) * PLLN - SYSCLK = PLL_VCO / PLLR - */ - pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC); - pllm = ((RCC->PLLCFGR & RCC_PLLCFGR_PLLM) >> 4) + 1U; - if (pllsource == 0x02UL) /* HSI used as PLL clock source */ - { - pllvco = (HSI_VALUE / pllm); - } else /* HSE used as PLL clock source */ - { - pllvco = (HSE_VALUE / pllm); - } - pllvco = pllvco * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 8); - pllr = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLR) >> 25) + 1U) * 2U; - SystemCoreClock = pllvco / pllr; - break; - - default: - break; + case 0x0C: /* PLL used as system clock source */ + /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLLM) * PLLN + SYSCLK = PLL_VCO / PLLR + */ + pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC); + pllm = ((RCC->PLLCFGR & RCC_PLLCFGR_PLLM) >> 4) + 1U; + if (pllsource == 0x02UL) { /* HSI used as PLL clock source */ + pllvco = (HSI_VALUE / pllm); + } else { /* HSE used as PLL clock source */ + pllvco = (HSE_VALUE / pllm); } - /* Compute HCLK clock frequency --------------------------------------------*/ - /* Get HCLK prescaler */ - tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; - /* HCLK clock frequency */ - SystemCoreClock >>= tmp; + pllvco = pllvco * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 8); + pllr = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLR) >> 25) + 1U) * 2U; + SystemCoreClock = pllvco / pllr; + break; + + default: + break; + } + /* Compute HCLK clock frequency --------------------------------------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; + /* HCLK clock frequency */ + SystemCoreClock >>= tmp; } /** diff --git a/Mcu/l431/Inc/IO.h b/Mcu/l431/Inc/IO.h index 8b1d7459..9be98616 100644 --- a/Mcu/l431/Inc/IO.h +++ b/Mcu/l431/Inc/IO.h @@ -30,6 +30,6 @@ extern uint8_t degrees_celsius; extern uint16_t ADC_raw_volts; extern uint16_t servo_low_threshold; // anything below this point considered 0 extern uint16_t - servo_high_threshold; // anything above this point considered 2000 (max) +servo_high_threshold; // anything above this point considered 2000 (max) extern uint16_t servo_neutral; extern uint8_t servo_dead_band; diff --git a/Mcu/l431/Inc/serial_telemetry.h b/Mcu/l431/Inc/serial_telemetry.h index b4f1c5cf..fb8cd3b9 100644 --- a/Mcu/l431/Inc/serial_telemetry.h +++ b/Mcu/l431/Inc/serial_telemetry.h @@ -11,7 +11,7 @@ #define SERIAL_TELEMETRY_H_ void makeTelemPackage(uint8_t temp, uint16_t voltage, uint16_t current, - uint16_t consumption, uint16_t e_rpm); + uint16_t consumption, uint16_t e_rpm); void telem_UART_Init(void); void send_telem_DMA(); diff --git a/Mcu/l431/Inc/stm32l4xx_it.h b/Mcu/l431/Inc/stm32l4xx_it.h index e16edb32..361885bf 100644 --- a/Mcu/l431/Inc/stm32l4xx_it.h +++ b/Mcu/l431/Inc/stm32l4xx_it.h @@ -22,7 +22,7 @@ #define __STM32L4xx_IT_H #ifdef __cplusplus - extern "C" { +extern "C" { #endif /* Private includes ----------------------------------------------------------*/ diff --git a/Src/DroneCAN/DroneCAN.c b/Src/DroneCAN/DroneCAN.c index 32440090..0332c543 100644 --- a/Src/DroneCAN/DroneCAN.c +++ b/Src/DroneCAN/DroneCAN.c @@ -53,27 +53,27 @@ static bool done_startup; application signature, filled in by set_app_signature.py */ const struct { - uint32_t magic1; - uint32_t magic2; - uint32_t fwlen; // total fw length in bytes - uint32_t crc1; // crc32 up to start of app_signature - uint32_t crc2; // crc32 from end of app_signature to end of fw - char mcu[16]; - uint32_t unused[2]; + uint32_t magic1; + uint32_t magic2; + uint32_t fwlen; // total fw length in bytes + uint32_t crc1; // crc32 up to start of app_signature + uint32_t crc2; // crc32 from end of app_signature to end of fw + char mcu[16]; + uint32_t unused[2]; } app_signature __attribute__((section(".app_signature"))) = { - .magic1 = APP_SIGNATURE_MAGIC1, - .magic2 = APP_SIGNATURE_MAGIC2, - .fwlen = 0, - .crc1 = 0, - .crc2 = 0, - .mcu = AM32_MCU, + .magic1 = APP_SIGNATURE_MAGIC1, + .magic2 = APP_SIGNATURE_MAGIC2, + .fwlen = 0, + .crc1 = 0, + .crc2 = 0, + .mcu = AM32_MCU, }; enum VarType { - T_BOOL = 0, - T_UINT8, - T_UINT16, - T_STRING, + T_BOOL = 0, + T_UINT8, + T_UINT16, + T_STRING, }; #define PACKED __attribute__((__packed__)) @@ -82,14 +82,14 @@ enum VarType { structure sent with FlexDebug */ static struct PACKED { - uint8_t version; - uint32_t commutation_interval; - uint16_t num_commands; - uint16_t num_input; - uint16_t rx_errors; - uint16_t rxframe_error; - int32_t rx_ecode; - uint8_t auto_advance_level; + uint8_t version; + uint32_t commutation_interval; + uint16_t num_commands; + uint16_t num_input; + uint16_t rx_errors; + uint16_t rxframe_error; + int32_t rx_ecode; + uint8_t auto_advance_level; } debug1; static void can_printf(const char *fmt, ...); @@ -110,8 +110,8 @@ extern uint16_t low_cell_volt_cutoff; static uint16_t last_can_input; static struct { - uint32_t sum; - uint32_t count; + uint32_t sum; + uint32_t count; } current; extern void saveEEpromSettings(void); @@ -122,55 +122,55 @@ static void set_input(uint16_t input); the set of parameters to present to the user over DroneCAN */ static const struct parameter { - char *name; - enum VarType vtype; - uint16_t min_value; - uint16_t max_value; - uint16_t default_value; - void *ptr; + char *name; + enum VarType vtype; + uint16_t min_value; + uint16_t max_value; + uint16_t default_value; + void *ptr; } parameters[] = { - // list of settable parameters - // dronecan specific parameters - { "CAN_NODE", T_UINT8, 0, 127, 0, &eepromBuffer.can.can_node}, - { "ESC_INDEX", T_UINT8, 0, 32, 0, &eepromBuffer.can.esc_index}, - { "TELEM_RATE", T_UINT8, 0, 200, 25, &eepromBuffer.can.telem_rate}, - { "DEBUG_RATE", T_UINT8, 0, 200, 0, &eepromBuffer.can.debug_rate}, - { "REQUIRE_ARMING", T_BOOL, 0, 1, 1, &eepromBuffer.can.require_arming}, - { "REQUIRE_ZERO_THROTTLE", T_BOOL, 0, 1, 1, &eepromBuffer.can.require_zero_throttle}, - { "MOTOR_KV", T_UINT16,20, 10220, 2000, &motor_kv}, - { "MOTOR_POLES", T_UINT8, 2, 64, 14, &eepromBuffer.motor_poles}, - - // motor_kv, low_cell_volt_cutoff, STARTUP_TUNE, CURRENT_LIMIT value need to adjust to dronecan gui tool - // motor_kv 1k/V - // CURRENT_LIMIT A, range 0 ~ 200, 0 to disable - // TEMPERATURE_LIMIT degrees celsius, range 70 ~ 141, 0 to disable - // low_cell_volt_cutoff 10mV, range 250 ~ 350 - // STARTUP_TUNE RTTTL string - - { "DIR_REVERSED", T_BOOL, 0, 1, 0, &eepromBuffer.dir_reversed}, - { "BI_DIRECTIONAL", T_BOOL, 0, 1, 0, &eepromBuffer.bi_direction}, - { "BEEP_VOLUME", T_UINT8, 0, 11, 5, &eepromBuffer.beep_volume}, - { "VARIABLE_PWM", T_BOOL, 0, 1, 1, &eepromBuffer.variable_pwm}, - { "PWM_FREQUENCY", T_UINT8, 8, 48, 24, &eepromBuffer.pwm_frequency}, - { "USE_SIN_START", T_BOOL, 0, 1, 0, &eepromBuffer.use_sine_start}, - { "COMP_PWM", T_BOOL, 0, 1, 1, &eepromBuffer.comp_pwm}, - { "STUCK_ROTOR_PROTECTION", T_BOOL, 0, 1, 1, &eepromBuffer.stuck_rotor_protection}, - { "ADVANCE_LEVEL", T_UINT8, 0, 4, 2, &eepromBuffer.advance_level}, - { "AUTO_ADVANCE", T_BOOL, 0, 1, 0, &eepromBuffer.auto_advance}, - { "STARTUP_POWER", T_UINT8, 50,150, 10, &eepromBuffer.startup_power}, - { "CURRENT_LIMIT", T_UINT8, 0, 200, 0, &eepromBuffer.limits.current}, - { "TEMPERATURE_LIMIT", T_UINT8, 70,255, 255,&eepromBuffer.limits.temperature}, - { "LOW_VOLTAGE_CUTOFF", T_BOOL, 0, 1, 0, &eepromBuffer.low_voltage_cut_off}, - { "CELL_VOLTAGE_THRESHOLD", T_UINT16, 250, 350, 300, &low_cell_volt_cutoff}, - { "BRAKE_ON_STOP", T_BOOL, 0, 1, 1, &eepromBuffer.brake_on_stop}, - { "DRIVING_BRAKE_STRENGTH", T_UINT8, 1, 10, 10, &eepromBuffer.driving_brake_strength}, - { "DRAG_BRAKE_STRENGTH", T_UINT8, 1, 10, 10, &eepromBuffer.drag_brake_strength}, - { "INPUT_SIGNAL_TYPE", T_UINT8, 0, 5, 5, &eepromBuffer.input_type}, - { "INPUT_FILTER_HZ", T_UINT8, 0, 100, 0, &eepromBuffer.can.filter_hz}, + // list of settable parameters + // dronecan specific parameters + { "CAN_NODE", T_UINT8, 0, 127, 0, &eepromBuffer.can.can_node}, + { "ESC_INDEX", T_UINT8, 0, 32, 0, &eepromBuffer.can.esc_index}, + { "TELEM_RATE", T_UINT8, 0, 200, 25, &eepromBuffer.can.telem_rate}, + { "DEBUG_RATE", T_UINT8, 0, 200, 0, &eepromBuffer.can.debug_rate}, + { "REQUIRE_ARMING", T_BOOL, 0, 1, 1, &eepromBuffer.can.require_arming}, + { "REQUIRE_ZERO_THROTTLE", T_BOOL, 0, 1, 1, &eepromBuffer.can.require_zero_throttle}, + { "MOTOR_KV", T_UINT16,20, 10220, 2000, &motor_kv}, + { "MOTOR_POLES", T_UINT8, 2, 64, 14, &eepromBuffer.motor_poles}, + + // motor_kv, low_cell_volt_cutoff, STARTUP_TUNE, CURRENT_LIMIT value need to adjust to dronecan gui tool + // motor_kv 1k/V + // CURRENT_LIMIT A, range 0 ~ 200, 0 to disable + // TEMPERATURE_LIMIT degrees celsius, range 70 ~ 141, 0 to disable + // low_cell_volt_cutoff 10mV, range 250 ~ 350 + // STARTUP_TUNE RTTTL string + + { "DIR_REVERSED", T_BOOL, 0, 1, 0, &eepromBuffer.dir_reversed}, + { "BI_DIRECTIONAL", T_BOOL, 0, 1, 0, &eepromBuffer.bi_direction}, + { "BEEP_VOLUME", T_UINT8, 0, 11, 5, &eepromBuffer.beep_volume}, + { "VARIABLE_PWM", T_BOOL, 0, 1, 1, &eepromBuffer.variable_pwm}, + { "PWM_FREQUENCY", T_UINT8, 8, 48, 24, &eepromBuffer.pwm_frequency}, + { "USE_SIN_START", T_BOOL, 0, 1, 0, &eepromBuffer.use_sine_start}, + { "COMP_PWM", T_BOOL, 0, 1, 1, &eepromBuffer.comp_pwm}, + { "STUCK_ROTOR_PROTECTION", T_BOOL, 0, 1, 1, &eepromBuffer.stuck_rotor_protection}, + { "ADVANCE_LEVEL", T_UINT8, 0, 4, 2, &eepromBuffer.advance_level}, + { "AUTO_ADVANCE", T_BOOL, 0, 1, 0, &eepromBuffer.auto_advance}, + { "STARTUP_POWER", T_UINT8, 50,150, 10, &eepromBuffer.startup_power}, + { "CURRENT_LIMIT", T_UINT8, 0, 200, 0, &eepromBuffer.limits.current}, + { "TEMPERATURE_LIMIT", T_UINT8, 70,255, 255,&eepromBuffer.limits.temperature}, + { "LOW_VOLTAGE_CUTOFF", T_BOOL, 0, 1, 0, &eepromBuffer.low_voltage_cut_off}, + { "CELL_VOLTAGE_THRESHOLD", T_UINT16, 250, 350, 300, &low_cell_volt_cutoff}, + { "BRAKE_ON_STOP", T_BOOL, 0, 1, 1, &eepromBuffer.brake_on_stop}, + { "DRIVING_BRAKE_STRENGTH", T_UINT8, 1, 10, 10, &eepromBuffer.driving_brake_strength}, + { "DRAG_BRAKE_STRENGTH", T_UINT8, 1, 10, 10, &eepromBuffer.drag_brake_strength}, + { "INPUT_SIGNAL_TYPE", T_UINT8, 0, 5, 5, &eepromBuffer.input_type}, + { "INPUT_FILTER_HZ", T_UINT8, 0, 100, 0, &eepromBuffer.can.filter_hz}, #ifdef CAN_TERM_PIN - { "CAN_TERM_ENABLE", T_BOOL, 0, 1, 0, &eepromBuffer.can.term_enable}, + { "CAN_TERM_ENABLE", T_BOOL, 0, 1, 0, &eepromBuffer.can.term_enable}, #endif - { "STARTUP_TUNE", T_STRING,0, 4, 0, &eepromBuffer.tune}, + { "STARTUP_TUNE", T_STRING,0, 4, 0, &eepromBuffer.tune}, }; /* @@ -178,35 +178,35 @@ static const struct parameter { */ static void load_settings(void) { + /* + run through parameters checking for those in the eepromBuffer + structure. For those parameters reset to default if out of + range. This allows the use of a defaults array that does not + include the CAN parameters + */ + for (uint8_t i=0; ivtype) { - case T_BOOL: - case T_UINT8: { - uint8_t *pvalue = (uint8_t *)p->ptr; - uint8_t max_value = p->max_value; - if (pvalue == &eepromBuffer.limits.current) { - max_value = max_value / 2; - } - if (*pvalue < p->min_value || *pvalue > max_value) { - *pvalue = p->default_value; - } - break; - } - case T_UINT16: - case T_STRING: - break; - } + switch (p->vtype) { + case T_BOOL: + case T_UINT8: { + uint8_t *pvalue = (uint8_t *)p->ptr; + uint8_t max_value = p->max_value; + if (pvalue == &eepromBuffer.limits.current) { + max_value = max_value / 2; + } + if (*pvalue < p->min_value || *pvalue > max_value) { + *pvalue = p->default_value; + } + break; + } + case T_UINT16: + case T_STRING: + break; } + } } /* @@ -214,8 +214,8 @@ static void load_settings(void) */ static void save_settings(void) { - saveEEpromSettings(); - can_printf("saved settings"); + saveEEpromSettings(); + can_printf("saved settings"); } /* @@ -225,7 +225,7 @@ static struct uavcan_protocol_NodeStatus node_status; static bool safe_to_write_settings(void) { - return !running || newinput == 0; + return !running || newinput == 0; } /* @@ -233,11 +233,11 @@ static bool safe_to_write_settings(void) */ static uint16_t get_random16(void) { - static uint32_t m_z = 1234; - static uint32_t m_w = 76542; - m_z = 36969 * (m_z & 0xFFFFu) + (m_z >> 16); - m_w = 18000 * (m_w & 0xFFFFu) + (m_w >> 16); - return ((m_z << 16) + m_w) & 0xFFFF; + static uint32_t m_z = 1234; + static uint32_t m_w = 76542; + m_z = 36969 * (m_z & 0xFFFFu) + (m_z >> 16); + m_w = 18000 * (m_w & 0xFFFFu) + (m_w >> 16); + return ((m_z << 16) + m_w) & 0xFFFF; } /* @@ -248,18 +248,18 @@ static uint16_t get_random16(void) */ static uint64_t micros64(void) { - static uint64_t base_us; - static uint16_t last_cnt; + static uint64_t base_us; + static uint16_t last_cnt; #ifdef ARTERY - uint16_t cnt = UTILITY_TIMER->cval; + uint16_t cnt = UTILITY_TIMER->cval; #else - uint16_t cnt = UTILITY_TIMER->CNT; + uint16_t cnt = UTILITY_TIMER->CNT; #endif - if (cnt < last_cnt) { - base_us += 0x10000; - } - last_cnt = cnt; - return base_us + cnt; + if (cnt < last_cnt) { + base_us += 0x10000; + } + last_cnt = cnt; + return base_us + cnt; } /* @@ -267,40 +267,40 @@ static uint64_t micros64(void) */ static uint32_t millis32(void) { - return micros64() / 1000ULL; + return micros64() / 1000ULL; } /* default settings, based on public/assets/eeprom_default.bin in AM32 configurator */ static const uint8_t default_settings[] = { - 0x01, 0x02, 0x01, 0x01, 0x23, 0x4e, 0x45, 0x4f, 0x45, 0x53, 0x43, 0x20, 0x66, 0x30, 0x35, 0x31, - 0x20, 0x00, 0x00, 0x00, 0x01, 0x01, 0x01, 0x02, 0x18, 0x64, 0x37, 0x0e, 0x00, 0x00, 0x05, 0x00, - 0x80, 0x80, 0x80, 0x32, 0x00, 0x32, 0x00, 0x00, 0x0f, 0x0a, 0x0a, 0x8d, 0x66, 0x06, 0x00, 0x00 + 0x01, 0x02, 0x01, 0x01, 0x23, 0x4e, 0x45, 0x4f, 0x45, 0x53, 0x43, 0x20, 0x66, 0x30, 0x35, 0x31, + 0x20, 0x00, 0x00, 0x00, 0x01, 0x01, 0x01, 0x02, 0x18, 0x64, 0x37, 0x0e, 0x00, 0x00, 0x05, 0x00, + 0x80, 0x80, 0x80, 0x32, 0x00, 0x32, 0x00, 0x00, 0x0f, 0x0a, 0x0a, 0x8d, 0x66, 0x06, 0x00, 0x00 }; // printf to CAN LogMessage for debugging static void can_printf(const char *fmt, ...) { - struct uavcan_protocol_debug_LogMessage pkt; - memset(&pkt, 0, sizeof(pkt)); - - uint8_t buffer[UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE]; - va_list ap; - va_start(ap, fmt); - uint32_t n = vsnprintf((char*)pkt.text.data, sizeof(pkt.text.data), fmt, ap); - va_end(ap); - pkt.text.len = MIN(n, sizeof(pkt.text.data)); - - uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer); - static uint8_t logmsg_transfer_id; - - canardBroadcast(&canard, - UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE, - UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID, - &logmsg_transfer_id, - CANARD_TRANSFER_PRIORITY_LOW, - buffer, len); + struct uavcan_protocol_debug_LogMessage pkt; + memset(&pkt, 0, sizeof(pkt)); + + uint8_t buffer[UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE]; + va_list ap; + va_start(ap, fmt); + uint32_t n = vsnprintf((char*)pkt.text.data, sizeof(pkt.text.data), fmt, ap); + va_end(ap); + pkt.text.len = MIN(n, sizeof(pkt.text.data)); + + uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer); + static uint8_t logmsg_transfer_id; + + canardBroadcast(&canard, + UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE, + UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID, + &logmsg_transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, len); } /* @@ -308,161 +308,161 @@ static void can_printf(const char *fmt, ...) */ static void handle_param_GetSet(CanardInstance* ins, CanardRxTransfer* transfer) { - struct uavcan_protocol_param_GetSetRequest req; - if (uavcan_protocol_param_GetSetRequest_decode(transfer, &req)) { - return; - } - - volatile const struct parameter *p = NULL; - if (req.name.len != 0) { - for (uint16_t i=0; ivtype) { - case T_UINT8: { - uint8_t *ptr8 = (uint8_t *)p->ptr; - if (ptr8 == &eepromBuffer.limits.current) { - *ptr8 = req.value.integer_value / 2; - } else { - *ptr8 = req.value.integer_value; - } - break; - } - case T_UINT16: { - uint16_t *ptr16 = (uint16_t *)p->ptr; - *ptr16 = req.value.integer_value; - if (ptr16 == &motor_kv) { - eepromBuffer.motor_kv = (uint8_t)((*(uint16_t *)p->ptr - 20) / 40); - } else if (ptr16 == &low_cell_volt_cutoff) { - eepromBuffer.low_cell_volt_cutoff = (uint8_t)(*ptr16 - 250); - } - break; - } - case T_BOOL: - *(uint8_t *)p->ptr = req.value.boolean_value?1:0; - break; - case T_STRING: - if (req.value.union_tag == UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE) { - if (p->ptr == (void*)eepromBuffer.tune) { - for (size_t i = 0; i < sizeof(eepromBuffer.tune); i++) { - if (i < req.value.string_value.len) { - eepromBuffer.tune[i] = req.value.string_value.data[i]; - } else { - eepromBuffer.tune[i] = 0xFF; - } - } - } - } - break; - default: - return; - } - - if (last_dir_reversed != eepromBuffer.dir_reversed || - last_bi_direction != eepromBuffer.bi_direction) { - // make dir_reversed and bi_direction change work without - // reboot - forward = 1 - eepromBuffer.dir_reversed; - running = 0; - armed = 0; - set_input(0); - } + struct uavcan_protocol_param_GetSetRequest req; + if (uavcan_protocol_param_GetSetRequest_decode(transfer, &req)) { + return; + } + + volatile const struct parameter *p = NULL; + if (req.name.len != 0) { + for (uint16_t i=0; iptr) - &eepromBuffer.buffer[0]); - switch (p->vtype) { - case T_UINT8: - pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; - pkt.value.integer_value = *(uint8_t *)p->ptr; - pkt.default_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; - if (eindex < sizeof(default_settings)) { - pkt.default_value.integer_value = default_settings[eindex]; - } else { - pkt.default_value.integer_value = p->default_value; - } - pkt.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE; - pkt.max_value.integer_value = p->max_value; - pkt.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE; - pkt.min_value.integer_value = p->min_value; - - // special case scaling - if ((uint8_t *)p->ptr == &eepromBuffer.limits.current) { - pkt.default_value.integer_value *= 2; - pkt.value.integer_value *= 2; - } - break; - case T_UINT16: - pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; - pkt.value.integer_value = *(uint16_t *)p->ptr; - pkt.default_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; - pkt.default_value.integer_value = p->default_value; - pkt.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE; - pkt.max_value.integer_value = p->max_value; - pkt.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE; - pkt.min_value.integer_value = p->min_value; - break; - case T_STRING: - pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE; - if (p->ptr == (void*)eepromBuffer.tune) { - pkt.value.string_value.len = sizeof(eepromBuffer.tune); - if (pkt.value.string_value.len > sizeof(pkt.value.string_value.data)) { - pkt.value.string_value.len = sizeof(pkt.value.string_value.data); - } - for (size_t i=0; i < pkt.value.string_value.len; i++) { - pkt.value.string_value.data[i] = eepromBuffer.tune[i]; - } - } - break; - case T_BOOL: - pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_BOOLEAN_VALUE; - pkt.value.boolean_value = (*(uint8_t *)p->ptr)?true:false; - pkt.default_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_BOOLEAN_VALUE; - if (eindex < sizeof(default_settings)) { - pkt.default_value.boolean_value = !!default_settings[eindex]; + switch (p->vtype) { + case T_UINT8: { + uint8_t *ptr8 = (uint8_t *)p->ptr; + if (ptr8 == &eepromBuffer.limits.current) { + *ptr8 = req.value.integer_value / 2; + } else { + *ptr8 = req.value.integer_value; + } + break; + } + case T_UINT16: { + uint16_t *ptr16 = (uint16_t *)p->ptr; + *ptr16 = req.value.integer_value; + if (ptr16 == &motor_kv) { + eepromBuffer.motor_kv = (uint8_t)((*(uint16_t *)p->ptr - 20) / 40); + } else if (ptr16 == &low_cell_volt_cutoff) { + eepromBuffer.low_cell_volt_cutoff = (uint8_t)(*ptr16 - 250); + } + break; + } + case T_BOOL: + *(uint8_t *)p->ptr = req.value.boolean_value?1:0; + break; + case T_STRING: + if (req.value.union_tag == UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE) { + if (p->ptr == (void*)eepromBuffer.tune) { + for (size_t i = 0; i < sizeof(eepromBuffer.tune); i++) { + if (i < req.value.string_value.len) { + eepromBuffer.tune[i] = req.value.string_value.data[i]; } else { - pkt.default_value.boolean_value = !!p->default_value; + eepromBuffer.tune[i] = 0xFF; } - break; - default: - return; + } } - pkt.name.len = strlen(p->name); - strcpy((char *)pkt.name.data, p->name); + } + break; + default: + return; } - uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE]; - uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, buffer); - - canardRequestOrRespond(ins, - transfer->source_node_id, - UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE, - UAVCAN_PROTOCOL_PARAM_GETSET_ID, - &transfer->transfer_id, - transfer->priority, - CanardResponse, - &buffer[0], - total_size); + if (last_dir_reversed != eepromBuffer.dir_reversed || + last_bi_direction != eepromBuffer.bi_direction) { + // make dir_reversed and bi_direction change work without + // reboot + forward = 1 - eepromBuffer.dir_reversed; + running = 0; + armed = 0; + set_input(0); + } + } + + /* + for both set and get we reply with the current value + */ + struct uavcan_protocol_param_GetSetResponse pkt; + memset(&pkt, 0, sizeof(pkt)); + + if (p != NULL) { + const uint32_t eindex = (uint32_t)(((const uint8_t *)p->ptr) - &eepromBuffer.buffer[0]); + switch (p->vtype) { + case T_UINT8: + pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + pkt.value.integer_value = *(uint8_t *)p->ptr; + pkt.default_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + if (eindex < sizeof(default_settings)) { + pkt.default_value.integer_value = default_settings[eindex]; + } else { + pkt.default_value.integer_value = p->default_value; + } + pkt.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE; + pkt.max_value.integer_value = p->max_value; + pkt.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE; + pkt.min_value.integer_value = p->min_value; + + // special case scaling + if ((uint8_t *)p->ptr == &eepromBuffer.limits.current) { + pkt.default_value.integer_value *= 2; + pkt.value.integer_value *= 2; + } + break; + case T_UINT16: + pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + pkt.value.integer_value = *(uint16_t *)p->ptr; + pkt.default_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + pkt.default_value.integer_value = p->default_value; + pkt.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE; + pkt.max_value.integer_value = p->max_value; + pkt.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE; + pkt.min_value.integer_value = p->min_value; + break; + case T_STRING: + pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE; + if (p->ptr == (void*)eepromBuffer.tune) { + pkt.value.string_value.len = sizeof(eepromBuffer.tune); + if (pkt.value.string_value.len > sizeof(pkt.value.string_value.data)) { + pkt.value.string_value.len = sizeof(pkt.value.string_value.data); + } + for (size_t i=0; i < pkt.value.string_value.len; i++) { + pkt.value.string_value.data[i] = eepromBuffer.tune[i]; + } + } + break; + case T_BOOL: + pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_BOOLEAN_VALUE; + pkt.value.boolean_value = (*(uint8_t *)p->ptr)?true:false; + pkt.default_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_BOOLEAN_VALUE; + if (eindex < sizeof(default_settings)) { + pkt.default_value.boolean_value = !!default_settings[eindex]; + } else { + pkt.default_value.boolean_value = !!p->default_value; + } + break; + default: + return; + } + pkt.name.len = strlen(p->name); + strcpy((char *)pkt.name.data, p->name); + } + + uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE]; + uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, buffer); + + canardRequestOrRespond(ins, + transfer->source_node_id, + UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE, + UAVCAN_PROTOCOL_PARAM_GETSET_ID, + &transfer->transfer_id, + transfer->priority, + CanardResponse, + &buffer[0], + total_size); } /* @@ -470,50 +470,50 @@ static void handle_param_GetSet(CanardInstance* ins, CanardRxTransfer* transfer) */ static void handle_param_ExecuteOpcode(CanardInstance* ins, CanardRxTransfer* transfer) { - struct uavcan_protocol_param_ExecuteOpcodeRequest req; - if (uavcan_protocol_param_ExecuteOpcodeRequest_decode(transfer, &req)) { - return; - } - struct uavcan_protocol_param_ExecuteOpcodeResponse pkt; - memset(&pkt, 0, sizeof(pkt)); - - pkt.ok = false; - - if (req.opcode == UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_REQUEST_OPCODE_ERASE) { - if (!safe_to_write_settings()) { - can_printf("No erase while running"); - } else { - can_printf("resetting to defaults"); - memset(eepromBuffer.buffer, 0xff, sizeof(eepromBuffer.buffer)); - memcpy(eepromBuffer.buffer, default_settings, sizeof(default_settings)); - save_flash_nolib(eepromBuffer.buffer, sizeof(eepromBuffer.buffer), eeprom_address); - loadEEpromSettings(); - load_settings(); - pkt.ok = true; - } + struct uavcan_protocol_param_ExecuteOpcodeRequest req; + if (uavcan_protocol_param_ExecuteOpcodeRequest_decode(transfer, &req)) { + return; + } + struct uavcan_protocol_param_ExecuteOpcodeResponse pkt; + memset(&pkt, 0, sizeof(pkt)); + + pkt.ok = false; + + if (req.opcode == UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_REQUEST_OPCODE_ERASE) { + if (!safe_to_write_settings()) { + can_printf("No erase while running"); + } else { + can_printf("resetting to defaults"); + memset(eepromBuffer.buffer, 0xff, sizeof(eepromBuffer.buffer)); + memcpy(eepromBuffer.buffer, default_settings, sizeof(default_settings)); + save_flash_nolib(eepromBuffer.buffer, sizeof(eepromBuffer.buffer), eeprom_address); + loadEEpromSettings(); + load_settings(); + pkt.ok = true; } - if (req.opcode == UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_REQUEST_OPCODE_SAVE) { - if (!safe_to_write_settings()) { - can_printf("No save while running"); - } else { - save_settings(); - pkt.ok = true; - } + } + if (req.opcode == UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_REQUEST_OPCODE_SAVE) { + if (!safe_to_write_settings()) { + can_printf("No save while running"); + } else { + save_settings(); + pkt.ok = true; } + } - uint8_t buffer[UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_MAX_SIZE]; - uint16_t total_size = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&pkt, buffer); + uint8_t buffer[UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_MAX_SIZE]; + uint16_t total_size = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&pkt, buffer); - canardRequestOrRespond(ins, - transfer->source_node_id, - UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE, - UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID, - &transfer->transfer_id, - transfer->priority, - CanardResponse, - &buffer[0], - total_size); + canardRequestOrRespond(ins, + transfer->source_node_id, + UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE, + UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID, + &transfer->transfer_id, + transfer->priority, + CanardResponse, + &buffer[0], + total_size); } /* @@ -521,10 +521,10 @@ static void handle_param_ExecuteOpcode(CanardInstance* ins, CanardRxTransfer* tr */ static void handle_RestartNode(CanardInstance* ins, CanardRxTransfer* transfer) { - // reboot the ESC - allOff(); - set_rtc_backup_register(0, 0); - NVIC_SystemReset(); + // reboot the ESC + allOff(); + set_rtc_backup_register(0, 0); + NVIC_SystemReset(); } /* @@ -532,40 +532,40 @@ static void handle_RestartNode(CanardInstance* ins, CanardRxTransfer* transfer) */ static void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) { - uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; - struct uavcan_protocol_GetNodeInfoResponse pkt; + uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; + struct uavcan_protocol_GetNodeInfoResponse pkt; - memset(&pkt, 0, sizeof(pkt)); + memset(&pkt, 0, sizeof(pkt)); - node_status.uptime_sec = micros64() / 1000000ULL; - pkt.status = node_status; + node_status.uptime_sec = micros64() / 1000000ULL; + pkt.status = node_status; - // fill in your major and minor firmware version - pkt.software_version.major = VERSION_MAJOR; - pkt.software_version.minor = VERSION_MINOR; - pkt.software_version.optional_field_flags = 0; - pkt.software_version.vcs_commit = 0; // should put git hash in here + // fill in your major and minor firmware version + pkt.software_version.major = VERSION_MAJOR; + pkt.software_version.minor = VERSION_MINOR; + pkt.software_version.optional_field_flags = 0; + pkt.software_version.vcs_commit = 0; // should put git hash in here - // should fill in hardware version - pkt.hardware_version.major = 2; - pkt.hardware_version.minor = 3; + // should fill in hardware version + pkt.hardware_version.major = 2; + pkt.hardware_version.minor = 3; - sys_can_getUniqueID(pkt.hardware_version.unique_id); + sys_can_getUniqueID(pkt.hardware_version.unique_id); - strncpy((char*)pkt.name.data, FIRMWARE_NAME, sizeof(pkt.name.data)); - pkt.name.len = strnlen((char*)pkt.name.data, sizeof(pkt.name.data)); + strncpy((char*)pkt.name.data, FIRMWARE_NAME, sizeof(pkt.name.data)); + pkt.name.len = strnlen((char*)pkt.name.data, sizeof(pkt.name.data)); - uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer); + uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer); - canardRequestOrRespond(ins, - transfer->source_node_id, - UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, - UAVCAN_PROTOCOL_GETNODEINFO_ID, - &transfer->transfer_id, - transfer->priority, - CanardResponse, - &buffer[0], - total_size); + canardRequestOrRespond(ins, + transfer->source_node_id, + UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, + UAVCAN_PROTOCOL_GETNODEINFO_ID, + &transfer->transfer_id, + transfer->priority, + CanardResponse, + &buffer[0], + total_size); } extern void transfercomplete(); @@ -577,26 +577,26 @@ extern void setInput(); */ static void set_input(uint16_t input) { - if (!armed && input != 0 && eepromBuffer.can.require_arming && - dronecan_armed && !eepromBuffer.can.require_zero_throttle) { - // allow restart if unexpected ESC reboot in flight - armed = 1; - } + if (!armed && input != 0 && eepromBuffer.can.require_arming && + dronecan_armed && !eepromBuffer.can.require_zero_throttle) { + // allow restart if unexpected ESC reboot in flight + armed = 1; + } - const uint16_t unfiltered_input = (dronecan_armed || !eepromBuffer.can.require_arming)? input : 0; - const uint16_t filtered_input = Filter2P_apply(unfiltered_input, eepromBuffer.can.filter_hz, 1000); + const uint16_t unfiltered_input = (dronecan_armed || !eepromBuffer.can.require_arming)? input : 0; + const uint16_t filtered_input = Filter2P_apply(unfiltered_input, eepromBuffer.can.filter_hz, 1000); - newinput = filtered_input; - last_can_input = unfiltered_input; - inputSet = 1; + newinput = filtered_input; + last_can_input = unfiltered_input; + inputSet = 1; - // we must set dshot for bi_direction to work - dshot = eepromBuffer.bi_direction; + // we must set dshot for bi_direction to work + dshot = eepromBuffer.bi_direction; - transfercomplete(); - setInput(); + transfercomplete(); + setInput(); - canstats.num_input++; + canstats.num_input++; } /* @@ -604,46 +604,46 @@ static void set_input(uint16_t input) */ static void handle_RawCommand(CanardInstance *ins, CanardRxTransfer *transfer) { - struct uavcan_equipment_esc_RawCommand cmd; - if (uavcan_equipment_esc_RawCommand_decode(transfer, &cmd)) { - return; - } - // see if it is for us - if (cmd.cmd.len <= eepromBuffer.can.esc_index) { - return; - } - - // throttle demand is a value from -8191 to 8191. Negative values - // are for reverse throttle - const int16_t input_can = cmd.cmd.data[(unsigned)eepromBuffer.can.esc_index]; - - /* - we need to map onto the AM32 expected range, which is a 11 bit number, where: - 0: off - 1-46: special codes - 47-2047: throttle - */ - uint16_t this_input = 0; - if (input_can == 0) { - this_input = 0; - } else if (eepromBuffer.bi_direction) { - const float scaled_value = input_can * (1000.0 / 8192); - if (scaled_value >= 0) { - this_input = (uint16_t)(47 + scaled_value); - } else { - this_input = (uint16_t)(47 + (1000 - scaled_value)); - } - } else if (input_can > 0) { - const float scaled_value = input_can * (2000.0 / 8192); - this_input = (uint16_t)(47 + scaled_value); + struct uavcan_equipment_esc_RawCommand cmd; + if (uavcan_equipment_esc_RawCommand_decode(transfer, &cmd)) { + return; + } + // see if it is for us + if (cmd.cmd.len <= eepromBuffer.can.esc_index) { + return; + } + + // throttle demand is a value from -8191 to 8191. Negative values + // are for reverse throttle + const int16_t input_can = cmd.cmd.data[(unsigned)eepromBuffer.can.esc_index]; + + /* + we need to map onto the AM32 expected range, which is a 11 bit number, where: + 0: off + 1-46: special codes + 47-2047: throttle + */ + uint16_t this_input = 0; + if (input_can == 0) { + this_input = 0; + } else if (eepromBuffer.bi_direction) { + const float scaled_value = input_can * (1000.0 / 8192); + if (scaled_value >= 0) { + this_input = (uint16_t)(47 + scaled_value); + } else { + this_input = (uint16_t)(47 + (1000 - scaled_value)); } + } else if (input_can > 0) { + const float scaled_value = input_can * (2000.0 / 8192); + this_input = (uint16_t)(47 + scaled_value); + } - const uint64_t ts = micros64(); - canstats.num_commands++; - canstats.total_commands++; - canstats.last_raw_command_us = ts; + const uint64_t ts = micros64(); + canstats.num_commands++; + canstats.total_commands++; + canstats.last_raw_command_us = ts; - set_input(this_input); + set_input(this_input); } /* @@ -651,15 +651,15 @@ static void handle_RawCommand(CanardInstance *ins, CanardRxTransfer *transfer) */ static void handle_ArmingStatus(CanardInstance *ins, CanardRxTransfer *transfer) { - struct uavcan_equipment_safety_ArmingStatus cmd; - if (uavcan_equipment_safety_ArmingStatus_decode(transfer, &cmd)) { - return; - } - - dronecan_armed = (cmd.status == UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_STATUS_FULLY_ARMED); - if (!dronecan_armed && eepromBuffer.can.require_arming && canstats.last_raw_command_us != 0) { - set_input(0); - } + struct uavcan_equipment_safety_ArmingStatus cmd; + if (uavcan_equipment_safety_ArmingStatus_decode(transfer, &cmd)) { + return; + } + + dronecan_armed = (cmd.status == UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_STATUS_FULLY_ARMED); + if (!dronecan_armed && eepromBuffer.can.require_arming && canstats.last_raw_command_us != 0) { + set_input(0); + } } /* @@ -668,71 +668,71 @@ static void handle_ArmingStatus(CanardInstance *ins, CanardRxTransfer *transfer) */ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* transfer) { - if (!safe_to_write_settings()) { - can_printf("No update while running"); - return; - } + if (!safe_to_write_settings()) { + can_printf("No update while running"); + return; + } - struct uavcan_protocol_file_BeginFirmwareUpdateRequest req; - if (uavcan_protocol_file_BeginFirmwareUpdateRequest_decode(transfer, &req)) { - return; - } + struct uavcan_protocol_file_BeginFirmwareUpdateRequest req; + if (uavcan_protocol_file_BeginFirmwareUpdateRequest_decode(transfer, &req)) { + return; + } - sys_can_disable_IRQ(); + sys_can_disable_IRQ(); - uint32_t reg[2] = {0,0}; - if (req.image_file_remote_path.path.len <= 8) { - // path is normally hashed and fits in 8 bytes, put in rtc backup registers 1 and 2 - memcpy((uint8_t *)reg, req.image_file_remote_path.path.data, req.image_file_remote_path.path.len); + uint32_t reg[2] = {0,0}; + if (req.image_file_remote_path.path.len <= 8) { + // path is normally hashed and fits in 8 bytes, put in rtc backup registers 1 and 2 + memcpy((uint8_t *)reg, req.image_file_remote_path.path.data, req.image_file_remote_path.path.len); - uint8_t buffer[UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_MAX_SIZE]; - struct uavcan_protocol_file_BeginFirmwareUpdateResponse reply; - memset(&reply, 0, sizeof(reply)); - reply.error = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK; + uint8_t buffer[UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_MAX_SIZE]; + struct uavcan_protocol_file_BeginFirmwareUpdateResponse reply; + memset(&reply, 0, sizeof(reply)); + reply.error = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK; - uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer); + uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer); - canardRequestOrRespond(ins, - transfer->source_node_id, - UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE, - UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID, - &transfer->transfer_id, - transfer->priority, - CanardResponse, - &buffer[0], - total_size); - - while (canardPeekTxQueue(&canard) != NULL) { - DroneCAN_processTxQueue(); - } + canardRequestOrRespond(ins, + transfer->source_node_id, + UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE, + UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID, + &transfer->transfer_id, + transfer->priority, + CanardResponse, + &buffer[0], + total_size); - // time to transmit - delayMillis(2); + while (canardPeekTxQueue(&canard) != NULL) { + DroneCAN_processTxQueue(); } - set_rtc_backup_register(1, reg[0]); - set_rtc_backup_register(2, reg[1]); - - // tell the bootloader we are doing fw update - set_rtc_backup_register(0, - (canardGetLocalNodeID(&canard)<<24) | - (transfer->source_node_id<<16) | - RTC_BKUP0_FWUPDATE); - - // reboot and let bootloader handle the request, this means the - // first request doesn't get a reply, and the client re-sends. We - // need this to get the path to the client. We could instead - // define a memory block which is not reset on boot and put the - // path there, but this is simpler - NVIC_SystemReset(); + // time to transmit + delayMillis(2); + } + + set_rtc_backup_register(1, reg[0]); + set_rtc_backup_register(2, reg[1]); + + // tell the bootloader we are doing fw update + set_rtc_backup_register(0, + (canardGetLocalNodeID(&canard)<<24) | + (transfer->source_node_id<<16) | + RTC_BKUP0_FWUPDATE); + + // reboot and let bootloader handle the request, this means the + // first request doesn't get a reply, and the client re-sends. We + // need this to get the path to the client. We could instead + // define a memory block which is not reset on boot and put the + // path there, but this is simpler + NVIC_SystemReset(); } /* data for dynamic node allocation process */ static struct { - uint32_t send_next_node_id_allocation_request_at_ms; - uint32_t node_id_allocation_unique_id_offset; + uint32_t send_next_node_id_allocation_request_at_ms; + uint32_t node_id_allocation_unique_id_offset; } DNA; /* @@ -740,50 +740,50 @@ static struct { */ static void handle_DNA_Allocation(CanardInstance *ins, CanardRxTransfer *transfer) { - if (canardGetLocalNodeID(&canard) != CANARD_BROADCAST_NODE_ID) { - // already allocated - return; - } - - // Rule C - updating the randomized time interval - DNA.send_next_node_id_allocation_request_at_ms = - millis32() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + - (get_random16() % UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS); + if (canardGetLocalNodeID(&canard) != CANARD_BROADCAST_NODE_ID) { + // already allocated + return; + } - if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID) { - DNA.node_id_allocation_unique_id_offset = 0; - return; - } + // Rule C - updating the randomized time interval + DNA.send_next_node_id_allocation_request_at_ms = + millis32() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + + (get_random16() % UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS); - // Copying the unique ID from the message - struct uavcan_protocol_dynamic_node_id_Allocation msg; - - if (uavcan_protocol_dynamic_node_id_Allocation_decode(transfer, &msg)) { - /* bad packet */ - return; - } + if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID) { + DNA.node_id_allocation_unique_id_offset = 0; + return; + } - // Obtaining the local unique ID - uint8_t my_unique_id[sizeof(msg.unique_id.data)]; - sys_can_getUniqueID(my_unique_id); + // Copying the unique ID from the message + struct uavcan_protocol_dynamic_node_id_Allocation msg; - // Matching the received UID against the local one - if (memcmp(msg.unique_id.data, my_unique_id, msg.unique_id.len) != 0) { - DNA.node_id_allocation_unique_id_offset = 0; - // No match, return - return; - } + if (uavcan_protocol_dynamic_node_id_Allocation_decode(transfer, &msg)) { + /* bad packet */ + return; + } - if (msg.unique_id.len < sizeof(msg.unique_id.data)) { - // The allocator has confirmed part of unique ID, switching to - // the next stage and updating the timeout. - DNA.node_id_allocation_unique_id_offset = msg.unique_id.len; - DNA.send_next_node_id_allocation_request_at_ms -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS; + // Obtaining the local unique ID + uint8_t my_unique_id[sizeof(msg.unique_id.data)]; + sys_can_getUniqueID(my_unique_id); - } else { - // Allocation complete - copying the allocated node ID from the message - canardSetLocalNodeID(ins, msg.node_id); - } + // Matching the received UID against the local one + if (memcmp(msg.unique_id.data, my_unique_id, msg.unique_id.len) != 0) { + DNA.node_id_allocation_unique_id_offset = 0; + // No match, return + return; + } + + if (msg.unique_id.len < sizeof(msg.unique_id.data)) { + // The allocator has confirmed part of unique ID, switching to + // the next stage and updating the timeout. + DNA.node_id_allocation_unique_id_offset = msg.unique_id.len; + DNA.send_next_node_id_allocation_request_at_ms -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS; + + } else { + // Allocation complete - copying the allocated node ID from the message + canardSetLocalNodeID(ins, msg.node_id); + } } /* @@ -791,45 +791,45 @@ static void handle_DNA_Allocation(CanardInstance *ins, CanardRxTransfer *transfe */ static void request_DNA() { - const uint32_t now = millis32(); - static uint8_t node_id_allocation_transfer_id = 0; + const uint32_t now = millis32(); + static uint8_t node_id_allocation_transfer_id = 0; - DNA.send_next_node_id_allocation_request_at_ms = - now + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + - (get_random16() % UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS); + DNA.send_next_node_id_allocation_request_at_ms = + now + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + + (get_random16() % UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS); - // Structure of the request is documented in the DSDL definition - // See http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation - uint8_t allocation_request[CANARD_CAN_FRAME_MAX_DATA_LEN - 1]; - allocation_request[0] = (uint8_t)(PREFERRED_NODE_ID << 1U); + // Structure of the request is documented in the DSDL definition + // See http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation + uint8_t allocation_request[CANARD_CAN_FRAME_MAX_DATA_LEN - 1]; + allocation_request[0] = (uint8_t)(PREFERRED_NODE_ID << 1U); - if (DNA.node_id_allocation_unique_id_offset == 0) { - allocation_request[0] |= 1; // First part of unique ID - } + if (DNA.node_id_allocation_unique_id_offset == 0) { + allocation_request[0] |= 1; // First part of unique ID + } - uint8_t my_unique_id[16]; - sys_can_getUniqueID(my_unique_id); + uint8_t my_unique_id[16]; + sys_can_getUniqueID(my_unique_id); - static const uint8_t MaxLenOfUniqueIDInRequest = 6; - uint8_t uid_size = (uint8_t)(16 - DNA.node_id_allocation_unique_id_offset); - - if (uid_size > MaxLenOfUniqueIDInRequest) { - uid_size = MaxLenOfUniqueIDInRequest; - } + static const uint8_t MaxLenOfUniqueIDInRequest = 6; + uint8_t uid_size = (uint8_t)(16 - DNA.node_id_allocation_unique_id_offset); - memmove(&allocation_request[1], &my_unique_id[DNA.node_id_allocation_unique_id_offset], uid_size); + if (uid_size > MaxLenOfUniqueIDInRequest) { + uid_size = MaxLenOfUniqueIDInRequest; + } - // Broadcasting the request - canardBroadcast(&canard, - UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE, - UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID, - &node_id_allocation_transfer_id, - CANARD_TRANSFER_PRIORITY_LOW, - &allocation_request[0], - (uint16_t) (uid_size + 1)); + memmove(&allocation_request[1], &my_unique_id[DNA.node_id_allocation_unique_id_offset], uid_size); - // Preparing for timeout; if response is received, this value will be updated from the callback. - DNA.node_id_allocation_unique_id_offset = 0; + // Broadcasting the request + canardBroadcast(&canard, + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE, + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID, + &node_id_allocation_transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + &allocation_request[0], + (uint16_t) (uid_size + 1)); + + // Preparing for timeout; if response is received, this value will be updated from the callback. + DNA.node_id_allocation_unique_id_offset = 0; } /* @@ -837,53 +837,53 @@ static void request_DNA() */ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) { - // tell main loop we have had signal so we don't reset - signaltimeout = 0; - - canstats.on_receive++; - // switch on data type ID to pass to the right handler function - if (transfer->transfer_type == CanardTransferTypeRequest) { - // check if we want to handle a specific service request - switch (transfer->data_type_id) { - case UAVCAN_PROTOCOL_GETNODEINFO_ID: { - handle_GetNodeInfo(ins, transfer); - break; - } - case UAVCAN_PROTOCOL_PARAM_GETSET_ID: { - handle_param_GetSet(ins, transfer); - break; - } - case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID: { - handle_param_ExecuteOpcode(ins, transfer); - break; - } - case UAVCAN_PROTOCOL_RESTARTNODE_ID: { - handle_RestartNode(ins, transfer); - break; - } - case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID: { - handle_begin_firmware_update(ins, transfer); - break; - } - } + // tell main loop we have had signal so we don't reset + signaltimeout = 0; + + canstats.on_receive++; + // switch on data type ID to pass to the right handler function + if (transfer->transfer_type == CanardTransferTypeRequest) { + // check if we want to handle a specific service request + switch (transfer->data_type_id) { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: { + handle_GetNodeInfo(ins, transfer); + break; } - if (transfer->transfer_type == CanardTransferTypeBroadcast) { - // check if we want to handle a specific broadcast message - switch (transfer->data_type_id) { - case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID: { - handle_RawCommand(ins, transfer); - break; - } - case UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID: { - handle_DNA_Allocation(ins, transfer); - break; - } - case UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_ID: { - handle_ArmingStatus(ins, transfer); - break; - } - } + case UAVCAN_PROTOCOL_PARAM_GETSET_ID: { + handle_param_GetSet(ins, transfer); + break; + } + case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID: { + handle_param_ExecuteOpcode(ins, transfer); + break; + } + case UAVCAN_PROTOCOL_RESTARTNODE_ID: { + handle_RestartNode(ins, transfer); + break; + } + case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID: { + handle_begin_firmware_update(ins, transfer); + break; + } + } + } + if (transfer->transfer_type == CanardTransferTypeBroadcast) { + // check if we want to handle a specific broadcast message + switch (transfer->data_type_id) { + case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID: { + handle_RawCommand(ins, transfer); + break; + } + case UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID: { + handle_DNA_Allocation(ins, transfer); + break; + } + case UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_ID: { + handle_ArmingStatus(ins, transfer); + break; } + } + } } @@ -902,51 +902,51 @@ static bool shouldAcceptTransfer(const CanardInstance *ins, CanardTransferType transfer_type, uint8_t source_node_id) { - canstats.should_accept++; - if (transfer_type == CanardTransferTypeRequest) { - // check if we want to handle a specific service request - switch (data_type_id) { - case UAVCAN_PROTOCOL_GETNODEINFO_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE; - return true; - } - case UAVCAN_PROTOCOL_PARAM_GETSET_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE; - return true; - } - case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE; - return true; - } - case UAVCAN_PROTOCOL_RESTARTNODE_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE; - return true; - } - case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE; - return true; - } - } + canstats.should_accept++; + if (transfer_type == CanardTransferTypeRequest) { + // check if we want to handle a specific service request + switch (data_type_id) { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: { + *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE; + return true; } - if (transfer_type == CanardTransferTypeBroadcast) { - // see if we want to handle a specific broadcast packet - switch (data_type_id) { - case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID: { - *out_data_type_signature = UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE; - return true; - } - case UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE; - return true; - } - case UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_ID: { - *out_data_type_signature = UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_SIGNATURE; - return true; - } - } + case UAVCAN_PROTOCOL_PARAM_GETSET_ID: { + *out_data_type_signature = UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE; + return true; } - // we don't want any other messages - return false; + case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID: { + *out_data_type_signature = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE; + return true; + } + case UAVCAN_PROTOCOL_RESTARTNODE_ID: { + *out_data_type_signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE; + return true; + } + case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID: { + *out_data_type_signature = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE; + return true; + } + } + } + if (transfer_type == CanardTransferTypeBroadcast) { + // see if we want to handle a specific broadcast packet + switch (data_type_id) { + case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID: { + *out_data_type_signature = UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE; + return true; + } + case UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID: { + *out_data_type_signature = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE; + return true; + } + case UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_ID: { + *out_data_type_signature = UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_SIGNATURE; + return true; + } + } + } + // we don't want any other messages + return false; } /* @@ -955,32 +955,32 @@ static bool shouldAcceptTransfer(const CanardInstance *ins, */ static void send_NodeStatus(void) { - uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; - - node_status.uptime_sec = micros64() / 1000000ULL; - node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; - node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; - node_status.sub_mode = 0; - - // put number of commands we have received in vendor status since the last NodeStatus - // this means vendor status gives us approximate command rate in commands/second - node_status.vendor_specific_status_code = canstats.num_commands; - canstats.num_commands = 0; - - uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer); - - // we need a static variable for the transfer ID. This is - // incremeneted on each transfer, allowing for detection of packet - // loss - static uint8_t transfer_id; - - canardBroadcast(&canard, - UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, - UAVCAN_PROTOCOL_NODESTATUS_ID, - &transfer_id, - CANARD_TRANSFER_PRIORITY_LOW, - buffer, - len); + uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; + + node_status.uptime_sec = micros64() / 1000000ULL; + node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; + node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; + node_status.sub_mode = 0; + + // put number of commands we have received in vendor status since the last NodeStatus + // this means vendor status gives us approximate command rate in commands/second + node_status.vendor_specific_status_code = canstats.num_commands; + canstats.num_commands = 0; + + uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer); + + // we need a static variable for the transfer ID. This is + // incremeneted on each transfer, allowing for detection of packet + // loss + static uint8_t transfer_id; + + canardBroadcast(&canard, + UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, + UAVCAN_PROTOCOL_NODESTATUS_ID, + &transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, + len); } /* @@ -988,18 +988,18 @@ static void send_NodeStatus(void) */ static void process1HzTasks(uint64_t timestamp_usec) { - /* - Purge transfers that are no longer transmitted. This can free up some memory - */ - canardCleanupStaleTransfers(&canard, timestamp_usec); + /* + Purge transfers that are no longer transmitted. This can free up some memory + */ + canardCleanupStaleTransfers(&canard, timestamp_usec); - /* - Transmit the node status message - */ - send_NodeStatus(); + /* + Transmit the node status message + */ + send_NodeStatus(); #ifdef CAN_TERM_PIN - setup_portpin(CAN_TERM_PIN, eepromBuffer.can.term_enable? CAN_TERM_POLARITY : !CAN_TERM_POLARITY); + setup_portpin(CAN_TERM_PIN, eepromBuffer.can.term_enable? CAN_TERM_POLARITY : !CAN_TERM_POLARITY); #endif } @@ -1008,36 +1008,36 @@ static void process1HzTasks(uint64_t timestamp_usec) */ static void send_ESCStatus(void) { - struct uavcan_equipment_esc_Status pkt; - uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE]; - - // make up some synthetic status data - pkt.error_count = 0; - pkt.voltage = battery_voltage * 0.01; - - pkt.current = (current.sum/(float)current.count) * 0.01; - current.sum = 0; - current.count = 0; - - pkt.temperature = C_TO_KELVIN(degrees_celsius); - pkt.rpm = (e_rpm * 200) / eepromBuffer.motor_poles; - pkt.power_rating_pct = 0; // how do we get this? - pkt.esc_index = eepromBuffer.can.esc_index; - - uint32_t len = uavcan_equipment_esc_Status_encode(&pkt, buffer); - - // we need a static variable for the transfer ID. This is - // incremeneted on each transfer, allowing for detection of packet - // loss - static uint8_t transfer_id; - - canardBroadcast(&canard, - UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, - UAVCAN_EQUIPMENT_ESC_STATUS_ID, - &transfer_id, - CANARD_TRANSFER_PRIORITY_LOW, - buffer, - len); + struct uavcan_equipment_esc_Status pkt; + uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE]; + + // make up some synthetic status data + pkt.error_count = 0; + pkt.voltage = battery_voltage * 0.01; + + pkt.current = (current.sum/(float)current.count) * 0.01; + current.sum = 0; + current.count = 0; + + pkt.temperature = C_TO_KELVIN(degrees_celsius); + pkt.rpm = (e_rpm * 200) / eepromBuffer.motor_poles; + pkt.power_rating_pct = 0; // how do we get this? + pkt.esc_index = eepromBuffer.can.esc_index; + + uint32_t len = uavcan_equipment_esc_Status_encode(&pkt, buffer); + + // we need a static variable for the transfer ID. This is + // incremeneted on each transfer, allowing for detection of packet + // loss + static uint8_t transfer_id; + + canardBroadcast(&canard, + UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, + UAVCAN_EQUIPMENT_ESC_STATUS_ID, + &transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, + len); } /* @@ -1045,42 +1045,42 @@ static void send_ESCStatus(void) */ static void send_FlexDebug(void) { - static struct { - uint32_t total_commands; - uint32_t num_input; - } last; - /* - popupate debug1 - */ - debug1.version = 1; - debug1.commutation_interval = commutation_interval; - debug1.auto_advance_level = auto_advance_level; - debug1.num_commands = canstats.total_commands - last.total_commands; - debug1.num_input = canstats.num_input - last.num_input; - debug1.rx_errors = canstats.rx_errors; - debug1.rxframe_error = canstats.rxframe_error; - debug1.rx_ecode = canstats.rx_ecode; - - last.num_input = canstats.num_input; - last.total_commands = canstats.total_commands; - - struct dronecan_protocol_FlexDebug pkt; - uint8_t buffer[DRONECAN_PROTOCOL_FLEXDEBUG_MAX_SIZE]; - - pkt.id = DRONECAN_PROTOCOL_FLEXDEBUG_AM32_RESERVE_START+0; - pkt.u8.len = sizeof(debug1); - memcpy(pkt.u8.data, (const uint8_t *)&debug1, sizeof(debug1)); - uint32_t len = dronecan_protocol_FlexDebug_encode(&pkt, buffer); - - static uint8_t transfer_id; - - canardBroadcast(&canard, - DRONECAN_PROTOCOL_FLEXDEBUG_SIGNATURE, - DRONECAN_PROTOCOL_FLEXDEBUG_ID, - &transfer_id, - CANARD_TRANSFER_PRIORITY_LOW, - buffer, - len); + static struct { + uint32_t total_commands; + uint32_t num_input; + } last; + /* + popupate debug1 + */ + debug1.version = 1; + debug1.commutation_interval = commutation_interval; + debug1.auto_advance_level = auto_advance_level; + debug1.num_commands = canstats.total_commands - last.total_commands; + debug1.num_input = canstats.num_input - last.num_input; + debug1.rx_errors = canstats.rx_errors; + debug1.rxframe_error = canstats.rxframe_error; + debug1.rx_ecode = canstats.rx_ecode; + + last.num_input = canstats.num_input; + last.total_commands = canstats.total_commands; + + struct dronecan_protocol_FlexDebug pkt; + uint8_t buffer[DRONECAN_PROTOCOL_FLEXDEBUG_MAX_SIZE]; + + pkt.id = DRONECAN_PROTOCOL_FLEXDEBUG_AM32_RESERVE_START+0; + pkt.u8.len = sizeof(debug1); + memcpy(pkt.u8.data, (const uint8_t *)&debug1, sizeof(debug1)); + uint32_t len = dronecan_protocol_FlexDebug_encode(&pkt, buffer); + + static uint8_t transfer_id; + + canardBroadcast(&canard, + DRONECAN_PROTOCOL_FLEXDEBUG_SIGNATURE, + DRONECAN_PROTOCOL_FLEXDEBUG_ID, + &transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, + len); } @@ -1089,15 +1089,15 @@ static void send_FlexDebug(void) */ void DroneCAN_receiveFrame(void) { - CanardCANFrame rx_frame = {0}; - while (sys_can_receive(&rx_frame) > 0) { - canstats.num_receive++; - int ecode = canardHandleRxFrame(&canard, &rx_frame, micros64()); - if (ecode != CANARD_OK && ecode != -CANARD_ERROR_RX_NOT_WANTED) { - canstats.rx_ecode = ecode; - canstats.rxframe_error++; - } + CanardCANFrame rx_frame = {0}; + while (sys_can_receive(&rx_frame) > 0) { + canstats.num_receive++; + int ecode = canardHandleRxFrame(&canard, &rx_frame, micros64()); + if (ecode != CANARD_OK && ecode != -CANARD_ERROR_RX_NOT_WANTED) { + canstats.rx_ecode = ecode; + canstats.rxframe_error++; } + } } /* @@ -1105,124 +1105,124 @@ void DroneCAN_receiveFrame(void) */ void DroneCAN_processTxQueue(void) { - for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard)) != NULL;) { - const int16_t tx_res = sys_can_transmit(txf); - if (tx_res == 0) { - // no space, stop trying - break; - } - // success or error, remove frame - canardPopTxQueue(&canard); + for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard)) != NULL;) { + const int16_t tx_res = sys_can_transmit(txf); + if (tx_res == 0) { + // no space, stop trying + break; } + // success or error, remove frame + canardPopTxQueue(&canard); + } } static void DroneCAN_Startup(void) { - load_settings(); + load_settings(); - canardInit(&canard, - canard_memory_pool, // Raw memory chunk used for dynamic allocation - sizeof(canard_memory_pool), - onTransferReceived, // Callback, see CanardOnTransferReception - shouldAcceptTransfer, // Callback, see CanardShouldAcceptTransfer - NULL); + canardInit(&canard, + canard_memory_pool, // Raw memory chunk used for dynamic allocation + sizeof(canard_memory_pool), + onTransferReceived, // Callback, see CanardOnTransferReception + shouldAcceptTransfer, // Callback, see CanardShouldAcceptTransfer + NULL); - if (eepromBuffer.can.can_node != 0) { - canardSetLocalNodeID(&canard, eepromBuffer.can.can_node); - } + if (eepromBuffer.can.can_node != 0) { + canardSetLocalNodeID(&canard, eepromBuffer.can.can_node); + } - // initialise low level CAN peripheral hardware - sys_can_init(); + // initialise low level CAN peripheral hardware + sys_can_init(); - if (eepromBuffer.input_type == DRONECAN_IN) { - /* - disable interrupts for DShot and PWM - */ + if (eepromBuffer.input_type == DRONECAN_IN) { + /* + disable interrupts for DShot and PWM + */ #ifdef MCU_L431 - NVIC_DisableIRQ(DMA1_Channel5_IRQn); - NVIC_DisableIRQ(EXTI15_10_IRQn); - EXTI->IMR1 &= ~(1U << 15); + NVIC_DisableIRQ(DMA1_Channel5_IRQn); + NVIC_DisableIRQ(EXTI15_10_IRQn); + EXTI->IMR1 &= ~(1U << 15); #elif defined(MCU_AT415) - NVIC_DisableIRQ(DMA1_Channel6_IRQn); - NVIC_DisableIRQ(EXINT15_10_IRQn); - EXINT->inten &= ~EXINT_LINE_15; + NVIC_DisableIRQ(DMA1_Channel6_IRQn); + NVIC_DisableIRQ(EXINT15_10_IRQn); + EXINT->inten &= ~EXINT_LINE_15; #else - #error "unsupported MCU" +#error "unsupported MCU" #endif - } + } } void DroneCAN_update() { - sys_can_disable_IRQ(); - - static uint64_t next_1hz_service_at; - static uint64_t next_telem_service_at; - static uint64_t next_flexdebug_at; - if (!done_startup) { - DroneCAN_Startup(); - done_startup = true; - set_rtc_backup_register(0, RTC_BKUP0_BOOTED); + sys_can_disable_IRQ(); + + static uint64_t next_1hz_service_at; + static uint64_t next_telem_service_at; + static uint64_t next_flexdebug_at; + if (!done_startup) { + DroneCAN_Startup(); + done_startup = true; + set_rtc_backup_register(0, RTC_BKUP0_BOOTED); + } + + if (canstats.on_receive == 5) { + // indicate to bootloader that we were fully operational + set_rtc_backup_register(0, RTC_BKUP0_SIGNAL); + } + + DroneCAN_processTxQueue(); + + // see if we are still doing DNA + if (canardGetLocalNodeID(&canard) == CANARD_BROADCAST_NODE_ID) { + // we're still waiting for a DNA allocation of our node ID + if (millis32() > DNA.send_next_node_id_allocation_request_at_ms) { + request_DNA(); } - - if (canstats.on_receive == 5) { - // indicate to bootloader that we were fully operational - set_rtc_backup_register(0, RTC_BKUP0_SIGNAL); - } - - DroneCAN_processTxQueue(); - - // see if we are still doing DNA - if (canardGetLocalNodeID(&canard) == CANARD_BROADCAST_NODE_ID) { - // we're still waiting for a DNA allocation of our node ID - if (millis32() > DNA.send_next_node_id_allocation_request_at_ms) { - request_DNA(); - } - sys_can_enable_IRQ(); - return; - } - - const uint64_t ts = micros64(); - - if (ts >= next_1hz_service_at) { - next_1hz_service_at += 1000000ULL; - process1HzTasks(ts); - } - if (eepromBuffer.can.telem_rate > 0 && ts >= next_telem_service_at) { - next_telem_service_at += 1000000ULL/eepromBuffer.can.telem_rate; - send_ESCStatus(); - } - if (eepromBuffer.can.debug_rate > 0 && ts >= next_flexdebug_at) { - next_flexdebug_at += 1000000ULL/eepromBuffer.can.debug_rate; - send_FlexDebug(); - } - - DroneCAN_processTxQueue(); - - if (canstats.last_raw_command_us != 0 && ts - canstats.last_raw_command_us > 250000ULL) { - /* - we have stopped getting CAN RawCommand, zero input - */ - canstats.last_raw_command_us = 0; - set_input(0); - } - if (ts - canstats.last_raw_command_us > TARGET_PERIOD_US) { - // ensure at least 1kHz signal is seen by main code - set_input(last_can_input); - canstats.last_raw_command_us = ts; - } - sys_can_enable_IRQ(); + return; + } + + const uint64_t ts = micros64(); + + if (ts >= next_1hz_service_at) { + next_1hz_service_at += 1000000ULL; + process1HzTasks(ts); + } + if (eepromBuffer.can.telem_rate > 0 && ts >= next_telem_service_at) { + next_telem_service_at += 1000000ULL/eepromBuffer.can.telem_rate; + send_ESCStatus(); + } + if (eepromBuffer.can.debug_rate > 0 && ts >= next_flexdebug_at) { + next_flexdebug_at += 1000000ULL/eepromBuffer.can.debug_rate; + send_FlexDebug(); + } + + DroneCAN_processTxQueue(); + + if (canstats.last_raw_command_us != 0 && ts - canstats.last_raw_command_us > 250000ULL) { + /* + we have stopped getting CAN RawCommand, zero input + */ + canstats.last_raw_command_us = 0; + set_input(0); + } + if (ts - canstats.last_raw_command_us > TARGET_PERIOD_US) { + // ensure at least 1kHz signal is seen by main code + set_input(last_can_input); + canstats.last_raw_command_us = ts; + } + + sys_can_enable_IRQ(); - // keep summed current for averaging - current.sum += actual_current; - current.count++; + // keep summed current for averaging + current.sum += actual_current; + current.count++; } bool DroneCAN_active(void) { - return canstats.total_commands != 0; + return canstats.total_commands != 0; } #endif // DRONECAN_SUPPORT diff --git a/Src/DroneCAN/filter.c b/Src/DroneCAN/filter.c index 668d28d0..87047be4 100644 --- a/Src/DroneCAN/filter.c +++ b/Src/DroneCAN/filter.c @@ -8,16 +8,16 @@ #include struct Filter2P { - float cutoff_freq; - float sample_freq; - float a1; - float a2; - float b0; - float b1; - float b2; + float cutoff_freq; + float sample_freq; + float a1; + float a2; + float b0; + float b1; + float b2; - float _delay_element_1; - float _delay_element_2; + float _delay_element_1; + float _delay_element_2; }; static struct Filter2P filter; @@ -26,41 +26,41 @@ static struct Filter2P filter; static void Filter2P_setup(float sample_freq, float cutoff_freq) { - // Keep well under Nyquist limit - filter.cutoff_freq = MIN(cutoff_freq, sample_freq * 0.4); - filter.sample_freq = sample_freq; + // Keep well under Nyquist limit + filter.cutoff_freq = MIN(cutoff_freq, sample_freq * 0.4); + filter.sample_freq = sample_freq; - const float fr = filter.sample_freq / filter.cutoff_freq; - const float fr_scaled = M_PI/fr; - const float ohm = tanf(fr_scaled); - const float c = 1.0f+2.0f*cosf(M_PI/4.0f)*ohm + ohm*ohm; + const float fr = filter.sample_freq / filter.cutoff_freq; + const float fr_scaled = M_PI/fr; + const float ohm = tanf(fr_scaled); + const float c = 1.0f+2.0f*cosf(M_PI/4.0f)*ohm + ohm*ohm; - filter.b0 = ohm*ohm/c; - filter.b1 = 2.0f*filter.b0; - filter.b2 = filter.b0; - filter.a1 = 2.0f*(ohm*ohm-1.0f)/c; - filter.a2 = (1.0f-2.0f*cosf(M_PI/4.0f)*ohm+ohm*ohm)/c; + filter.b0 = ohm*ohm/c; + filter.b1 = 2.0f*filter.b0; + filter.b2 = filter.b0; + filter.a1 = 2.0f*(ohm*ohm-1.0f)/c; + filter.a2 = (1.0f-2.0f*cosf(M_PI/4.0f)*ohm+ohm*ohm)/c; } float Filter2P_apply(const float sample, float cutoff_freq, float sample_freq) { - if (cutoff_freq <= 0 || sample_freq <= 0) { - // passthru - return sample; - } + if (cutoff_freq <= 0 || sample_freq <= 0) { + // passthru + return sample; + } - if (filter.cutoff_freq != cutoff_freq || filter.sample_freq != sample_freq) { - Filter2P_setup(sample_freq, cutoff_freq); - filter._delay_element_1 = filter._delay_element_2 = sample * (1.0 / (1 + filter.a1 + filter.a2)); - } + if (filter.cutoff_freq != cutoff_freq || filter.sample_freq != sample_freq) { + Filter2P_setup(sample_freq, cutoff_freq); + filter._delay_element_1 = filter._delay_element_2 = sample * (1.0 / (1 + filter.a1 + filter.a2)); + } - const float delay_element_0 = sample - filter._delay_element_1 * filter.a1 - filter._delay_element_2 * filter.a2; - const float output = delay_element_0 * filter.b0 + filter._delay_element_1 * filter.b1 + filter._delay_element_2 * filter.b2; + const float delay_element_0 = sample - filter._delay_element_1 * filter.a1 - filter._delay_element_2 * filter.a2; + const float output = delay_element_0 * filter.b0 + filter._delay_element_1 * filter.b1 + filter._delay_element_2 * filter.b2; - filter._delay_element_2 = filter._delay_element_1; - filter._delay_element_1 = delay_element_0; + filter._delay_element_2 = filter._delay_element_1; + filter._delay_element_1 = delay_element_0; - return output; + return output; } /* @@ -68,5 +68,5 @@ float Filter2P_apply(const float sample, float cutoff_freq, float sample_freq) */ void abort(void) { - __builtin_unreachable(); + __builtin_unreachable(); } diff --git a/Src/DroneCAN/sys_can.h b/Src/DroneCAN/sys_can.h index c244dbd4..cbbd6ddb 100644 --- a/Src/DroneCAN/sys_can.h +++ b/Src/DroneCAN/sys_can.h @@ -10,19 +10,19 @@ CAN statistics shared by low level and high level code */ struct CANStats { - uint32_t num_commands; - uint32_t num_input; - uint32_t total_commands; - uint32_t num_receive; - uint32_t num_tx_interrupts; - uint32_t num_rx_interrupts; - uint32_t rx_errors; - uint32_t esr; - uint32_t rxframe_error; - int32_t rx_ecode; - uint32_t should_accept; - uint32_t on_receive; - uint64_t last_raw_command_us; + uint32_t num_commands; + uint32_t num_input; + uint32_t total_commands; + uint32_t num_receive; + uint32_t num_tx_interrupts; + uint32_t num_rx_interrupts; + uint32_t rx_errors; + uint32_t esr; + uint32_t rxframe_error; + int32_t rx_ecode; + uint32_t should_accept; + uint32_t on_receive; + uint64_t last_raw_command_us; }; extern struct CANStats canstats; diff --git a/Src/DroneCAN/sys_can_at32.c b/Src/DroneCAN/sys_can_at32.c index a33b89b4..23266e9e 100644 --- a/Src/DroneCAN/sys_can_at32.c +++ b/Src/DroneCAN/sys_can_at32.c @@ -21,7 +21,7 @@ */ void usleep(uint32_t usec) { - delayMicros(usec); + delayMicros(usec); } /* @@ -29,9 +29,9 @@ void usleep(uint32_t usec) */ void sys_can_getUniqueID(uint8_t id[16]) { - const uint8_t *uidbase = (const uint8_t *)0x1FFFF7E8; // 96 bit UID - memcpy(id, uidbase, 12); - memset(&id[12], 0, 4); + const uint8_t *uidbase = (const uint8_t *)0x1FFFF7E8; // 96 bit UID + memcpy(id, uidbase, 12); + memset(&id[12], 0, 4); } /** @@ -41,27 +41,27 @@ void sys_can_getUniqueID(uint8_t id[16]) */ static void can_gpio_config(void) { - gpio_init_type gpio_init_struct; - - crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE); - crm_periph_clock_enable(CRM_IOMUX_PERIPH_CLOCK, TRUE); - gpio_pin_remap_config(CAN1_GMUX_0000,TRUE); // CAN_RX=PA11/CAN_TX=PA12 - // gpio_pin_remap_config(CAN1_GMUX_0010,TRUE); // CAN_RX=PB8/CAN_TX=PB9 - - gpio_default_para_init(&gpio_init_struct); - /* can tx pin */ - gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER; - gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL; - gpio_init_struct.gpio_mode = GPIO_MODE_MUX; - gpio_init_struct.gpio_pins = GPIO_PINS_12; - gpio_init_struct.gpio_pull = GPIO_PULL_NONE; - gpio_init(GPIOA, &gpio_init_struct); - /* can rx pin */ - gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER; - gpio_init_struct.gpio_mode = GPIO_MODE_INPUT; - gpio_init_struct.gpio_pins = GPIO_PINS_11; - gpio_init_struct.gpio_pull = GPIO_PULL_UP; - gpio_init(GPIOA, &gpio_init_struct); + gpio_init_type gpio_init_struct; + + crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_IOMUX_PERIPH_CLOCK, TRUE); + gpio_pin_remap_config(CAN1_GMUX_0000,TRUE); // CAN_RX=PA11/CAN_TX=PA12 + // gpio_pin_remap_config(CAN1_GMUX_0010,TRUE); // CAN_RX=PB8/CAN_TX=PB9 + + gpio_default_para_init(&gpio_init_struct); + /* can tx pin */ + gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER; + gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL; + gpio_init_struct.gpio_mode = GPIO_MODE_MUX; + gpio_init_struct.gpio_pins = GPIO_PINS_12; + gpio_init_struct.gpio_pull = GPIO_PULL_NONE; + gpio_init(GPIOA, &gpio_init_struct); + /* can rx pin */ + gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER; + gpio_init_struct.gpio_mode = GPIO_MODE_INPUT; + gpio_init_struct.gpio_pins = GPIO_PINS_11; + gpio_init_struct.gpio_pull = GPIO_PULL_UP; + gpio_init(GPIOA, &gpio_init_struct); } /* @@ -69,78 +69,78 @@ static void can_gpio_config(void) */ void sys_can_init(void) { - can_gpio_config(); - - can_base_type can_base_struct; - can_baudrate_type can_baudrate_struct; - - crm_periph_clock_enable(CRM_CAN1_PERIPH_CLOCK, TRUE); - /* can base init */ - can_default_para_init(&can_base_struct); - can_base_struct.mode_selection = CAN_MODE_COMMUNICATE; - can_base_struct.ttc_enable = FALSE; - can_base_struct.aebo_enable = TRUE; - can_base_struct.aed_enable = TRUE; - can_base_struct.prsf_enable = FALSE; - can_base_struct.mdrsel_selection = CAN_DISCARDING_FIRST_RECEIVED; - can_base_struct.mmssr_selection = CAN_SENDING_BY_REQUEST; - can_base_init(CAN1, &can_base_struct); - - crm_clocks_freq_type clocks; - crm_clocks_freq_get(&clocks); - - /* can baudrate, set baudrate = pclk/(baudrate_div *(1 + bts1_size + bts2_size)) */ - can_baudrate_struct.baudrate_div = 6; - can_baudrate_struct.rsaw_size = CAN_RSAW_3TQ; - can_baudrate_struct.bts1_size = CAN_BTS1_8TQ; - can_baudrate_struct.bts2_size = CAN_BTS2_3TQ; - can_baudrate_set(CAN1, &can_baudrate_struct); - - /* can filter init */ - can_filter_init_type can_filter_init_struct; - can_filter_init_struct.filter_activate_enable = TRUE; - can_filter_init_struct.filter_fifo = CAN_FILTER_FIFO0; - can_filter_init_struct.filter_number = 0; - can_filter_init_struct.filter_bit = CAN_FILTER_32BIT; - can_filter_init_struct.filter_id_high = 0; - can_filter_init_struct.filter_id_low = 0; - can_filter_init_struct.filter_mask_high = 0; - can_filter_init_struct.filter_mask_low = 0; - can_filter_init(CAN1, &can_filter_init_struct); - - can_filter_init_struct.filter_fifo = CAN_FILTER_FIFO1; - can_filter_init_struct.filter_number = 1; - can_filter_init(CAN1, &can_filter_init_struct); - - // interrupt priorities, needs to be a higher number (lower priority) than - // motor control interrupts - NVIC_SetPriority(CAN1_SE_IRQn, 5); - NVIC_SetPriority(CAN1_RX0_IRQn, 5); - NVIC_SetPriority(CAN1_RX1_IRQn, 5); - NVIC_SetPriority(CAN1_TX_IRQn, 5); - - /* interrupt enable */ - can_interrupt_enable(CAN1, CAN_TCIEN_INT, TRUE); - can_interrupt_enable(CAN1, CAN_RF0MIEN_INT, TRUE); - can_interrupt_enable(CAN1, CAN_RF1MIEN_INT, TRUE); - can_interrupt_enable(CAN1, CAN_ETRIEN_INT, TRUE); - can_interrupt_enable(CAN1, CAN_EOIEN_INT, TRUE); + can_gpio_config(); + + can_base_type can_base_struct; + can_baudrate_type can_baudrate_struct; + + crm_periph_clock_enable(CRM_CAN1_PERIPH_CLOCK, TRUE); + /* can base init */ + can_default_para_init(&can_base_struct); + can_base_struct.mode_selection = CAN_MODE_COMMUNICATE; + can_base_struct.ttc_enable = FALSE; + can_base_struct.aebo_enable = TRUE; + can_base_struct.aed_enable = TRUE; + can_base_struct.prsf_enable = FALSE; + can_base_struct.mdrsel_selection = CAN_DISCARDING_FIRST_RECEIVED; + can_base_struct.mmssr_selection = CAN_SENDING_BY_REQUEST; + can_base_init(CAN1, &can_base_struct); + + crm_clocks_freq_type clocks; + crm_clocks_freq_get(&clocks); + + /* can baudrate, set baudrate = pclk/(baudrate_div *(1 + bts1_size + bts2_size)) */ + can_baudrate_struct.baudrate_div = 6; + can_baudrate_struct.rsaw_size = CAN_RSAW_3TQ; + can_baudrate_struct.bts1_size = CAN_BTS1_8TQ; + can_baudrate_struct.bts2_size = CAN_BTS2_3TQ; + can_baudrate_set(CAN1, &can_baudrate_struct); + + /* can filter init */ + can_filter_init_type can_filter_init_struct; + can_filter_init_struct.filter_activate_enable = TRUE; + can_filter_init_struct.filter_fifo = CAN_FILTER_FIFO0; + can_filter_init_struct.filter_number = 0; + can_filter_init_struct.filter_bit = CAN_FILTER_32BIT; + can_filter_init_struct.filter_id_high = 0; + can_filter_init_struct.filter_id_low = 0; + can_filter_init_struct.filter_mask_high = 0; + can_filter_init_struct.filter_mask_low = 0; + can_filter_init(CAN1, &can_filter_init_struct); + + can_filter_init_struct.filter_fifo = CAN_FILTER_FIFO1; + can_filter_init_struct.filter_number = 1; + can_filter_init(CAN1, &can_filter_init_struct); + + // interrupt priorities, needs to be a higher number (lower priority) than + // motor control interrupts + NVIC_SetPriority(CAN1_SE_IRQn, 5); + NVIC_SetPriority(CAN1_RX0_IRQn, 5); + NVIC_SetPriority(CAN1_RX1_IRQn, 5); + NVIC_SetPriority(CAN1_TX_IRQn, 5); + + /* interrupt enable */ + can_interrupt_enable(CAN1, CAN_TCIEN_INT, TRUE); + can_interrupt_enable(CAN1, CAN_RF0MIEN_INT, TRUE); + can_interrupt_enable(CAN1, CAN_RF1MIEN_INT, TRUE); + can_interrupt_enable(CAN1, CAN_ETRIEN_INT, TRUE); + can_interrupt_enable(CAN1, CAN_EOIEN_INT, TRUE); } void sys_can_enable_IRQ(void) { - NVIC_EnableIRQ(CAN1_SE_IRQn); - NVIC_EnableIRQ(CAN1_RX0_IRQn); - NVIC_EnableIRQ(CAN1_RX1_IRQn); - NVIC_EnableIRQ(CAN1_TX_IRQn); + NVIC_EnableIRQ(CAN1_SE_IRQn); + NVIC_EnableIRQ(CAN1_RX0_IRQn); + NVIC_EnableIRQ(CAN1_RX1_IRQn); + NVIC_EnableIRQ(CAN1_TX_IRQn); } void sys_can_disable_IRQ(void) { - NVIC_DisableIRQ(CAN1_SE_IRQn); - NVIC_DisableIRQ(CAN1_RX0_IRQn); - NVIC_DisableIRQ(CAN1_RX1_IRQn); - NVIC_DisableIRQ(CAN1_TX_IRQn); + NVIC_DisableIRQ(CAN1_SE_IRQn); + NVIC_DisableIRQ(CAN1_RX0_IRQn); + NVIC_DisableIRQ(CAN1_RX1_IRQn); + NVIC_DisableIRQ(CAN1_TX_IRQn); } /* @@ -149,24 +149,24 @@ void sys_can_disable_IRQ(void) */ int16_t sys_can_transmit(const CanardCANFrame* txf) { - can_tx_message_type tx_message_struct; - if (txf->id & CANARD_CAN_FRAME_EFF) { - tx_message_struct.id_type = CAN_ID_EXTENDED; - tx_message_struct.standard_id = 0; - tx_message_struct.extended_id = txf->id & CANARD_CAN_EXT_ID_MASK; - } else { - tx_message_struct.id_type = CAN_ID_STANDARD; - tx_message_struct.standard_id = txf->id & CANARD_CAN_STD_ID_MASK; - tx_message_struct.extended_id = 0; - } - tx_message_struct.frame_type = CAN_TFT_DATA; - tx_message_struct.dlc = txf->data_len; - memcpy(tx_message_struct.data, txf->data, txf->data_len); - const uint8_t transmit_mailbox = can_message_transmit(CAN1, &tx_message_struct); - if (transmit_mailbox == CAN_TX_STATUS_NO_EMPTY) { - return 0; - } - return 1; + can_tx_message_type tx_message_struct; + if (txf->id & CANARD_CAN_FRAME_EFF) { + tx_message_struct.id_type = CAN_ID_EXTENDED; + tx_message_struct.standard_id = 0; + tx_message_struct.extended_id = txf->id & CANARD_CAN_EXT_ID_MASK; + } else { + tx_message_struct.id_type = CAN_ID_STANDARD; + tx_message_struct.standard_id = txf->id & CANARD_CAN_STD_ID_MASK; + tx_message_struct.extended_id = 0; + } + tx_message_struct.frame_type = CAN_TFT_DATA; + tx_message_struct.dlc = txf->data_len; + memcpy(tx_message_struct.data, txf->data, txf->data_len); + const uint8_t transmit_mailbox = can_message_transmit(CAN1, &tx_message_struct); + if (transmit_mailbox == CAN_TX_STATUS_NO_EMPTY) { + return 0; + } + return 1; } /* @@ -175,28 +175,28 @@ int16_t sys_can_transmit(const CanardCANFrame* txf) */ int16_t sys_can_receive(CanardCANFrame *rx_frame) { - can_rx_message_type frm; - bool have_frame = false; - if (CAN1->rf0_bit.rf0mn) { - can_message_receive(CAN1, CAN_RX_FIFO0, &frm); - have_frame = true; - } else if (CAN1->rf1_bit.rf1mn) { - can_message_receive(CAN1, CAN_RX_FIFO1, &frm); - have_frame = true; - } - if (!have_frame) { - return 0; - } - if (frm.id_type == CAN_ID_EXTENDED) { - rx_frame->id = frm.extended_id | CANARD_CAN_FRAME_EFF; - } else if (frm.id_type == CAN_ID_STANDARD) { - rx_frame->id = frm.standard_id; - } else { - return 0; - } - rx_frame->data_len = frm.dlc; - memcpy(rx_frame->data, frm.data, rx_frame->data_len); - return 1; + can_rx_message_type frm; + bool have_frame = false; + if (CAN1->rf0_bit.rf0mn) { + can_message_receive(CAN1, CAN_RX_FIFO0, &frm); + have_frame = true; + } else if (CAN1->rf1_bit.rf1mn) { + can_message_receive(CAN1, CAN_RX_FIFO1, &frm); + have_frame = true; + } + if (!have_frame) { + return 0; + } + if (frm.id_type == CAN_ID_EXTENDED) { + rx_frame->id = frm.extended_id | CANARD_CAN_FRAME_EFF; + } else if (frm.id_type == CAN_ID_STANDARD) { + rx_frame->id = frm.standard_id; + } else { + return 0; + } + rx_frame->data_len = frm.dlc; + memcpy(rx_frame->data, frm.data, rx_frame->data_len); + return 1; } /** @@ -206,18 +206,18 @@ int16_t sys_can_receive(CanardCANFrame *rx_frame) */ void CAN1_RX0_IRQHandler(void) { - DroneCAN_receiveFrame(); + DroneCAN_receiveFrame(); } void CAN1_RX1_IRQHandler(void) { - DroneCAN_receiveFrame(); + DroneCAN_receiveFrame(); } void CAN1_TX_IRQHandler(void) { - CAN1->tsts = CAN_TSTS_TM0TCF_VAL | CAN_TSTS_TM1TCF_VAL | CAN_TSTS_TM2TCF_VAL; - DroneCAN_processTxQueue(); + CAN1->tsts = CAN_TSTS_TM0TCF_VAL | CAN_TSTS_TM1TCF_VAL | CAN_TSTS_TM2TCF_VAL; + DroneCAN_processTxQueue(); } /** @@ -228,29 +228,27 @@ void CAN1_TX_IRQHandler(void) void CAN1_SE_IRQHandler(void) { __IO uint32_t err_index = 0; - if (CAN1->ests_bit.etr) - { + if (CAN1->ests_bit.etr) { err_index = CAN1->ests & 0x70; CAN1->msts = CAN_MSTS_EOIF_VAL; CAN1->ests = 0; /* error type is stuff error */ - if (err_index == 0x00000010) - { - /* when stuff error occur: in order to ensure communication normally, - user must restart can or send a frame of highest priority message here - */ + if (err_index == 0x00000010) { + /* when stuff error occur: in order to ensure communication normally, + user must restart can or send a frame of highest priority message here + */ } } } uint32_t get_rtc_backup_register(uint8_t idx) { - return ertc_bpr_data_read(idx); + return ertc_bpr_data_read(idx); } void set_rtc_backup_register(uint8_t idx, uint32_t value) { - ertc_bpr_data_write(idx, value); + ertc_bpr_data_write(idx, value); } /* @@ -258,25 +256,25 @@ void set_rtc_backup_register(uint8_t idx, uint32_t value) */ void setup_portpin(uint16_t portpin, bool enable) { - const uint8_t port = portpin >> 8; - const uint8_t pin = portpin & 0xff; - const uint32_t pinshift = 1U<scr = pinshift; - } else { - pport->clr = pinshift; - } - - gpio_init_type gpio_init_struct; - - gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER; - gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL; - gpio_init_struct.gpio_mode = GPIO_MODE_OUTPUT; - gpio_init_struct.gpio_pins = pinshift; - gpio_init_struct.gpio_pull = GPIO_PULL_NONE; - gpio_init(pport, &gpio_init_struct); + const uint8_t port = portpin >> 8; + const uint8_t pin = portpin & 0xff; + const uint32_t pinshift = 1U<scr = pinshift; + } else { + pport->clr = pinshift; + } + + gpio_init_type gpio_init_struct; + + gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER; + gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL; + gpio_init_struct.gpio_mode = GPIO_MODE_OUTPUT; + gpio_init_struct.gpio_pins = pinshift; + gpio_init_struct.gpio_pull = GPIO_PULL_NONE; + gpio_init(pport, &gpio_init_struct); } #endif // DRONECAN_SUPPORT && defined(ARTERY) diff --git a/Src/DroneCAN/sys_can_stm32.c b/Src/DroneCAN/sys_can_stm32.c index 224bf52c..97a4d100 100644 --- a/Src/DroneCAN/sys_can_stm32.c +++ b/Src/DroneCAN/sys_can_stm32.c @@ -20,7 +20,7 @@ */ void usleep(uint32_t usec) { - delayMicros(usec); + delayMicros(usec); } /* @@ -28,12 +28,12 @@ void usleep(uint32_t usec) */ void sys_can_getUniqueID(uint8_t id[16]) { - const uint8_t *uidbase = (const uint8_t *)UID_BASE; - memcpy(id, uidbase, 12); + const uint8_t *uidbase = (const uint8_t *)UID_BASE; + memcpy(id, uidbase, 12); - // put CPU ID in last 4 bytes, handy for knowing the exact MCU we are on - const uint32_t cpuid = SCB->CPUID; - memcpy(&id[12], &cpuid, 4); + // put CPU ID in last 4 bytes, handy for knowing the exact MCU we are on + const uint32_t cpuid = SCB->CPUID; + memcpy(&id[12], &cpuid, 4); } /* @@ -41,7 +41,7 @@ void sys_can_getUniqueID(uint8_t id[16]) */ static void handleTxMailboxInterrupt(uint8_t mbox, bool txok) { - DroneCAN_processTxQueue(); + DroneCAN_processTxQueue(); } /* @@ -49,11 +49,11 @@ static void handleTxMailboxInterrupt(uint8_t mbox, bool txok) */ static void pollErrorFlagsFromISR() { - const uint8_t lec = (uint8_t)((BXCAN->ESR & CANARD_STM32_CAN_ESR_LEC_MASK) >> CANARD_STM32_CAN_ESR_LEC_SHIFT); - if (lec != 0) { - canstats.esr = BXCAN->ESR; // Record error status - BXCAN->ESR = 0; - } + const uint8_t lec = (uint8_t)((BXCAN->ESR & CANARD_STM32_CAN_ESR_LEC_MASK) >> CANARD_STM32_CAN_ESR_LEC_SHIFT); + if (lec != 0) { + canstats.esr = BXCAN->ESR; // Record error status + BXCAN->ESR = 0; + } } /* @@ -61,26 +61,26 @@ static void pollErrorFlagsFromISR() */ static void handleTxInterrupt(void) { - canstats.num_tx_interrupts++; - - // TXOK == false means that there was a hardware failure - if (BXCAN->TSR & CANARD_STM32_CAN_TSR_RQCP0) { - const bool txok = BXCAN->TSR & CANARD_STM32_CAN_TSR_TXOK0; - BXCAN->TSR = CANARD_STM32_CAN_TSR_RQCP0; - handleTxMailboxInterrupt(0, txok); - } - if (BXCAN->TSR & CANARD_STM32_CAN_TSR_RQCP1) { - const bool txok = BXCAN->TSR & CANARD_STM32_CAN_TSR_TXOK1; - BXCAN->TSR = CANARD_STM32_CAN_TSR_RQCP1; - handleTxMailboxInterrupt(1, txok); - } - if (BXCAN->TSR & CANARD_STM32_CAN_TSR_RQCP2) { - const bool txok = BXCAN->TSR & CANARD_STM32_CAN_TSR_TXOK2; - BXCAN->TSR = CANARD_STM32_CAN_TSR_RQCP2; - handleTxMailboxInterrupt(2, txok); - } - - pollErrorFlagsFromISR(); + canstats.num_tx_interrupts++; + + // TXOK == false means that there was a hardware failure + if (BXCAN->TSR & CANARD_STM32_CAN_TSR_RQCP0) { + const bool txok = BXCAN->TSR & CANARD_STM32_CAN_TSR_TXOK0; + BXCAN->TSR = CANARD_STM32_CAN_TSR_RQCP0; + handleTxMailboxInterrupt(0, txok); + } + if (BXCAN->TSR & CANARD_STM32_CAN_TSR_RQCP1) { + const bool txok = BXCAN->TSR & CANARD_STM32_CAN_TSR_TXOK1; + BXCAN->TSR = CANARD_STM32_CAN_TSR_RQCP1; + handleTxMailboxInterrupt(1, txok); + } + if (BXCAN->TSR & CANARD_STM32_CAN_TSR_RQCP2) { + const bool txok = BXCAN->TSR & CANARD_STM32_CAN_TSR_TXOK2; + BXCAN->TSR = CANARD_STM32_CAN_TSR_RQCP2; + handleTxMailboxInterrupt(2, txok); + } + + pollErrorFlagsFromISR(); } /* @@ -88,23 +88,23 @@ static void handleTxInterrupt(void) */ static void handleRxInterrupt(uint8_t fifo_index) { - canstats.num_rx_interrupts++; + canstats.num_rx_interrupts++; - volatile uint32_t* const rfr_reg = (fifo_index == 0) ? &BXCAN->RF0R : &BXCAN->RF1R; - if ((*rfr_reg & CANARD_STM32_CAN_RFR_FMP_MASK) == 0) { - return; - } + volatile uint32_t* const rfr_reg = (fifo_index == 0) ? &BXCAN->RF0R : &BXCAN->RF1R; + if ((*rfr_reg & CANARD_STM32_CAN_RFR_FMP_MASK) == 0) { + return; + } - /* - * Register overflow as a hardware error - */ - if ((*rfr_reg & CANARD_STM32_CAN_RFR_FOVR) != 0) { - canstats.rx_errors++; - } + /* + * Register overflow as a hardware error + */ + if ((*rfr_reg & CANARD_STM32_CAN_RFR_FOVR) != 0) { + canstats.rx_errors++; + } - DroneCAN_receiveFrame(); + DroneCAN_receiveFrame(); - pollErrorFlagsFromISR(); + pollErrorFlagsFromISR(); } /* @@ -112,17 +112,17 @@ static void handleRxInterrupt(uint8_t fifo_index) */ void CAN1_RX0_IRQHandler(void) { - handleRxInterrupt(0); + handleRxInterrupt(0); } void CAN1_RX1_IRQHandler(void) { - handleRxInterrupt(1); + handleRxInterrupt(1); } void CAN1_TX_IRQHandler(void) { - handleTxInterrupt(); + handleTxInterrupt(); } /* @@ -131,7 +131,7 @@ void CAN1_TX_IRQHandler(void) */ int16_t sys_can_transmit(const CanardCANFrame* txf) { - return canardSTM32Transmit(txf); + return canardSTM32Transmit(txf); } /* @@ -140,7 +140,7 @@ int16_t sys_can_transmit(const CanardCANFrame* txf) */ int16_t sys_can_receive(CanardCANFrame *rx_frame) { - return canardSTM32Receive(rx_frame); + return canardSTM32Receive(rx_frame); } /* @@ -148,9 +148,9 @@ int16_t sys_can_receive(CanardCANFrame *rx_frame) */ void sys_can_disable_IRQ(void) { - NVIC_DisableIRQ(CAN1_RX0_IRQn); - NVIC_DisableIRQ(CAN1_RX1_IRQn); - NVIC_DisableIRQ(CAN1_TX_IRQn); + NVIC_DisableIRQ(CAN1_RX0_IRQn); + NVIC_DisableIRQ(CAN1_RX1_IRQn); + NVIC_DisableIRQ(CAN1_TX_IRQn); } /* @@ -158,9 +158,9 @@ void sys_can_disable_IRQ(void) */ void sys_can_enable_IRQ(void) { - NVIC_EnableIRQ(CAN1_RX0_IRQn); - NVIC_EnableIRQ(CAN1_RX1_IRQn); - NVIC_EnableIRQ(CAN1_TX_IRQn); + NVIC_EnableIRQ(CAN1_RX0_IRQn); + NVIC_EnableIRQ(CAN1_RX1_IRQn); + NVIC_EnableIRQ(CAN1_TX_IRQn); } /* @@ -173,70 +173,70 @@ void sys_can_enable_IRQ(void) */ void sys_can_init(void) { - /* - setup CAN RX and TX pins - */ - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_CAN1); - LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOA); - - // assume PA11/PA12 for now - LL_GPIO_InitTypeDef GPIO_InitStruct = {0}; - GPIO_InitStruct.Pin = LL_GPIO_PIN_11 | LL_GPIO_PIN_12; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH; - GPIO_InitStruct.Alternate = 9; // AF9==CAN - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - - LL_RCC_ClocksTypeDef Clocks; - LL_RCC_GetSystemClocksFreq(&Clocks); - - /* - work out timing for 1MBps CAN - */ - CanardSTM32CANTimings timings; + /* + setup CAN RX and TX pins + */ + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_CAN1); + LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOA); + + // assume PA11/PA12 for now + LL_GPIO_InitTypeDef GPIO_InitStruct = {0}; + GPIO_InitStruct.Pin = LL_GPIO_PIN_11 | LL_GPIO_PIN_12; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.Alternate = 9; // AF9==CAN + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + LL_RCC_ClocksTypeDef Clocks; + LL_RCC_GetSystemClocksFreq(&Clocks); + + /* + work out timing for 1MBps CAN + */ + CanardSTM32CANTimings timings; #if 0 - // use this to get timings for a new board - canardSTM32ComputeCANTimings(Clocks.PCLK1_Frequency, 1000000, &timings); + // use this to get timings for a new board + canardSTM32ComputeCANTimings(Clocks.PCLK1_Frequency, 1000000, &timings); #else - switch (Clocks.PCLK1_Frequency) { - case 80000000UL: - timings.bit_rate_prescaler = 8; - timings.bit_segment_1 = 8; - timings.bit_segment_2 = 1; - timings.max_resynchronization_jump_width = 1; - break; - default: - // need to port to this board - while (true) { - delayMicros(1000); - } - break; + switch (Clocks.PCLK1_Frequency) { + case 80000000UL: + timings.bit_rate_prescaler = 8; + timings.bit_segment_1 = 8; + timings.bit_segment_2 = 1; + timings.max_resynchronization_jump_width = 1; + break; + default: + // need to port to this board + while (true) { + delayMicros(1000); } + break; + } #endif - canardSTM32Init(&timings, CanardSTM32IfaceModeNormal); + canardSTM32Init(&timings, CanardSTM32IfaceModeNormal); - /* - enable interrupt for CAN receive and transmit - */ - NVIC_SetPriority(CAN1_RX0_IRQn, 5); - NVIC_SetPriority(CAN1_RX1_IRQn, 5); - NVIC_SetPriority(CAN1_TX_IRQn, 5); - BXCAN->IER = CANARD_STM32_CAN_IER_FMPIE0 | CANARD_STM32_CAN_IER_FMPIE1 | CANARD_STM32_CAN_IER_TMEIE; + /* + enable interrupt for CAN receive and transmit + */ + NVIC_SetPriority(CAN1_RX0_IRQn, 5); + NVIC_SetPriority(CAN1_RX1_IRQn, 5); + NVIC_SetPriority(CAN1_TX_IRQn, 5); + BXCAN->IER = CANARD_STM32_CAN_IER_FMPIE0 | CANARD_STM32_CAN_IER_FMPIE1 | CANARD_STM32_CAN_IER_TMEIE; } uint32_t get_rtc_backup_register(uint8_t idx) { - const volatile uint32_t *bkp = &RTC->BKP0R; - return bkp[idx]; + const volatile uint32_t *bkp = &RTC->BKP0R; + return bkp[idx]; } void set_rtc_backup_register(uint8_t idx, uint32_t value) { - volatile uint32_t *bkp = &RTC->BKP0R; - bkp[idx] = value; + volatile uint32_t *bkp = &RTC->BKP0R; + bkp[idx] = value; } /* @@ -244,23 +244,23 @@ void set_rtc_backup_register(uint8_t idx, uint32_t value) */ void setup_portpin(uint16_t portpin, bool enable) { - const uint8_t port = portpin >> 8; - const uint8_t pin = portpin & 0xff; - const uint32_t pinshift = 1U<> 8; + const uint8_t pin = portpin & 0xff; + const uint32_t pinshift = 1U<> 5; - if ((dshot_frametime > dshot_frametime_low) && (dshot_frametime < dshot_frametime_high)) { - signaltimeout = 0; - for (int i = 0; i < 16; i++) { - // note that dma_buffer[] is uint32_t, we cast the difference to uint16_t to handle - // timer wrap correctly - const uint16_t pdiff = dma_buffer[(i << 1) + 1] - dma_buffer[(i << 1)]; - dpulse[i] = (pdiff > halfpulsetime); - } - uint8_t calcCRC = ((dpulse[0] ^ dpulse[4] ^ dpulse[8]) << 3 | (dpulse[1] ^ dpulse[5] ^ dpulse[9]) << 2 | (dpulse[2] ^ dpulse[6] ^ dpulse[10]) << 1 | (dpulse[3] ^ dpulse[7] ^ dpulse[11])); - uint8_t checkCRC = (dpulse[12] << 3 | dpulse[13] << 2 | dpulse[14] << 1 | dpulse[15]); + dshot_frametime = dma_buffer[31] - dma_buffer[0]; + halfpulsetime = dshot_frametime >> 5; + if ((dshot_frametime > dshot_frametime_low) && (dshot_frametime < dshot_frametime_high)) { + signaltimeout = 0; + for (int i = 0; i < 16; i++) { + // note that dma_buffer[] is uint32_t, we cast the difference to uint16_t to handle + // timer wrap correctly + const uint16_t pdiff = dma_buffer[(i << 1) + 1] - dma_buffer[(i << 1)]; + dpulse[i] = (pdiff > halfpulsetime); + } + uint8_t calcCRC = ((dpulse[0] ^ dpulse[4] ^ dpulse[8]) << 3 | (dpulse[1] ^ dpulse[5] ^ dpulse[9]) << 2 | (dpulse[2] ^ dpulse[6] ^ dpulse[10]) << 1 | (dpulse[3] ^ dpulse[7] ^ dpulse[11])); + uint8_t checkCRC = (dpulse[12] << 3 | dpulse[13] << 2 | dpulse[14] << 1 | dpulse[15]); - if (!armed) { - if (dshot_telemetry == 0) { - if (getInputPinState()) { // if the pin is high for 100 checks between - // signal pulses its inverted - high_pin_count++; - if (high_pin_count > 100) { - dshot_telemetry = 1; - } - } - } - } - if (dshot_telemetry) { - checkCRC = ~checkCRC + 16; + if (!armed) { + if (dshot_telemetry == 0) { + if (getInputPinState()) { // if the pin is high for 100 checks between + // signal pulses its inverted + high_pin_count++; + if (high_pin_count > 100) { + dshot_telemetry = 1; + } } + } + } + if (dshot_telemetry) { + checkCRC = ~checkCRC + 16; + } - int tocheck = (dpulse[0] << 10 | dpulse[1] << 9 | dpulse[2] << 8 | dpulse[3] << 7 | dpulse[4] << 6 | dpulse[5] << 5 | dpulse[6] << 4 | dpulse[7] << 3 | dpulse[8] << 2 | dpulse[9] << 1 | dpulse[10]); + int tocheck = (dpulse[0] << 10 | dpulse[1] << 9 | dpulse[2] << 8 | dpulse[3] << 7 | dpulse[4] << 6 | dpulse[5] << 5 | dpulse[6] << 4 | dpulse[7] << 3 | dpulse[8] << 2 | dpulse[9] << 1 | dpulse[10]); - if (calcCRC == checkCRC) { - signaltimeout = 0; - dshot_goodcounts++; - if (dpulse[11] == 1) { - send_telemetry = 1; - } - if (tocheck > 47) { - if (EDT_ARMED) { - newinput = tocheck; - dshotcommand = 0; - command_count = 0; - return; - } - } + if (calcCRC == checkCRC) { + signaltimeout = 0; + dshot_goodcounts++; + if (dpulse[11] == 1) { + send_telemetry = 1; + } + if (tocheck > 47) { + if (EDT_ARMED) { + newinput = tocheck; + dshotcommand = 0; + command_count = 0; + return; + } + } - if ((tocheck <= 47) && (tocheck > 0)) { - newinput = 0; - dshotcommand = tocheck; // todo - } - if (tocheck == 0) { - if (EDT_ARM_ENABLE == 1) { - EDT_ARMED = 0; - } + if ((tocheck <= 47) && (tocheck > 0)) { + newinput = 0; + dshotcommand = tocheck; // todo + } + if (tocheck == 0) { + if (EDT_ARM_ENABLE == 1) { + EDT_ARMED = 0; + } #if DRONECAN_SUPPORT - if (DroneCAN_active()) { - // allow DroneCAN to override DShot input - return; - } + if (DroneCAN_active()) { + // allow DroneCAN to override DShot input + return; + } #endif - newinput = 0; - dshotcommand = 0; - command_count = 0; - } + newinput = 0; + dshotcommand = 0; + command_count = 0; + } - if ((dshotcommand > 0) && (running == 0) && armed) { - if (dshotcommand != last_command) { - last_command = dshotcommand; - command_count = 0; - } - if (dshotcommand < 5) { // beacons - command_count = 6; // go on right away - } - command_count++; - if (command_count >= 6) { - command_count = 0; - switch (dshotcommand) { // todo + if ((dshotcommand > 0) && (running == 0) && armed) { + if (dshotcommand != last_command) { + last_command = dshotcommand; + command_count = 0; + } + if (dshotcommand < 5) { // beacons + command_count = 6; // go on right away + } + command_count++; + if (command_count >= 6) { + command_count = 0; + switch (dshotcommand) { // todo - case 1: - play_tone_flag = 1; - break; - case 2: - play_tone_flag = 2; - break; - case 3: - play_tone_flag = 3; - break; - case 4: - play_tone_flag = 4; - break; - case 5: - play_tone_flag = 5; - break; - case 7: - eepromBuffer.dir_reversed = 0; - forward = 1 - eepromBuffer.dir_reversed; - // play_tone_flag = 1; - break; - case 8: - eepromBuffer.dir_reversed = 1; - forward = 1 - eepromBuffer.dir_reversed; - // play_tone_flag = 2; - break; - case 9: - eepromBuffer.bi_direction = 0; - break; - case 10: - eepromBuffer.bi_direction = 1; - break; - case 12: - saveEEpromSettings(); - play_tone_flag = 1 + eepromBuffer.dir_reversed; - // NVIC_SystemReset(); - break; - case 13: - dshot_extended_telemetry = 1; - send_extended_dshot = 0b111000000000; - if (EDT_ARM_ENABLE == 1) { - EDT_ARMED = 1; - } - break; - case 14: - dshot_extended_telemetry = 0; - send_extended_dshot = 0b111011111111; - // make_dshot_package(); - break; - case 20: - forward = 1 - eepromBuffer.dir_reversed; - break; - case 21: - forward = eepromBuffer.dir_reversed; - break; - } - last_dshot_command = dshotcommand; - dshotcommand = 0; - } + case 1: + play_tone_flag = 1; + break; + case 2: + play_tone_flag = 2; + break; + case 3: + play_tone_flag = 3; + break; + case 4: + play_tone_flag = 4; + break; + case 5: + play_tone_flag = 5; + break; + case 7: + eepromBuffer.dir_reversed = 0; + forward = 1 - eepromBuffer.dir_reversed; + // play_tone_flag = 1; + break; + case 8: + eepromBuffer.dir_reversed = 1; + forward = 1 - eepromBuffer.dir_reversed; + // play_tone_flag = 2; + break; + case 9: + eepromBuffer.bi_direction = 0; + break; + case 10: + eepromBuffer.bi_direction = 1; + break; + case 12: + saveEEpromSettings(); + play_tone_flag = 1 + eepromBuffer.dir_reversed; + // NVIC_SystemReset(); + break; + case 13: + dshot_extended_telemetry = 1; + send_extended_dshot = 0b111000000000; + if (EDT_ARM_ENABLE == 1) { + EDT_ARMED = 1; } - } else { - dshot_badcounts++; + break; + case 14: + dshot_extended_telemetry = 0; + send_extended_dshot = 0b111011111111; + // make_dshot_package(); + break; + case 20: + forward = 1 - eepromBuffer.dir_reversed; + break; + case 21: + forward = eepromBuffer.dir_reversed; + break; + } + last_dshot_command = dshotcommand; + dshotcommand = 0; } + } + } else { + dshot_badcounts++; } + } } void make_dshot_package(uint16_t com_time) { - if (send_extended_dshot > 0) { - dshot_full_number = send_extended_dshot; - send_extended_dshot = 0; - } else { - if (!running) { - com_time = 65535; - } - // calculate shift amount for data in format eee mmm mmm mmm, first 1 found - // in first seven bits of data determines shift amount - // this allows for a range of up to 65408 microseconds which would be - // shifted 0b111 (eee) or 7 times. - for (int i = 15; i >= 9; i--) { - if (com_time >> i == 1) { - shift_amount = i + 1 - 9; - break; - } else { - shift_amount = 0; - } - } - // shift the commutation time to allow for expanded range and put shift - // amount in first three bits - dshot_full_number = ((shift_amount << 9) | (com_time >> shift_amount)); + if (send_extended_dshot > 0) { + dshot_full_number = send_extended_dshot; + send_extended_dshot = 0; + } else { + if (!running) { + com_time = 65535; } - // calculate checksum - uint16_t csum = 0; - uint16_t csum_data = dshot_full_number; - for (int i = 0; i < 3; i++) { - csum ^= csum_data; // xor data by nibbles - csum_data >>= 4; + // calculate shift amount for data in format eee mmm mmm mmm, first 1 found + // in first seven bits of data determines shift amount + // this allows for a range of up to 65408 microseconds which would be + // shifted 0b111 (eee) or 7 times. + for (int i = 15; i >= 9; i--) { + if (com_time >> i == 1) { + shift_amount = i + 1 - 9; + break; + } else { + shift_amount = 0; + } } - csum = ~csum; // invert it - csum &= 0xf; + // shift the commutation time to allow for expanded range and put shift + // amount in first three bits + dshot_full_number = ((shift_amount << 9) | (com_time >> shift_amount)); + } + // calculate checksum + uint16_t csum = 0; + uint16_t csum_data = dshot_full_number; + for (int i = 0; i < 3; i++) { + csum ^= csum_data; // xor data by nibbles + csum_data >>= 4; + } + csum = ~csum; // invert it + csum &= 0xf; - dshot_full_number = (dshot_full_number << 4) | csum; // put checksum at the end of 12 bit dshot number + dshot_full_number = (dshot_full_number << 4) | csum; // put checksum at the end of 12 bit dshot number - // GCR RLL encode 16 to 20 bit + // GCR RLL encode 16 to 20 bit - gcrnumber = gcr_encode_table[(dshot_full_number >> 12)] - << 15 // first set of four digits - | gcr_encode_table[(((1 << 4) - 1) & (dshot_full_number >> 8))] - << 10 // 2nd set of 4 digits - | gcr_encode_table[(((1 << 4) - 1) & (dshot_full_number >> 4))] - << 5 // 3rd set of four digits - | gcr_encode_table[(((1 << 4) - 1) & (dshot_full_number >> 0))]; // last four digits -// GCR RLL encode 20 to 21bit output + gcrnumber = gcr_encode_table[(dshot_full_number >> 12)] + << 15 // first set of four digits + | gcr_encode_table[(((1 << 4) - 1) & (dshot_full_number >> 8))] + << 10 // 2nd set of 4 digits + | gcr_encode_table[(((1 << 4) - 1) & (dshot_full_number >> 4))] + << 5 // 3rd set of four digits + | gcr_encode_table[(((1 << 4) - 1) & (dshot_full_number >> 0))]; // last four digits + // GCR RLL encode 20 to 21bit output #if defined(MCU_F051) || defined(MCU_F031) - gcr[1 + buffer_padding] = 64; - for (int i = 19; i >= 0; i--) { // each digit in gcrnumber - gcr[buffer_padding + 20 - i + 1] = ((((gcrnumber & 1 << i)) >> i) ^ (gcr[buffer_padding + 20 - i] >> 6)) - << 6; // exclusive ored with number before it multiplied by 64 to match - // output timer. - } - gcr[buffer_padding] = 0; + gcr[1 + buffer_padding] = 64; + for (int i = 19; i >= 0; i--) { // each digit in gcrnumber + gcr[buffer_padding + 20 - i + 1] = ((((gcrnumber & 1 << i)) >> i) ^ (gcr[buffer_padding + 20 - i] >> 6)) + << 6; // exclusive ored with number before it multiplied by 64 to match + // output timer. + } + gcr[buffer_padding] = 0; #else - gcr[1 + buffer_padding] = 128; - for (int i = 19; i >= 0; i--) { // each digit in gcrnumber - gcr[buffer_padding + 20 - i + 1] = ((((gcrnumber & 1 << i)) >> i) ^ (gcr[buffer_padding + 20 - i] >> 7)) - << 7; // exclusive ored with number before it multiplied by 64 to match - // output timer. - } - gcr[buffer_padding] = 0; + gcr[1 + buffer_padding] = 128; + for (int i = 19; i >= 0; i--) { // each digit in gcrnumber + gcr[buffer_padding + 20 - i + 1] = ((((gcrnumber & 1 << i)) >> i) ^ (gcr[buffer_padding + 20 - i] >> 7)) + << 7; // exclusive ored with number before it multiplied by 64 to match + // output timer. + } + gcr[buffer_padding] = 0; #endif } diff --git a/Src/functions.c b/Src/functions.c index 0b950761..1afe90bb 100644 --- a/Src/functions.c +++ b/Src/functions.c @@ -21,45 +21,52 @@ long map(long x, long in_min, long in_max, long out_min, long out_max) { - if (x >= in_max) - return out_max; - if (x <= in_min) - return out_min; - if (in_min > in_max) - return map(x, in_max, in_min, out_max, out_min); - if (out_min == out_max) - return out_min; - const long in_mid = (in_min + in_max) >> 1; - const long out_mid = (out_min + out_max) >> 1; - if (in_min == in_mid) - return out_mid; - if (x <= in_mid) - return map(x, in_min, in_mid, out_min, out_mid); - else - return map(x, in_mid + 1, in_max, out_mid, out_max); + if (x >= in_max) { + return out_max; + } + if (x <= in_min) { + return out_min; + } + if (in_min > in_max) { + return map(x, in_max, in_min, out_max, out_min); + } + if (out_min == out_max) { + return out_min; + } + const long in_mid = (in_min + in_max) >> 1; + const long out_mid = (out_min + out_max) >> 1; + if (in_min == in_mid) { + return out_mid; + } + if (x <= in_mid) { + return map(x, in_min, in_mid, out_min, out_mid); + } else { + return map(x, in_mid + 1, in_max, out_mid, out_max); + } } uint32_t getAbsDif(int number1, int number2) { - int result = number1 - number2; - if (result < 0) { - result = -result; - } - return (uint32_t)result; + int result = number1 - number2; + if (result < 0) { + result = -result; + } + return (uint32_t)result; } /* get current value of UTILITY_TIMER timer as 16bit microseconds */ -static inline uint16_t get_timer_us16(void) { +static inline uint16_t get_timer_us16(void) +{ #if defined(STMICRO) - return UTILITY_TIMER->CNT; + return UTILITY_TIMER->CNT; #elif defined(GIGADEVICES) - return TIMER_CNT(UTILITY_TIMER); + return TIMER_CNT(UTILITY_TIMER); #elif defined(ARTERY) - return UTILITY_TIMER->cval; + return UTILITY_TIMER->cval; #else - #error unsupported MCU +#error unsupported MCU #endif } @@ -68,9 +75,9 @@ static inline uint16_t get_timer_us16(void) { */ void delayMicros(uint32_t micros) { - const uint16_t cval_start = get_timer_us16(); - while ((uint16_t)(get_timer_us16() - cval_start) < (uint16_t)micros) { - } + const uint16_t cval_start = get_timer_us16(); + while ((uint16_t)(get_timer_us16() - cval_start) < (uint16_t)micros) { + } } /* @@ -78,42 +85,42 @@ void delayMicros(uint32_t micros) */ void delayMillis(uint32_t millis) { - while (millis-- > 0) { - delayMicros(1000UL); - } + while (millis-- > 0) { + delayMicros(1000UL); + } } #ifdef MCU_AT421 void gpio_mode_QUICK(gpio_type* gpio_periph, uint32_t mode, - uint32_t pull_up_down, uint32_t pin) + uint32_t pull_up_down, uint32_t pin) { - gpio_periph->cfgr = (((((gpio_periph->cfgr))) & (~(((pin * pin) * (0x3UL << (0U)))))) | (((pin * pin) * mode))); + gpio_periph->cfgr = (((((gpio_periph->cfgr))) & (~(((pin * pin) * (0x3UL << (0U)))))) | (((pin * pin) * mode))); } void gpio_mode_set(gpio_type* gpio_periph, uint32_t mode, uint32_t pull_up_down, - uint32_t pin) + uint32_t pin) { - gpio_periph->cfgr = (((((gpio_periph->cfgr))) & (~(((pin * pin) * (0x3UL << (0U)))))) | (((pin * pin) * mode))); - gpio_periph->pull = ((((((gpio_periph->pull))) & (~(((pin * pin) * (0x3UL << (0U)))))) | (((pin * pin) * pull_up_down)))); + gpio_periph->cfgr = (((((gpio_periph->cfgr))) & (~(((pin * pin) * (0x3UL << (0U)))))) | (((pin * pin) * mode))); + gpio_periph->pull = ((((((gpio_periph->pull))) & (~(((pin * pin) * (0x3UL << (0U)))))) | (((pin * pin) * pull_up_down)))); } #endif #ifdef MCU_AT415 void gpio_mode_QUICK(gpio_type* gpio_periph, uint32_t mode, - uint32_t pull_up_down, uint32_t pin) + uint32_t pull_up_down, uint32_t pin) { - __disable_irq(); - gpio_init_type gpio_init_struct; - gpio_default_para_init(&gpio_init_struct); + __disable_irq(); + gpio_init_type gpio_init_struct; + gpio_default_para_init(&gpio_init_struct); - if (GPIO_MODE_MUX) { - } + if (GPIO_MODE_MUX) { + } - gpio_init_struct.gpio_mode = mode; - gpio_init_struct.gpio_pins = pin; - gpio_init_struct.gpio_pull = pull_up_down; + gpio_init_struct.gpio_mode = mode; + gpio_init_struct.gpio_pins = pin; + gpio_init_struct.gpio_pull = pull_up_down; - gpio_init(gpio_periph, &gpio_init_struct); + gpio_init(gpio_periph, &gpio_init_struct); - __enable_irq(); + __enable_irq(); } #endif diff --git a/Src/main.c b/Src/main.c index ef2f6529..53c6c754 100644 --- a/Src/main.c +++ b/Src/main.c @@ -204,14 +204,14 @@ an settings option) *2.11 - RC-Car mode fix *2.12 - Reduce Advance on hard braking *2.13 - Remove Input capture filter for dshot2400 - - Change dshot 300 speed detection threshold + - Change dshot 300 speed detection threshold *2.14 - Reduce G071 zero cross checks - Assign all mcu's duty cycle resolution 2000 steps *2.15 - Enforce 1/2 commutation interval as minimum for g071 - Revert timing change on braking - Add per target over-ride option to max duty cycle change. - todo fix signal detection -*2.16 - add L431 +*2.16 - add L431 - add variable auto timing - add droneCAN */ @@ -253,18 +253,18 @@ void zcfoundroutine(void); // with sinusoidal startup !! //#define FIXED_DUTY_MODE // bypasses signal input and arming, uses a set duty -// cycle. For pumps, slot cars etc +// cycle. For pumps, slot cars etc //#define FIXED_DUTY_MODE_POWER 100 // // 0-100 percent not used in fixed speed mode // #define FIXED_SPEED_MODE // bypasses input signal and runs at a fixed rpm -// using the speed control loop PID +// using the speed control loop PID //#define FIXED_SPEED_MODE_RPM 1000 // // intended final rpm , ensure pole pair numbers are entered correctly in config // tool. // #define BRUSHED_MODE // overrides all brushless config settings, -// enables two channels for brushed control +// enables two channels for brushed control //#define GIMBAL_MODE // also // sinusoidal_startup needs to be on, maps input to sinusoidal angle. @@ -278,31 +278,31 @@ uint32_t MINIMUM_RPM_SPEED_CONTROL = 1000; // assign speed control PID values values are x10000 fastPID speedPid = { // commutation speed loop time - .Kp = 10, - .Ki = 0, - .Kd = 100, - .integral_limit = 10000, - .output_limit = 50000 + .Kp = 10, + .Ki = 0, + .Kd = 100, + .integral_limit = 10000, + .output_limit = 50000 }; fastPID currentPid = { // 1khz loop time - .Kp = 400, - .Ki = 0, - .Kd = 1000, - .integral_limit = 20000, - .output_limit = 100000 + .Kp = 400, + .Ki = 0, + .Kd = 1000, + .integral_limit = 20000, + .output_limit = 100000 }; fastPID stallPid = { // 1khz loop time - .Kp = 1, - .Ki = 0, - .Kd = 50, - .integral_limit = 10000, - .output_limit = 50000 + .Kp = 1, + .Ki = 0, + .Kd = 50, + .integral_limit = 10000, + .output_limit = 50000 }; EEprom_t eepromBuffer; -uint32_t eeprom_address = EEPROM_START_ADD; +uint32_t eeprom_address = EEPROM_START_ADD; uint16_t prop_brake_duty_cycle = 0; uint16_t ledcounter = 0; uint32_t process_time = 0; @@ -336,7 +336,7 @@ uint16_t low_cell_volt_cutoff = 330; // 3.3volts per cell //=========================== END EEPROM Defaults =========================== const char filename[30] __attribute__((section(".file_name"))) = FILE_NAME; -_Static_assert(sizeof(FIRMWARE_NAME) <=13,"Firmware name too long"); // max 12 character firmware name plus NULL +_Static_assert(sizeof(FIRMWARE_NAME) <=13,"Firmware name too long"); // max 12 character firmware name plus NULL // move these to targets folder or peripherals for each mcu uint16_t ADC_CCR = 30; @@ -370,7 +370,7 @@ uint16_t telem_ms_count; uint16_t VOLTAGE_DIVIDER = TARGET_VOLTAGE_DIVIDER; // 100k upper and 10k lower resistor in divider uint16_t - battery_voltage; // scale in volts * 10. 1260 is a battery voltage of 12.60 +battery_voltage; // scale in volts * 10. 1260 is a battery voltage of 12.60 char cell_count = 0; char brushed_direction_set = 0; @@ -398,7 +398,8 @@ uint16_t duty_cycle_setpoint = 0; char play_tone_flag = 0; typedef enum { GPIO_PIN_RESET = 0U, - GPIO_PIN_SET } GPIO_PinState; + GPIO_PIN_SET + } GPIO_PinState; uint16_t startup_max_duty_cycle = 200; uint16_t minimum_duty_cycle = DEAD_TIME; @@ -480,30 +481,30 @@ char rising = 1; ////Sine Wave PWM /////////////////// int16_t pwmSin[] = { - 180, 183, 186, 189, 193, 196, 199, 202, 205, 208, 211, 214, 217, 220, 224, - 227, 230, 233, 236, 239, 242, 245, 247, 250, 253, 256, 259, 262, 265, 267, - 270, 273, 275, 278, 281, 283, 286, 288, 291, 293, 296, 298, 300, 303, 305, - 307, 309, 312, 314, 316, 318, 320, 322, 324, 326, 327, 329, 331, 333, 334, - 336, 337, 339, 340, 342, 343, 344, 346, 347, 348, 349, 350, 351, 352, 353, - 354, 355, 355, 356, 357, 357, 358, 358, 359, 359, 359, 360, 360, 360, 360, - 360, 360, 360, 360, 360, 359, 359, 359, 358, 358, 357, 357, 356, 355, 355, - 354, 353, 352, 351, 350, 349, 348, 347, 346, 344, 343, 342, 340, 339, 337, - 336, 334, 333, 331, 329, 327, 326, 324, 322, 320, 318, 316, 314, 312, 309, - 307, 305, 303, 300, 298, 296, 293, 291, 288, 286, 283, 281, 278, 275, 273, - 270, 267, 265, 262, 259, 256, 253, 250, 247, 245, 242, 239, 236, 233, 230, - 227, 224, 220, 217, 214, 211, 208, 205, 202, 199, 196, 193, 189, 186, 183, - 180, 177, 174, 171, 167, 164, 161, 158, 155, 152, 149, 146, 143, 140, 136, - 133, 130, 127, 124, 121, 118, 115, 113, 110, 107, 104, 101, 98, 95, 93, - 90, 87, 85, 82, 79, 77, 74, 72, 69, 67, 64, 62, 60, 57, 55, - 53, 51, 48, 46, 44, 42, 40, 38, 36, 34, 33, 31, 29, 27, 26, - 24, 23, 21, 20, 18, 17, 16, 14, 13, 12, 11, 10, 9, 8, 7, - 6, 5, 5, 4, 3, 3, 2, 2, 1, 1, 1, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1, 1, 1, 2, 2, 3, 3, 4, 5, 5, - 6, 7, 8, 9, 10, 11, 12, 13, 14, 16, 17, 18, 20, 21, 23, - 24, 26, 27, 29, 31, 33, 34, 36, 38, 40, 42, 44, 46, 48, 51, - 53, 55, 57, 60, 62, 64, 67, 69, 72, 74, 77, 79, 82, 85, 87, - 90, 93, 95, 98, 101, 104, 107, 110, 113, 115, 118, 121, 124, 127, 130, - 133, 136, 140, 143, 146, 149, 152, 155, 158, 161, 164, 167, 171, 174, 177 + 180, 183, 186, 189, 193, 196, 199, 202, 205, 208, 211, 214, 217, 220, 224, + 227, 230, 233, 236, 239, 242, 245, 247, 250, 253, 256, 259, 262, 265, 267, + 270, 273, 275, 278, 281, 283, 286, 288, 291, 293, 296, 298, 300, 303, 305, + 307, 309, 312, 314, 316, 318, 320, 322, 324, 326, 327, 329, 331, 333, 334, + 336, 337, 339, 340, 342, 343, 344, 346, 347, 348, 349, 350, 351, 352, 353, + 354, 355, 355, 356, 357, 357, 358, 358, 359, 359, 359, 360, 360, 360, 360, + 360, 360, 360, 360, 360, 359, 359, 359, 358, 358, 357, 357, 356, 355, 355, + 354, 353, 352, 351, 350, 349, 348, 347, 346, 344, 343, 342, 340, 339, 337, + 336, 334, 333, 331, 329, 327, 326, 324, 322, 320, 318, 316, 314, 312, 309, + 307, 305, 303, 300, 298, 296, 293, 291, 288, 286, 283, 281, 278, 275, 273, + 270, 267, 265, 262, 259, 256, 253, 250, 247, 245, 242, 239, 236, 233, 230, + 227, 224, 220, 217, 214, 211, 208, 205, 202, 199, 196, 193, 189, 186, 183, + 180, 177, 174, 171, 167, 164, 161, 158, 155, 152, 149, 146, 143, 140, 136, + 133, 130, 127, 124, 121, 118, 115, 113, 110, 107, 104, 101, 98, 95, 93, + 90, 87, 85, 82, 79, 77, 74, 72, 69, 67, 64, 62, 60, 57, 55, + 53, 51, 48, 46, 44, 42, 40, 38, 36, 34, 33, 31, 29, 27, 26, + 24, 23, 21, 20, 18, 17, 16, 14, 13, 12, 11, 10, 9, 8, 7, + 6, 5, 5, 4, 3, 3, 2, 2, 1, 1, 1, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 1, 1, 2, 2, 3, 3, 4, 5, 5, + 6, 7, 8, 9, 10, 11, 12, 13, 14, 16, 17, 18, 20, 21, 23, + 24, 26, 27, 29, 31, 33, 34, 36, 38, 40, 42, 44, 46, 48, 51, + 53, 55, 57, 60, 62, 64, 67, 69, 72, 74, 77, 79, 82, 85, 87, + 90, 93, 95, 98, 101, 104, 107, 110, 113, 115, 118, 121, 124, 127, 130, + 133, 136, 140, 143, 146, 149, 152, 155, 158, 161, 164, 167, 171, 174, 177 }; // int sin_divider = 2; @@ -556,1017 +557,1019 @@ uint8_t ubAnalogWatchdogStatus = RESET; float doPidCalculations(struct fastPID* pidnow, int actual, int target) { - pidnow->error = actual - target; - pidnow->integral = pidnow->integral + pidnow->error * pidnow->Ki; - if (pidnow->integral > pidnow->integral_limit) { - pidnow->integral = pidnow->integral_limit; - } - if (pidnow->integral < -pidnow->integral_limit) { - pidnow->integral = -pidnow->integral_limit; - } - - pidnow->derivative = pidnow->Kd * (pidnow->error - pidnow->last_error); - pidnow->last_error = pidnow->error; - - pidnow->pid_output = pidnow->error * pidnow->Kp + pidnow->integral + pidnow->derivative; - - if (pidnow->pid_output > pidnow->output_limit) { - pidnow->pid_output = pidnow->output_limit; - } - if (pidnow->pid_output < -pidnow->output_limit) { - pidnow->pid_output = -pidnow->output_limit; - } - return pidnow->pid_output; + pidnow->error = actual - target; + pidnow->integral = pidnow->integral + pidnow->error * pidnow->Ki; + if (pidnow->integral > pidnow->integral_limit) { + pidnow->integral = pidnow->integral_limit; + } + if (pidnow->integral < -pidnow->integral_limit) { + pidnow->integral = -pidnow->integral_limit; + } + + pidnow->derivative = pidnow->Kd * (pidnow->error - pidnow->last_error); + pidnow->last_error = pidnow->error; + + pidnow->pid_output = pidnow->error * pidnow->Kp + pidnow->integral + pidnow->derivative; + + if (pidnow->pid_output > pidnow->output_limit) { + pidnow->pid_output = pidnow->output_limit; + } + if (pidnow->pid_output < -pidnow->output_limit) { + pidnow->pid_output = -pidnow->output_limit; + } + return pidnow->pid_output; } void loadEEpromSettings() { - read_flash_bin(eepromBuffer.buffer, eeprom_address, sizeof(eepromBuffer.buffer)); + read_flash_bin(eepromBuffer.buffer, eeprom_address, sizeof(eepromBuffer.buffer)); - if (eepromBuffer.advance_level > 3) { - eepromBuffer.advance_level = 2; - } + if (eepromBuffer.advance_level > 3) { + eepromBuffer.advance_level = 2; + } - if (eepromBuffer.pwm_frequency < 49 && eepromBuffer.pwm_frequency > 7) { - if (eepromBuffer.pwm_frequency < 49 && eepromBuffer.pwm_frequency > 23) { - TIMER1_MAX_ARR = map(eepromBuffer.pwm_frequency, 24, 48, TIM1_AUTORELOAD, TIM1_AUTORELOAD / 2); - } - if (eepromBuffer.pwm_frequency < 24 && eepromBuffer.pwm_frequency > 11) { - TIMER1_MAX_ARR = map(eepromBuffer.pwm_frequency, 12, 24, TIM1_AUTORELOAD * 2, TIM1_AUTORELOAD); - } - if (eepromBuffer.pwm_frequency < 12 && eepromBuffer.pwm_frequency > 7) { - TIMER1_MAX_ARR = map(eepromBuffer.pwm_frequency, 7, 16, TIM1_AUTORELOAD * 3, - TIM1_AUTORELOAD / 2 * 3); - } - SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR); - } else { - tim1_arr = TIM1_AUTORELOAD; - SET_AUTO_RELOAD_PWM(tim1_arr); - } - - if (eepromBuffer.startup_power < 151 && eepromBuffer.startup_power > 49) { - if(!eepromBuffer.comp_pwm){ // higher startup power for non-complementary pwm - min_startup_duty = (eepromBuffer.startup_power) *2 ; - minimum_duty_cycle = (eepromBuffer.startup_power / 2); - stall_protect_minimum_duty = minimum_duty_cycle + 10; - }else{ - min_startup_duty = (eepromBuffer.startup_power); - minimum_duty_cycle = (eepromBuffer.startup_power / 3); - stall_protect_minimum_duty = minimum_duty_cycle + 10; - } + if (eepromBuffer.pwm_frequency < 49 && eepromBuffer.pwm_frequency > 7) { + if (eepromBuffer.pwm_frequency < 49 && eepromBuffer.pwm_frequency > 23) { + TIMER1_MAX_ARR = map(eepromBuffer.pwm_frequency, 24, 48, TIM1_AUTORELOAD, TIM1_AUTORELOAD / 2); + } + if (eepromBuffer.pwm_frequency < 24 && eepromBuffer.pwm_frequency > 11) { + TIMER1_MAX_ARR = map(eepromBuffer.pwm_frequency, 12, 24, TIM1_AUTORELOAD * 2, TIM1_AUTORELOAD); + } + if (eepromBuffer.pwm_frequency < 12 && eepromBuffer.pwm_frequency > 7) { + TIMER1_MAX_ARR = map(eepromBuffer.pwm_frequency, 7, 16, TIM1_AUTORELOAD * 3, + TIM1_AUTORELOAD / 2 * 3); + } + SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR); + } else { + tim1_arr = TIM1_AUTORELOAD; + SET_AUTO_RELOAD_PWM(tim1_arr); + } + + if (eepromBuffer.startup_power < 151 && eepromBuffer.startup_power > 49) { + if (!eepromBuffer.comp_pwm) { // higher startup power for non-complementary pwm + min_startup_duty = (eepromBuffer.startup_power) *2 ; + minimum_duty_cycle = (eepromBuffer.startup_power / 2); + stall_protect_minimum_duty = minimum_duty_cycle + 10; } else { - min_startup_duty = 150; - minimum_duty_cycle = (min_startup_duty / 2) + 10; + min_startup_duty = (eepromBuffer.startup_power); + minimum_duty_cycle = (eepromBuffer.startup_power / 3); + stall_protect_minimum_duty = minimum_duty_cycle + 10; } - motor_kv = (eepromBuffer.motor_kv * 40) + 20; + } else { + min_startup_duty = 150; + minimum_duty_cycle = (min_startup_duty / 2) + 10; + } + motor_kv = (eepromBuffer.motor_kv * 40) + 20; #ifdef THREE_CELL_MAX - motor_kv = motor_kv / 2; + motor_kv = motor_kv / 2; #endif - setVolume(2); - if (eepromBuffer.eeprom_version > 0) { // these commands weren't introduced until eeprom version 1. + setVolume(2); + if (eepromBuffer.eeprom_version > 0) { // these commands weren't introduced until eeprom version 1. #ifdef CUSTOM_RAMP #else - if (eepromBuffer.beep_volume > 11) { - setVolume(5); - } else { - setVolume(eepromBuffer.beep_volume); - } + if (eepromBuffer.beep_volume > 11) { + setVolume(5); + } else { + setVolume(eepromBuffer.beep_volume); + } #endif - servo_low_threshold = (eepromBuffer.servo.low_threshold * 2) + 750; // anything below this point considered 0 - servo_high_threshold = (eepromBuffer.servo.high_threshold * 2) + 1750; - ; // anything above this point considered 2000 (max) - servo_neutral = (eepromBuffer.servo.neutral) + 1374; - servo_dead_band = eepromBuffer.servo.dead_band; - - if (eepromBuffer.low_voltage_cut_off == 0x01) { - LOW_VOLTAGE_CUTOFF = 1; - } else { - LOW_VOLTAGE_CUTOFF = 0; - } + servo_low_threshold = (eepromBuffer.servo.low_threshold * 2) + 750; // anything below this point considered 0 + servo_high_threshold = (eepromBuffer.servo.high_threshold * 2) + 1750; + ; // anything above this point considered 2000 (max) + servo_neutral = (eepromBuffer.servo.neutral) + 1374; + servo_dead_band = eepromBuffer.servo.dead_band; + + if (eepromBuffer.low_voltage_cut_off == 0x01) { + LOW_VOLTAGE_CUTOFF = 1; + } else { + LOW_VOLTAGE_CUTOFF = 0; + } - low_cell_volt_cutoff = eepromBuffer.low_cell_volt_cutoff + 250; // 2.5 to 3.5 volts per cell range + low_cell_volt_cutoff = eepromBuffer.low_cell_volt_cutoff + 250; // 2.5 to 3.5 volts per cell range #ifndef HAS_HALL_SENSORS - eepromBuffer.use_hall_sensors = 0; + eepromBuffer.use_hall_sensors = 0; #endif - if (eepromBuffer.sine_mode_changeover_thottle_level < 5 || eepromBuffer.sine_mode_changeover_thottle_level > 25) { // sine mode changeover 5-25 percent throttle - eepromBuffer.sine_mode_changeover_thottle_level = 5; - } - if (eepromBuffer.drag_brake_strength == 0 || eepromBuffer.drag_brake_strength > 10) { // drag brake 1-10 - eepromBuffer.drag_brake_strength = 10; - } + if (eepromBuffer.sine_mode_changeover_thottle_level < 5 || eepromBuffer.sine_mode_changeover_thottle_level > 25) { // sine mode changeover 5-25 percent throttle + eepromBuffer.sine_mode_changeover_thottle_level = 5; + } + if (eepromBuffer.drag_brake_strength == 0 || eepromBuffer.drag_brake_strength > 10) { // drag brake 1-10 + eepromBuffer.drag_brake_strength = 10; + } - if (eepromBuffer.driving_brake_strength == 0 || eepromBuffer.driving_brake_strength > 9) { // motor brake 1-9 - eepromBuffer.driving_brake_strength = 10; - } + if (eepromBuffer.driving_brake_strength == 0 || eepromBuffer.driving_brake_strength > 9) { // motor brake 1-9 + eepromBuffer.driving_brake_strength = 10; + } - if(eepromBuffer.driving_brake_strength < 10){ - dead_time_override = DEAD_TIME + (150 - (eepromBuffer.driving_brake_strength * 10)); - if (dead_time_override > 200) { - dead_time_override = 200; - } - min_startup_duty = min_startup_duty + dead_time_override; - minimum_duty_cycle = minimum_duty_cycle + dead_time_override; - throttle_max_at_low_rpm = throttle_max_at_low_rpm + dead_time_override; - startup_max_duty_cycle = startup_max_duty_cycle + dead_time_override; + if (eepromBuffer.driving_brake_strength < 10) { + dead_time_override = DEAD_TIME + (150 - (eepromBuffer.driving_brake_strength * 10)); + if (dead_time_override > 200) { + dead_time_override = 200; + } + min_startup_duty = min_startup_duty + dead_time_override; + minimum_duty_cycle = minimum_duty_cycle + dead_time_override; + throttle_max_at_low_rpm = throttle_max_at_low_rpm + dead_time_override; + startup_max_duty_cycle = startup_max_duty_cycle + dead_time_override; #ifdef STMICRO - TIM1->BDTR |= dead_time_override; + TIM1->BDTR |= dead_time_override; #endif #ifdef ARTERY - TMR1->brk |= dead_time_override; + TMR1->brk |= dead_time_override; #endif #ifdef GIGADEVICES - TIMER_CCHP(TIMER0) |= dead_time_override; + TIMER_CCHP(TIMER0) |= dead_time_override; #endif - } - if (eepromBuffer.limits.temperature < 70 || eepromBuffer.limits.temperature > 140) { - eepromBuffer.limits.temperature = 255; - } + } + if (eepromBuffer.limits.temperature < 70 || eepromBuffer.limits.temperature > 140) { + eepromBuffer.limits.temperature = 255; + } - if (eepromBuffer.limits.current > 0 && eepromBuffer.limits.current < 100) { - use_current_limit = 1; - } - - if (eepromBuffer.sine_mode_power == 0 || eepromBuffer.sine_mode_power > 10) { - eepromBuffer.sine_mode_power = 5; - } + if (eepromBuffer.limits.current > 0 && eepromBuffer.limits.current < 100) { + use_current_limit = 1; + } - // unsinged int cant be less than 0 - if (eepromBuffer.input_type < 10) { - switch (eepromBuffer.input_type) { - case AUTO_IN: - dshot = 0; - servoPwm = 0; - EDT_ARMED = 1; - break; - case DSHOT_IN: - dshot = 1; - EDT_ARMED = 1; - break; - case SERVO_IN: - servoPwm = 1; - break; - case SERIAL_IN: - break; - case EDTARM_IN: - EDT_ARM_ENABLE = 1; - EDT_ARMED = 0; - dshot = 1; - break; - }; - } else { - dshot = 0; - servoPwm = 0; - EDT_ARMED = 1; - } - if (motor_kv < 300) { - low_rpm_throttle_limit = 0; - } - low_rpm_level = motor_kv / 100 / (32 / eepromBuffer.motor_poles); + if (eepromBuffer.sine_mode_power == 0 || eepromBuffer.sine_mode_power > 10) { + eepromBuffer.sine_mode_power = 5; + } - high_rpm_level = motor_kv / 12 / (32 / eepromBuffer.motor_poles); + // unsinged int cant be less than 0 + if (eepromBuffer.input_type < 10) { + switch (eepromBuffer.input_type) { + case AUTO_IN: + dshot = 0; + servoPwm = 0; + EDT_ARMED = 1; + break; + case DSHOT_IN: + dshot = 1; + EDT_ARMED = 1; + break; + case SERVO_IN: + servoPwm = 1; + break; + case SERIAL_IN: + break; + case EDTARM_IN: + EDT_ARM_ENABLE = 1; + EDT_ARMED = 0; + dshot = 1; + break; + }; + } else { + dshot = 0; + servoPwm = 0; + EDT_ARMED = 1; } - reverse_speed_threshold = map(motor_kv, 300, 3000, 1000, 500); + if (motor_kv < 300) { + low_rpm_throttle_limit = 0; + } + low_rpm_level = motor_kv / 100 / (32 / eepromBuffer.motor_poles); + + high_rpm_level = motor_kv / 12 / (32 / eepromBuffer.motor_poles); + } + reverse_speed_threshold = map(motor_kv, 300, 3000, 1000, 500); } void saveEEpromSettings() { - eepromBuffer.eeprom_version = eeprom_layout_version; + eepromBuffer.eeprom_version = eeprom_layout_version; - save_flash_nolib(eepromBuffer.buffer, sizeof(eepromBuffer.buffer), eeprom_address); + save_flash_nolib(eepromBuffer.buffer, sizeof(eepromBuffer.buffer), eeprom_address); } uint16_t getSmoothedCurrent() { - total = total - readings[readIndex]; - readings[readIndex] = ADC_raw_current; - total = total + readings[readIndex]; - readIndex = readIndex + 1; - if (readIndex >= numReadings) { - readIndex = 0; - } - smoothedcurrent = total / numReadings; - return smoothedcurrent; + total = total - readings[readIndex]; + readings[readIndex] = ADC_raw_current; + total = total + readings[readIndex]; + readIndex = readIndex + 1; + if (readIndex >= numReadings) { + readIndex = 0; + } + smoothedcurrent = total / numReadings; + return smoothedcurrent; } void getBemfState() { - uint8_t current_state = 0; + uint8_t current_state = 0; #ifdef MCU_F031 - if (step == 1 || step == 4) { - current_state = PHASE_C_EXTI_PORT->IDR & PHASE_C_EXTI_PIN; - } - if (step == 2 || step == 5) { // in phase two or 5 read from phase A Pf1 - current_state = PHASE_A_EXTI_PORT->IDR & PHASE_A_EXTI_PIN; - } - if (step == 3 || step == 6) { // phase B pf0 - current_state = PHASE_B_EXTI_PORT->IDR & PHASE_B_EXTI_PIN; - } + if (step == 1 || step == 4) { + current_state = PHASE_C_EXTI_PORT->IDR & PHASE_C_EXTI_PIN; + } + if (step == 2 || step == 5) { // in phase two or 5 read from phase A Pf1 + current_state = PHASE_A_EXTI_PORT->IDR & PHASE_A_EXTI_PIN; + } + if (step == 3 || step == 6) { // phase B pf0 + current_state = PHASE_B_EXTI_PORT->IDR & PHASE_B_EXTI_PIN; + } #else - current_state = !getCompOutputLevel(); // polarity reversed + current_state = !getCompOutputLevel(); // polarity reversed #endif - if (rising) { - if (current_state) { - bemfcounter++; - } else { - bad_count++; - if (bad_count > bad_count_threshold) { - bemfcounter = 0; - } - } + if (rising) { + if (current_state) { + bemfcounter++; } else { - if (!current_state) { - bemfcounter++; - } else { - bad_count++; - if (bad_count > bad_count_threshold) { - bemfcounter = 0; - } - } + bad_count++; + if (bad_count > bad_count_threshold) { + bemfcounter = 0; + } + } + } else { + if (!current_state) { + bemfcounter++; + } else { + bad_count++; + if (bad_count > bad_count_threshold) { + bemfcounter = 0; + } } + } } void commutate() { - if (forward == 1) { - step++; - if (step > 6) { - step = 1; - desync_check = 1; - } - rising = step % 2; - } else { - step--; - if (step < 1) { - step = 6; - desync_check = 1; - } - rising = !(step % 2); + if (forward == 1) { + step++; + if (step > 6) { + step = 1; + desync_check = 1; } - __disable_irq(); // don't let dshot interrupt - if (!prop_brake_active) { - comStep(step); - } - __enable_irq(); - changeCompInput(); -// if (average_interval > 2500) { -// old_routine = 1; -// } - bemfcounter = 0; - zcfound = 0; - commutation_intervals[step - 1] = commutation_interval; // just used to calulate average + rising = step % 2; + } else { + step--; + if (step < 1) { + step = 6; + desync_check = 1; + } + rising = !(step % 2); + } + __disable_irq(); // don't let dshot interrupt + if (!prop_brake_active) { + comStep(step); + } + __enable_irq(); + changeCompInput(); + // if (average_interval > 2500) { + // old_routine = 1; + // } + bemfcounter = 0; + zcfound = 0; + commutation_intervals[step - 1] = commutation_interval; // just used to calulate average #ifdef USE_PULSE_OUT - if(rising){ - GPIOB->scr = GPIO_PINS_8; - }else{ - GPIOB->clr = GPIO_PINS_8; - } + if (rising) { + GPIOB->scr = GPIO_PINS_8; + } else { + GPIOB->clr = GPIO_PINS_8; + } #endif } void PeriodElapsedCallback() { - DISABLE_COM_TIMER_INT(); // disable interrupt - commutate(); - commutation_interval = (3 * commutation_interval + thiszctime) >> 2; - if (!eepromBuffer.auto_advance) { - advance = (commutation_interval >> 3) * temp_advance; // 60 divde 8 7.5 degree increments - } else { - advance = (commutation_interval * auto_advance_level) >> 6; // 60 divde 64 0.9375 degree increments - } - waitTime = (commutation_interval >> 1) - advance; - if (!old_routine) { - enableCompInterrupts(); // enable comp interrupt - } - if (zero_crosses < 10000) { - zero_crosses++; - } + DISABLE_COM_TIMER_INT(); // disable interrupt + commutate(); + commutation_interval = (3 * commutation_interval + thiszctime) >> 2; + if (!eepromBuffer.auto_advance) { + advance = (commutation_interval >> 3) * temp_advance; // 60 divde 8 7.5 degree increments + } else { + advance = (commutation_interval * auto_advance_level) >> 6; // 60 divde 64 0.9375 degree increments + } + waitTime = (commutation_interval >> 1) - advance; + if (!old_routine) { + enableCompInterrupts(); // enable comp interrupt + } + if (zero_crosses < 10000) { + zero_crosses++; + } } void interruptRoutine() { - if (average_interval > 125) { - if ((INTERVAL_TIMER_COUNT < 125) && (duty_cycle < 600) && (zero_crosses < 500)) { // should be impossible, desync?exit anyway - return; - } - stuckcounter++; // stuck at 100 interrupts before the main loop happens - // again. - if (stuckcounter > 100) { - maskPhaseInterrupts(); - zero_crosses = 0; - return; - } + if (average_interval > 125) { + if ((INTERVAL_TIMER_COUNT < 125) && (duty_cycle < 600) && (zero_crosses < 500)) { // should be impossible, desync?exit anyway + return; + } + stuckcounter++; // stuck at 100 interrupts before the main loop happens + // again. + if (stuckcounter > 100) { + maskPhaseInterrupts(); + zero_crosses = 0; + return; } - for (int i = 0; i < filter_level; i++) { + } + for (int i = 0; i < filter_level; i++) { #ifdef MCU_F031 - if (((current_GPIO_PORT->IDR & current_GPIO_PIN) == !(rising))) { + if (((current_GPIO_PORT->IDR & current_GPIO_PIN) == !(rising))) { #else - if (getCompOutputLevel() == rising) { + if (getCompOutputLevel() == rising) { #endif - return; - } - } - __disable_irq(); - maskPhaseInterrupts(); - thiszctime = INTERVAL_TIMER_COUNT; - SET_INTERVAL_TIMER_COUNT(0); - SET_AND_ENABLE_COM_INT(waitTime+1); // enable COM_TIMER interrupt - __enable_irq(); + return; + } + } + __disable_irq(); + maskPhaseInterrupts(); + thiszctime = INTERVAL_TIMER_COUNT; + SET_INTERVAL_TIMER_COUNT(0); + SET_AND_ENABLE_COM_INT(waitTime+1); // enable COM_TIMER interrupt + __enable_irq(); } void startMotor() { - if (running == 0) { - commutate(); - commutation_interval = 10000; - SET_INTERVAL_TIMER_COUNT(5000); - running = 1; - } - enableCompInterrupts(); + if (running == 0) { + commutate(); + commutation_interval = 10000; + SET_INTERVAL_TIMER_COUNT(5000); + running = 1; + } + enableCompInterrupts(); } void setInput() { - if (eepromBuffer.bi_direction) { - if (dshot == 0) { - if (eepromBuffer.rc_car_reverse) { - if (newinput > (1000 + (servo_dead_band << 1))) { - if (forward == eepromBuffer.dir_reversed) { - adjusted_input = 0; - // if (running) { - prop_brake_active = 1; - if (return_to_center) { - forward = 1 - eepromBuffer.dir_reversed; - prop_brake_active = 0; - return_to_center = 0; - } - } - if (prop_brake_active == 0) { - return_to_center = 0; - adjusted_input = map(newinput, 1000 + (servo_dead_band << 1), 2000, 47, 2047); - } - } - if (newinput < (1000 - (servo_dead_band << 1))) { - if (forward == (1 - eepromBuffer.dir_reversed)) { - adjusted_input = 0; - prop_brake_active = 1; - if (return_to_center) { - forward = eepromBuffer.dir_reversed; - prop_brake_active = 0; - return_to_center = 0; - } - } - if (prop_brake_active == 0) { - return_to_center = 0; - adjusted_input = map(newinput, 0, 1000 - (servo_dead_band << 1), 2047, 47); - } - } - if (newinput >= (1000 - (servo_dead_band << 1)) && newinput <= (1000 + (servo_dead_band << 1))) { - adjusted_input = 0; - if (prop_brake_active) { - prop_brake_active = 0; - return_to_center = 1; - } - } - } else { - if (newinput > (1000 + (servo_dead_band << 1))) { - if (forward == eepromBuffer.dir_reversed) { - if (((commutation_interval > reverse_speed_threshold) && (duty_cycle < 200)) || stepper_sine) { - forward = 1 - eepromBuffer.dir_reversed; - zero_crosses = 0; - old_routine = 1; - maskPhaseInterrupts(); - brushed_direction_set = 0; - } else { - newinput = 1000; - } - } - adjusted_input = map(newinput, 1000 + (servo_dead_band << 1), 2000, 47, 2047); - } - if (newinput < (1000 - (servo_dead_band << 1))) { - if (forward == (1 - eepromBuffer.dir_reversed)) { - if (((commutation_interval > reverse_speed_threshold) && (duty_cycle < 200)) || stepper_sine) { - zero_crosses = 0; - old_routine = 1; - forward = eepromBuffer.dir_reversed; - maskPhaseInterrupts(); - brushed_direction_set = 0; - } else { - newinput = 1000; - } - } - adjusted_input = map(newinput, 0, 1000 - (servo_dead_band << 1), 2047, 47); - } - - if (newinput >= (1000 - (servo_dead_band << 1)) && newinput <= (1000 + (servo_dead_band << 1))) { - adjusted_input = 0; - brushed_direction_set = 0; - } + if (eepromBuffer.bi_direction) { + if (dshot == 0) { + if (eepromBuffer.rc_car_reverse) { + if (newinput > (1000 + (servo_dead_band << 1))) { + if (forward == eepromBuffer.dir_reversed) { + adjusted_input = 0; + // if (running) { + prop_brake_active = 1; + if (return_to_center) { + forward = 1 - eepromBuffer.dir_reversed; + prop_brake_active = 0; + return_to_center = 0; } + } + if (prop_brake_active == 0) { + return_to_center = 0; + adjusted_input = map(newinput, 1000 + (servo_dead_band << 1), 2000, 47, 2047); + } } - - if (dshot) { - if (newinput > 1047) { - - if (forward == eepromBuffer.dir_reversed) { - if (((commutation_interval > reverse_speed_threshold) && (duty_cycle < 200)) || stepper_sine) { - forward = 1 - eepromBuffer.dir_reversed; - zero_crosses = 0; - old_routine = 1; - maskPhaseInterrupts(); - brushed_direction_set = 0; - } else { - newinput = 0; - } - } - adjusted_input = ((newinput - 1048) * 2 + 47) - reversing_dead_band; + if (newinput < (1000 - (servo_dead_band << 1))) { + if (forward == (1 - eepromBuffer.dir_reversed)) { + adjusted_input = 0; + prop_brake_active = 1; + if (return_to_center) { + forward = eepromBuffer.dir_reversed; + prop_brake_active = 0; + return_to_center = 0; } - if (newinput <= 1047 && newinput > 47) { - if (forward == (1 - eepromBuffer.dir_reversed)) { - if (((commutation_interval > reverse_speed_threshold) && (duty_cycle < 200)) || stepper_sine) { - zero_crosses = 0; - old_routine = 1; - forward = eepromBuffer.dir_reversed; - maskPhaseInterrupts(); - brushed_direction_set = 0; - } else { - newinput = 0; - } - } - adjusted_input = ((newinput - 48) * 2 + 47) - reversing_dead_band; + } + if (prop_brake_active == 0) { + return_to_center = 0; + adjusted_input = map(newinput, 0, 1000 - (servo_dead_band << 1), 2047, 47); + } + } + if (newinput >= (1000 - (servo_dead_band << 1)) && newinput <= (1000 + (servo_dead_band << 1))) { + adjusted_input = 0; + if (prop_brake_active) { + prop_brake_active = 0; + return_to_center = 1; + } + } + } else { + if (newinput > (1000 + (servo_dead_band << 1))) { + if (forward == eepromBuffer.dir_reversed) { + if (((commutation_interval > reverse_speed_threshold) && (duty_cycle < 200)) || stepper_sine) { + forward = 1 - eepromBuffer.dir_reversed; + zero_crosses = 0; + old_routine = 1; + maskPhaseInterrupts(); + brushed_direction_set = 0; + } else { + newinput = 1000; } - if (newinput < 48) { - adjusted_input = 0; - brushed_direction_set = 0; + } + adjusted_input = map(newinput, 1000 + (servo_dead_band << 1), 2000, 47, 2047); + } + if (newinput < (1000 - (servo_dead_band << 1))) { + if (forward == (1 - eepromBuffer.dir_reversed)) { + if (((commutation_interval > reverse_speed_threshold) && (duty_cycle < 200)) || stepper_sine) { + zero_crosses = 0; + old_routine = 1; + forward = eepromBuffer.dir_reversed; + maskPhaseInterrupts(); + brushed_direction_set = 0; + } else { + newinput = 1000; } + } + adjusted_input = map(newinput, 0, 1000 - (servo_dead_band << 1), 2047, 47); } - } else { - adjusted_input = newinput; + + if (newinput >= (1000 - (servo_dead_band << 1)) && newinput <= (1000 + (servo_dead_band << 1))) { + adjusted_input = 0; + brushed_direction_set = 0; + } + } + } + + if (dshot) { + if (newinput > 1047) { + + if (forward == eepromBuffer.dir_reversed) { + if (((commutation_interval > reverse_speed_threshold) && (duty_cycle < 200)) || stepper_sine) { + forward = 1 - eepromBuffer.dir_reversed; + zero_crosses = 0; + old_routine = 1; + maskPhaseInterrupts(); + brushed_direction_set = 0; + } else { + newinput = 0; + } + } + adjusted_input = ((newinput - 1048) * 2 + 47) - reversing_dead_band; + } + if (newinput <= 1047 && newinput > 47) { + if (forward == (1 - eepromBuffer.dir_reversed)) { + if (((commutation_interval > reverse_speed_threshold) && (duty_cycle < 200)) || stepper_sine) { + zero_crosses = 0; + old_routine = 1; + forward = eepromBuffer.dir_reversed; + maskPhaseInterrupts(); + brushed_direction_set = 0; + } else { + newinput = 0; + } + } + adjusted_input = ((newinput - 48) * 2 + 47) - reversing_dead_band; + } + if (newinput < 48) { + adjusted_input = 0; + brushed_direction_set = 0; + } } + } else { + adjusted_input = newinput; + } #ifndef BRUSHED_MODE - if ((bemf_timeout_happened > bemf_timeout) && eepromBuffer.stuck_rotor_protection) { - allOff(); - maskPhaseInterrupts(); - input = 0; - bemf_timeout_happened = 102; + if ((bemf_timeout_happened > bemf_timeout) && eepromBuffer.stuck_rotor_protection) { + allOff(); + maskPhaseInterrupts(); + input = 0; + bemf_timeout_happened = 102; #ifdef USE_RGB_LED - GPIOB->BRR = LL_GPIO_PIN_8; // on red - GPIOB->BSRR = LL_GPIO_PIN_5; // - GPIOB->BSRR = LL_GPIO_PIN_3; + GPIOB->BRR = LL_GPIO_PIN_8; // on red + GPIOB->BSRR = LL_GPIO_PIN_5; // + GPIOB->BSRR = LL_GPIO_PIN_3; #endif - } else { + } else { #ifdef FIXED_DUTY_MODE - input = FIXED_DUTY_MODE_POWER * 20 + 47; + input = FIXED_DUTY_MODE_POWER * 20 + 47; #else - if (eepromBuffer.use_sine_start) { - if (adjusted_input < 30) { // dead band ? - input = 0; - } - if (adjusted_input > 30 && adjusted_input < (eepromBuffer.sine_mode_changeover_thottle_level * 20)) { - input = map(adjusted_input, 30, + if (eepromBuffer.use_sine_start) { + if (adjusted_input < 30) { // dead band ? + input = 0; + } + if (adjusted_input > 30 && adjusted_input < (eepromBuffer.sine_mode_changeover_thottle_level * 20)) { + input = map(adjusted_input, 30, (eepromBuffer.sine_mode_changeover_thottle_level * 20), 47, 160); - } - if (adjusted_input >= (eepromBuffer.sine_mode_changeover_thottle_level * 20)) { - input = map(adjusted_input, (eepromBuffer.sine_mode_changeover_thottle_level * 20), + } + if (adjusted_input >= (eepromBuffer.sine_mode_changeover_thottle_level * 20)) { + input = map(adjusted_input, (eepromBuffer.sine_mode_changeover_thottle_level * 20), 2047, 160, 2047); + } + } else { + if (use_speed_control_loop) { + if (drive_by_rpm) { + target_e_com_time = 60000000 / map(adjusted_input, 47, 2047, MINIMUM_RPM_SPEED_CONTROL, MAXIMUM_RPM_SPEED_CONTROL) / (eepromBuffer.motor_poles / 2); + if (adjusted_input < 47) { // dead band ? + input = 0; + speedPid.error = 0; + input_override = 0; + } else { + input = (uint16_t)input_override; // speed control pid override + if (input_override > 2047) { + input = 2047; + } + if (input_override < 48) { + input = 48; } + } } else { - if (use_speed_control_loop) { - if (drive_by_rpm) { - target_e_com_time = 60000000 / map(adjusted_input, 47, 2047, MINIMUM_RPM_SPEED_CONTROL, MAXIMUM_RPM_SPEED_CONTROL) / (eepromBuffer.motor_poles / 2); - if (adjusted_input < 47) { // dead band ? - input = 0; - speedPid.error = 0; - input_override = 0; - } else { - input = (uint16_t)input_override; // speed control pid override - if (input_override > 2047) { - input = 2047; - } - if (input_override < 48) { - input = 48; - } - } - } else { - - input = (uint16_t)input_override; // speed control pid override - if (input_override > 2047) { - input = 2047; - } - if (input_override < 48) { - input = 48; - } - } - } else { - input = adjusted_input; - } + input = (uint16_t)input_override; // speed control pid override + if (input_override > 2047) { + input = 2047; + } + if (input_override < 48) { + input = 48; + } } -#endif + } else { + + input = adjusted_input; + } } +#endif + } #endif #ifndef BRUSHED_MODE -if (!stepper_sine && armed) { - if (input >= 47 + (80 * eepromBuffer.use_sine_start)) { - if (running == 0) { - allOff(); - if (!old_routine) { - startMotor(); - } - running = 1; - last_duty_cycle = min_startup_duty; - } - - if (eepromBuffer.use_sine_start) { - duty_cycle_setpoint = map(input, 137, 2047, minimum_duty_cycle+40, 2000); - } else { - duty_cycle_setpoint = map(input, 47, 2047, minimum_duty_cycle, 2000); - } - - if (!eepromBuffer.rc_car_reverse) { - prop_brake_active = 0; - } + if (!stepper_sine && armed) { + if (input >= 47 + (80 * eepromBuffer.use_sine_start)) { + if (running == 0) { + allOff(); + if (!old_routine) { + startMotor(); } + running = 1; + last_duty_cycle = min_startup_duty; + } + + if (eepromBuffer.use_sine_start) { + duty_cycle_setpoint = map(input, 137, 2047, minimum_duty_cycle+40, 2000); + } else { + duty_cycle_setpoint = map(input, 47, 2047, minimum_duty_cycle, 2000); + } + + if (!eepromBuffer.rc_car_reverse) { + prop_brake_active = 0; + } + } - if (input < 47 + (80 * eepromBuffer.use_sine_start)) { - if (play_tone_flag != 0) { - switch (play_tone_flag) { - - case 1: - playDefaultTone(); - break; - case 2: - playChangedTone(); - break; - case 3: - playBeaconTune3(); - break; - case 4: - playInputTune2(); - break; - case 5: - playDefaultTone(); - break; - } - play_tone_flag = 0; + if (input < 47 + (80 * eepromBuffer.use_sine_start)) { + if (play_tone_flag != 0) { + switch (play_tone_flag) { + + case 1: + playDefaultTone(); + break; + case 2: + playChangedTone(); + break; + case 3: + playBeaconTune3(); + break; + case 4: + playInputTune2(); + break; + case 5: + playDefaultTone(); + break; + } + play_tone_flag = 0; + } + + if (!eepromBuffer.comp_pwm) { + duty_cycle_setpoint = 0; + if (!running) { + old_routine = 1; + zero_crosses = 0; + if (eepromBuffer.brake_on_stop) { + fullBrake(); + } else { + if (!prop_brake_active) { + allOff(); } - - if (!eepromBuffer.comp_pwm) { - duty_cycle_setpoint = 0; - if (!running) { - old_routine = 1; - zero_crosses = 0; - if (eepromBuffer.brake_on_stop) { - fullBrake(); - } else { - if (!prop_brake_active) { - allOff(); - } - } - } - if (eepromBuffer.rc_car_reverse && prop_brake_active) { + } + } + if (eepromBuffer.rc_car_reverse && prop_brake_active) { #ifndef PWM_ENABLE_BRIDGE - prop_brake_duty_cycle = (getAbsDif(1000, newinput) + 1000); - if (prop_brake_duty_cycle >= (TIMER1_MAX_ARR - 1)) { - fullBrake(); - } else { - proportionalBrake(); - } + prop_brake_duty_cycle = (getAbsDif(1000, newinput) + 1000); + if (prop_brake_duty_cycle >= (TIMER1_MAX_ARR - 1)) { + fullBrake(); + } else { + proportionalBrake(); + } #endif - } - } else { - if (!running) { - - old_routine = 1; - zero_crosses = 0; - bad_count = 0; - if (eepromBuffer.brake_on_stop) { - if (!eepromBuffer.use_sine_start) { + } + } else { + if (!running) { + + old_routine = 1; + zero_crosses = 0; + bad_count = 0; + if (eepromBuffer.brake_on_stop) { + if (!eepromBuffer.use_sine_start) { #ifndef PWM_ENABLE_BRIDGE - prop_brake_duty_cycle = (1980) + eepromBuffer.drag_brake_strength * 2; - proportionalBrake(); - prop_brake_active = 1; + prop_brake_duty_cycle = (1980) + eepromBuffer.drag_brake_strength * 2; + proportionalBrake(); + prop_brake_active = 1; #else - // todo add proportional braking for pwm/enable style bridge. + // todo add proportional braking for pwm/enable style bridge. #endif - } - } else { - allOff(); - } - duty_cycle_setpoint = 0; - } - - phase_A_position = ((step - 1) * 60) + enter_sine_angle; - if (phase_A_position > 359) { - phase_A_position -= 360; - } - phase_B_position = phase_A_position + 119; - if (phase_B_position > 359) { - phase_B_position -= 360; - } - phase_C_position = phase_A_position + 239; - if (phase_C_position > 359) { - phase_C_position -= 360; - } - - if (eepromBuffer.use_sine_start == 1) { - stepper_sine = 1; - } - duty_cycle_setpoint = 0; } + } else { + allOff(); + } + duty_cycle_setpoint = 0; } - if (!prop_brake_active) { - if (input >= 47 && (zero_crosses < (uint32_t)(30 >> eepromBuffer.stall_protection))) { - if (duty_cycle_setpoint < min_startup_duty) { - duty_cycle_setpoint = min_startup_duty; - } - if (duty_cycle_setpoint > startup_max_duty_cycle) { - duty_cycle_setpoint = startup_max_duty_cycle; - } - } - if (duty_cycle_setpoint > duty_cycle_maximum) { - duty_cycle_setpoint = duty_cycle_maximum; - } - if (use_current_limit) { - if (duty_cycle_setpoint > use_current_limit_adjust) { - duty_cycle_setpoint = use_current_limit_adjust; - } - } - - if (stall_protection_adjust > 0 && input > 47) { + phase_A_position = ((step - 1) * 60) + enter_sine_angle; + if (phase_A_position > 359) { + phase_A_position -= 360; + } + phase_B_position = phase_A_position + 119; + if (phase_B_position > 359) { + phase_B_position -= 360; + } + phase_C_position = phase_A_position + 239; + if (phase_C_position > 359) { + phase_C_position -= 360; + } - duty_cycle_setpoint = duty_cycle_setpoint + (uint16_t)stall_protection_adjust; - } + if (eepromBuffer.use_sine_start == 1) { + stepper_sine = 1; } + duty_cycle_setpoint = 0; + } } + if (!prop_brake_active) { + if (input >= 47 && (zero_crosses < (uint32_t)(30 >> eepromBuffer.stall_protection))) { + if (duty_cycle_setpoint < min_startup_duty) { + duty_cycle_setpoint = min_startup_duty; + } + if (duty_cycle_setpoint > startup_max_duty_cycle) { + duty_cycle_setpoint = startup_max_duty_cycle; + } + } + + if (duty_cycle_setpoint > duty_cycle_maximum) { + duty_cycle_setpoint = duty_cycle_maximum; + } + if (use_current_limit) { + if (duty_cycle_setpoint > use_current_limit_adjust) { + duty_cycle_setpoint = use_current_limit_adjust; + } + } + + if (stall_protection_adjust > 0 && input > 47) { + + duty_cycle_setpoint = duty_cycle_setpoint + (uint16_t)stall_protection_adjust; + } + } + } #endif } void tenKhzRoutine() -{ // 20khz as of 2.00 to be renamed - duty_cycle = duty_cycle_setpoint; - tenkhzcounter++; - ledcounter++; - one_khz_loop_counter++; - if (!armed) { - if (cell_count == 0) { - if (inputSet) { - if (adjusted_input == 0) { - armed_timeout_count++; - if (armed_timeout_count > LOOP_FREQUENCY_HZ) { // one second - if (zero_input_count > 30) { - armed = 1; +{ + // 20khz as of 2.00 to be renamed + duty_cycle = duty_cycle_setpoint; + tenkhzcounter++; + ledcounter++; + one_khz_loop_counter++; + if (!armed) { + if (cell_count == 0) { + if (inputSet) { + if (adjusted_input == 0) { + armed_timeout_count++; + if (armed_timeout_count > LOOP_FREQUENCY_HZ) { // one second + if (zero_input_count > 30) { + armed = 1; #ifdef USE_LED_STRIP - // send_LED_RGB(0,0,0); - delayMicros(1000); - send_LED_RGB(0, 255, 0); + // send_LED_RGB(0,0,0); + delayMicros(1000); + send_LED_RGB(0, 255, 0); #endif #ifdef USE_RGB_LED - GPIOB->BRR = LL_GPIO_PIN_3; // turn on green - GPIOB->BSRR = LL_GPIO_PIN_8; // turn on green - GPIOB->BSRR = LL_GPIO_PIN_5; + GPIOB->BRR = LL_GPIO_PIN_3; // turn on green + GPIOB->BSRR = LL_GPIO_PIN_8; // turn on green + GPIOB->BSRR = LL_GPIO_PIN_5; #endif - if ((cell_count == 0) && LOW_VOLTAGE_CUTOFF) { - cell_count = battery_voltage / 370; - for (int i = 0; i < cell_count; i++) { - playInputTune(); - delayMillis(100); - RELOAD_WATCHDOG_COUNTER(); - } - } else { + if ((cell_count == 0) && LOW_VOLTAGE_CUTOFF) { + cell_count = battery_voltage / 370; + for (int i = 0; i < cell_count; i++) { + playInputTune(); + delayMillis(100); + RELOAD_WATCHDOG_COUNTER(); + } + } else { #ifdef MCU_AT415 - play_tone_flag = 4; + play_tone_flag = 4; #else - playInputTune(); + playInputTune(); #endif - } - if (!servoPwm) { - eepromBuffer.rc_car_reverse = 0; - } - } else { - inputSet = 0; - armed_timeout_count = 0; - } - } - } else { - armed_timeout_count = 0; - } + } + if (!servoPwm) { + eepromBuffer.rc_car_reverse = 0; + } + } else { + inputSet = 0; + armed_timeout_count = 0; } + } + } else { + armed_timeout_count = 0; } + } } + } - if (eepromBuffer.telementry_on_interval) { - telem_ms_count++; - if (telem_ms_count > telemetry_interval_ms * 20) { - send_telemetry = 1; - telem_ms_count = 0; - } + if (eepromBuffer.telementry_on_interval) { + telem_ms_count++; + if (telem_ms_count > telemetry_interval_ms * 20) { + send_telemetry = 1; + telem_ms_count = 0; } + } #ifndef BRUSHED_MODE - if (!stepper_sine) { + if (!stepper_sine) { #ifndef CUSTOM_RAMP - if (old_routine && running) { - // send_LED_RGB(255, 0, 0); - maskPhaseInterrupts(); - getBemfState(); - if (!zcfound) { - if (rising) { - if (bemfcounter > min_bemf_counts_up) { - zcfound = 1; - zcfoundroutine(); - } - } else { - if (bemfcounter > min_bemf_counts_down) { - zcfound = 1; - zcfoundroutine(); - } - } - } + if (old_routine && running) { + // send_LED_RGB(255, 0, 0); + maskPhaseInterrupts(); + getBemfState(); + if (!zcfound) { + if (rising) { + if (bemfcounter > min_bemf_counts_up) { + zcfound = 1; + zcfoundroutine(); + } + } else { + if (bemfcounter > min_bemf_counts_down) { + zcfound = 1; + zcfoundroutine(); + } } + } + } #endif - if (one_khz_loop_counter > PID_LOOP_DIVIDER) { // 1khz PID loop - one_khz_loop_counter = 0; - if (use_current_limit && running) { - use_current_limit_adjust -= (int16_t)(doPidCalculations(¤tPid, actual_current, - eepromBuffer.limits.current * 2 * 100) - / 10000); - if (use_current_limit_adjust < minimum_duty_cycle) { - use_current_limit_adjust = minimum_duty_cycle; - } - if (use_current_limit_adjust > tim1_arr) { - use_current_limit_adjust = tim1_arr; - } - } - if (eepromBuffer.stall_protection && running) { // this boosts throttle as the rpm gets lower, for crawlers - // and rc cars only, do not use for multirotors. - stall_protection_adjust += (doPidCalculations(&stallPid, commutation_interval, - stall_protect_target_interval))/ 10000; - if (stall_protection_adjust > 150) { - stall_protection_adjust = 150; - } - if (stall_protection_adjust <= 0) { - stall_protection_adjust = 0; - } - } - if (use_speed_control_loop && running) { - input_override += doPidCalculations(&speedPid, e_com_time, target_e_com_time) / 10000; - if (input_override > 2047) { - input_override = 2047; - } - if (input_override < 0) { - input_override = 0; - } - if (zero_crosses < 100) { - speedPid.integral = 0; - } - } + if (one_khz_loop_counter > PID_LOOP_DIVIDER) { // 1khz PID loop + one_khz_loop_counter = 0; + if (use_current_limit && running) { + use_current_limit_adjust -= (int16_t)(doPidCalculations(¤tPid, actual_current, + eepromBuffer.limits.current * 2 * 100) + / 10000); + if (use_current_limit_adjust < minimum_duty_cycle) { + use_current_limit_adjust = minimum_duty_cycle; + } + if (use_current_limit_adjust > tim1_arr) { + use_current_limit_adjust = tim1_arr; + } + } + if (eepromBuffer.stall_protection && running) { // this boosts throttle as the rpm gets lower, for crawlers + // and rc cars only, do not use for multirotors. + stall_protection_adjust += (doPidCalculations(&stallPid, commutation_interval, + stall_protect_target_interval))/ 10000; + if (stall_protection_adjust > 150) { + stall_protection_adjust = 150; + } + if (stall_protection_adjust <= 0) { + stall_protection_adjust = 0; } - if (maximum_throttle_change_ramp) { - // max_duty_cycle_change = map(k_erpm, low_rpm_level, - // high_rpm_level, 1, 40); + } + if (use_speed_control_loop && running) { + input_override += doPidCalculations(&speedPid, e_com_time, target_e_com_time) / 10000; + if (input_override > 2047) { + input_override = 2047; + } + if (input_override < 0) { + input_override = 0; + } + if (zero_crosses < 100) { + speedPid.integral = 0; + } + } + } + if (maximum_throttle_change_ramp) { + // max_duty_cycle_change = map(k_erpm, low_rpm_level, + // high_rpm_level, 1, 40); #ifdef VOLTAGE_BASED_RAMP - uint16_t voltage_based_max_change = map(battery_voltage, 800, 2200, 10, 1); - if (average_interval > 200) { - max_duty_cycle_change = voltage_based_max_change; - } else { - max_duty_cycle_change = voltage_based_max_change * 3; - } + uint16_t voltage_based_max_change = map(battery_voltage, 800, 2200, 10, 1); + if (average_interval > 200) { + max_duty_cycle_change = voltage_based_max_change; + } else { + max_duty_cycle_change = voltage_based_max_change * 3; + } #else - if (zero_crosses < 150 || last_duty_cycle < 150) { - max_duty_cycle_change = RAMP_SPEED_STARTUP; - } else { - if (average_interval > 500) { - max_duty_cycle_change = RAMP_SPEED_LOW_RPM; - } else { - max_duty_cycle_change = RAMP_SPEED_HIGH_RPM; - } - } + if (zero_crosses < 150 || last_duty_cycle < 150) { + max_duty_cycle_change = RAMP_SPEED_STARTUP; + } else { + if (average_interval > 500) { + max_duty_cycle_change = RAMP_SPEED_LOW_RPM; + } else { + max_duty_cycle_change = RAMP_SPEED_HIGH_RPM; + } + } #endif #ifdef CUSTOM_RAMP - // max_duty_cycle_change = eepromBuffer[30]; + // max_duty_cycle_change = eepromBuffer[30]; #endif - if ((duty_cycle - last_duty_cycle) > max_duty_cycle_change) { - duty_cycle = last_duty_cycle + max_duty_cycle_change; - if (commutation_interval > 500) { - fast_accel = 1; - temp_advance = eepromBuffer.advance_level; - } else { - fast_accel = 0; - } - - } else if ((last_duty_cycle - duty_cycle) > max_duty_cycle_change) { - duty_cycle = last_duty_cycle - max_duty_cycle_change; - fast_accel = 0; - temp_advance = eepromBuffer.advance_level; - } else { - if(duty_cycle < 300 && commutation_interval < 300){ - temp_advance = eepromBuffer.advance_level; - }else{ - temp_advance = eepromBuffer.advance_level; - } - - fast_accel = 0; - } + if ((duty_cycle - last_duty_cycle) > max_duty_cycle_change) { + duty_cycle = last_duty_cycle + max_duty_cycle_change; + if (commutation_interval > 500) { + fast_accel = 1; + temp_advance = eepromBuffer.advance_level; + } else { + fast_accel = 0; } - if ((armed && running) && input > 47) { - if (eepromBuffer.variable_pwm) { - } - adjusted_duty_cycle = ((duty_cycle * tim1_arr) / 2000) + 1; + } else if ((last_duty_cycle - duty_cycle) > max_duty_cycle_change) { + duty_cycle = last_duty_cycle - max_duty_cycle_change; + fast_accel = 0; + temp_advance = eepromBuffer.advance_level; + } else { + if (duty_cycle < 300 && commutation_interval < 300) { + temp_advance = eepromBuffer.advance_level; } else { - - if (prop_brake_active) { - adjusted_duty_cycle = TIMER1_MAX_ARR - ((prop_brake_duty_cycle * tim1_arr) / 2000) + 1; - } else { - adjusted_duty_cycle = ((duty_cycle * tim1_arr) / 2000); - } + temp_advance = eepromBuffer.advance_level; } - last_duty_cycle = duty_cycle; - SET_AUTO_RELOAD_PWM(tim1_arr); - SET_DUTY_CYCLE_ALL(adjusted_duty_cycle); + + fast_accel = 0; + } + } + if ((armed && running) && input > 47) { + if (eepromBuffer.variable_pwm) { + } + adjusted_duty_cycle = ((duty_cycle * tim1_arr) / 2000) + 1; + + } else { + + if (prop_brake_active) { + adjusted_duty_cycle = TIMER1_MAX_ARR - ((prop_brake_duty_cycle * tim1_arr) / 2000) + 1; + } else { + adjusted_duty_cycle = ((duty_cycle * tim1_arr) / 2000); + } } + last_duty_cycle = duty_cycle; + SET_AUTO_RELOAD_PWM(tim1_arr); + SET_DUTY_CYCLE_ALL(adjusted_duty_cycle); + } #endif // ndef brushed_mode #if defined(FIXED_DUTY_MODE) || defined(FIXED_SPEED_MODE) - if (getInputPinState()) { - signaltimeout++; - if (signaltimeout > LOOP_FREQUENCY_HZ) { - NVIC_SystemReset(); - } - } else { - signaltimeout = 0; + if (getInputPinState()) { + signaltimeout++; + if (signaltimeout > LOOP_FREQUENCY_HZ) { + NVIC_SystemReset(); } + } else { + signaltimeout = 0; + } #else - signaltimeout++; + signaltimeout++; #endif } void processDshot() { - if (compute_dshot_flag == 1) { - computeDshotDMA(); - compute_dshot_flag = 0; - } - if (compute_dshot_flag == 2) { - make_dshot_package(e_com_time); - compute_dshot_flag = 0; - return; - } - setInput(); + if (compute_dshot_flag == 1) { + computeDshotDMA(); + compute_dshot_flag = 0; + } + if (compute_dshot_flag == 2) { + make_dshot_package(e_com_time); + compute_dshot_flag = 0; + return; + } + setInput(); } void advanceincrement() { - if (!forward) { - phase_A_position++; - if (phase_A_position > 359) { - phase_A_position = 0; - } - phase_B_position++; - if (phase_B_position > 359) { - phase_B_position = 0; - } - phase_C_position++; - if (phase_C_position > 359) { - phase_C_position = 0; - } - } else { - phase_A_position--; - if (phase_A_position < 0) { - phase_A_position = 359; - } - phase_B_position--; - if (phase_B_position < 0) { - phase_B_position = 359; - } - phase_C_position--; - if (phase_C_position < 0) { - phase_C_position = 359; - } + if (!forward) { + phase_A_position++; + if (phase_A_position > 359) { + phase_A_position = 0; + } + phase_B_position++; + if (phase_B_position > 359) { + phase_B_position = 0; } + phase_C_position++; + if (phase_C_position > 359) { + phase_C_position = 0; + } + } else { + phase_A_position--; + if (phase_A_position < 0) { + phase_A_position = 359; + } + phase_B_position--; + if (phase_B_position < 0) { + phase_B_position = 359; + } + phase_C_position--; + if (phase_C_position < 0) { + phase_C_position = 359; + } + } #ifdef GIMBAL_MODE - setPWMCompare1(((2 * pwmSin[phase_A_position]) + gate_drive_offset) * TIMER1_MAX_ARR / 2000); - setPWMCompare2(((2 * pwmSin[phase_B_position]) + gate_drive_offset) * TIMER1_MAX_ARR / 2000); - setPWMCompare3(((2 * pwmSin[phase_C_position]) + gate_drive_offset) * TIMER1_MAX_ARR / 2000); + setPWMCompare1(((2 * pwmSin[phase_A_position]) + gate_drive_offset) * TIMER1_MAX_ARR / 2000); + setPWMCompare2(((2 * pwmSin[phase_B_position]) + gate_drive_offset) * TIMER1_MAX_ARR / 2000); + setPWMCompare3(((2 * pwmSin[phase_C_position]) + gate_drive_offset) * TIMER1_MAX_ARR / 2000); #else - setPWMCompare1( - (((2 * pwmSin[phase_A_position] / SINE_DIVIDER) + gate_drive_offset) * TIMER1_MAX_ARR / 2000) * eepromBuffer.sine_mode_power / 10); - setPWMCompare2( - (((2 * pwmSin[phase_B_position] / SINE_DIVIDER) + gate_drive_offset) * TIMER1_MAX_ARR / 2000) * eepromBuffer.sine_mode_power / 10); - setPWMCompare3( - (((2 * pwmSin[phase_C_position] / SINE_DIVIDER) + gate_drive_offset) * TIMER1_MAX_ARR / 2000) * eepromBuffer.sine_mode_power / 10); + setPWMCompare1( + (((2 * pwmSin[phase_A_position] / SINE_DIVIDER) + gate_drive_offset) * TIMER1_MAX_ARR / 2000) * eepromBuffer.sine_mode_power / 10); + setPWMCompare2( + (((2 * pwmSin[phase_B_position] / SINE_DIVIDER) + gate_drive_offset) * TIMER1_MAX_ARR / 2000) * eepromBuffer.sine_mode_power / 10); + setPWMCompare3( + (((2 * pwmSin[phase_C_position] / SINE_DIVIDER) + gate_drive_offset) * TIMER1_MAX_ARR / 2000) * eepromBuffer.sine_mode_power / 10); #endif } void zcfoundroutine() -{ // only used in polling mode, blocking routine. - thiszctime = INTERVAL_TIMER_COUNT; - SET_INTERVAL_TIMER_COUNT(0); - commutation_interval = (thiszctime + (3 * commutation_interval)) / 4; - advance = (commutation_interval >> 3) * 2; // 7.5 degree increments - waitTime = commutation_interval / 2 - advance; -// if(thiszctime < (commutation_interval - (commutation_interval>>2))){ -// waitTime = waitTime + commutation_interval - thiszctime; -// thiszctime = commutation_interval - (commutation_interval>>2); -// }else if(thiszctime > (commutation_interval + (commutation_interval>>2))){ -// waitTime = waitTime - thiszctime - commutation_interval; -// thiszctime = commutation_interval - (commutation_interval>>2); -// } - - while ((INTERVAL_TIMER_COUNT) < (waitTime)) { - if (zero_crosses < 5) { - break; - } +{ + // only used in polling mode, blocking routine. + thiszctime = INTERVAL_TIMER_COUNT; + SET_INTERVAL_TIMER_COUNT(0); + commutation_interval = (thiszctime + (3 * commutation_interval)) / 4; + advance = (commutation_interval >> 3) * 2; // 7.5 degree increments + waitTime = commutation_interval / 2 - advance; + // if(thiszctime < (commutation_interval - (commutation_interval>>2))){ + // waitTime = waitTime + commutation_interval - thiszctime; + // thiszctime = commutation_interval - (commutation_interval>>2); + // }else if(thiszctime > (commutation_interval + (commutation_interval>>2))){ + // waitTime = waitTime - thiszctime - commutation_interval; + // thiszctime = commutation_interval - (commutation_interval>>2); + // } + + while ((INTERVAL_TIMER_COUNT) < (waitTime)) { + if (zero_crosses < 5) { + break; } + } #ifdef MCU_GDE23 - TIMER_CAR(COM_TIMER) = waitTime; + TIMER_CAR(COM_TIMER) = waitTime; #endif #ifdef STMICRO - COM_TIMER->ARR = waitTime; + COM_TIMER->ARR = waitTime; #endif #ifdef MCU_AT32 - COM_TIMER->pr = waitTime; + COM_TIMER->pr = waitTime; #endif - commutate(); - bemfcounter = 0; - bad_count = 0; - - zero_crosses++; - if (eepromBuffer.stall_protection || eepromBuffer.rc_car_reverse) { - if (zero_crosses >= 20 && commutation_interval <= 2000) { - old_routine = 0; - enableCompInterrupts(); // enable interrupt - } - } else { - if (zero_crosses > 30) { - old_routine = 0; - enableCompInterrupts(); // enable interrupt - } + commutate(); + bemfcounter = 0; + bad_count = 0; + + zero_crosses++; + if (eepromBuffer.stall_protection || eepromBuffer.rc_car_reverse) { + if (zero_crosses >= 20 && commutation_interval <= 2000) { + old_routine = 0; + enableCompInterrupts(); // enable interrupt } + } else { + if (zero_crosses > 30) { + old_routine = 0; + enableCompInterrupts(); // enable interrupt + } + } } #ifdef BRUSHED_MODE void runBrushedLoop() { - uint16_t brushed_duty_cycle = 0; - - if (brushed_direction_set == 0 && adjusted_input > 48) { - if (forward) { - allOff(); - delayMicros(10); - twoChannelForward(); - } else { - allOff(); - delayMicros(10); - twoChannelReverse(); - } - brushed_direction_set = 1; - } + uint16_t brushed_duty_cycle = 0; - brushed_duty_cycle = map(adjusted_input, 48, 2047, 0, - (TIMER1_MAX_ARR - (TIMER1_MAX_ARR / 20))); - - if (degrees_celsius > eepromBuffer.limits.temperature) { - duty_cycle_maximum = map(degrees_celsius, eepromBuffer.limits.temperature, - eepromBuffer.limits.temperature + 20, TIMER1_MAX_ARR / 2, 1); + if (brushed_direction_set == 0 && adjusted_input > 48) { + if (forward) { + allOff(); + delayMicros(10); + twoChannelForward(); } else { - duty_cycle_maximum = TIMER1_MAX_ARR - 50; + allOff(); + delayMicros(10); + twoChannelReverse(); } - if (brushed_duty_cycle > duty_cycle_maximum) { - brushed_duty_cycle = duty_cycle_maximum; + brushed_direction_set = 1; + } + + brushed_duty_cycle = map(adjusted_input, 48, 2047, 0, + (TIMER1_MAX_ARR - (TIMER1_MAX_ARR / 20))); + + if (degrees_celsius > eepromBuffer.limits.temperature) { + duty_cycle_maximum = map(degrees_celsius, eepromBuffer.limits.temperature, + eepromBuffer.limits.temperature + 20, TIMER1_MAX_ARR / 2, 1); + } else { + duty_cycle_maximum = TIMER1_MAX_ARR - 50; + } + if (brushed_duty_cycle > duty_cycle_maximum) { + brushed_duty_cycle = duty_cycle_maximum; + } + + if (use_current_limit) { + use_current_limit_adjust -= (int16_t)(doPidCalculations(¤tPid, actual_current, + CURRENT_LIMIT * 100) + / 10000); + if (use_current_limit_adjust < minimum_duty_cycle) { + use_current_limit_adjust = minimum_duty_cycle; } - if (use_current_limit) { - use_current_limit_adjust -= (int16_t)(doPidCalculations(¤tPid, actual_current, - CURRENT_LIMIT * 100) - / 10000); - if (use_current_limit_adjust < minimum_duty_cycle) { - use_current_limit_adjust = minimum_duty_cycle; - } - - if (brushed_duty_cycle > use_current_limit_adjust) { - brushed_duty_cycle = use_current_limit_adjust; - } - } - if ((brushed_duty_cycle > 0) && armed) { - SET_DUTY_CYCLE_ALL(brushed_duty_cycle); - // TIM1->CCR1 = brushed_duty_cycle; - // TIM1->CCR2 = brushed_duty_cycle; - // TIM1->CCR3 = brushed_duty_cycle; - - } else { - SET_DUTY_CYCLE_ALL(0); - // TIM1->CCR1 = 0; - //// TIM1->CCR2 = 0; TIM1->CCR3 = 0; - brushed_direction_set = 0; + if (brushed_duty_cycle > use_current_limit_adjust) { + brushed_duty_cycle = use_current_limit_adjust; } + } + if ((brushed_duty_cycle > 0) && armed) { + SET_DUTY_CYCLE_ALL(brushed_duty_cycle); + // TIM1->CCR1 = brushed_duty_cycle; + // TIM1->CCR2 = brushed_duty_cycle; + // TIM1->CCR3 = brushed_duty_cycle; + + } else { + SET_DUTY_CYCLE_ALL(0); + // TIM1->CCR1 = 0; + //// TIM1->CCR2 = 0; TIM1->CCR3 = 0; + brushed_direction_set = 0; + } } #endif @@ -1579,171 +1582,171 @@ static void checkDeviceInfo(void) #define DEVINFO_MAGIC1 0x5925e3da #define DEVINFO_MAGIC2 0x4eb863d9 - const struct devinfo { - uint32_t magic1; - uint32_t magic2; - const uint8_t deviceInfo[9]; - } *devinfo = (struct devinfo *)(0x1000 - 32); - if (devinfo->magic1 != DEVINFO_MAGIC1 || - devinfo->magic2 != DEVINFO_MAGIC2) { - // bootloader does not support this feature, nothing to do - return; - } - // change eeprom_address based on the code in the bootloaders device info - switch (devinfo->deviceInfo[4]) { - case 0x1f: - eeprom_address = 0x08007c00; - break; - case 0x35: - eeprom_address = 0x0800f800; - break; - case 0x2b: - eeprom_address = 0x0801f800; - break; - } - - // TODO: check pin code and reboot to bootloader if incorrect + const struct devinfo { + uint32_t magic1; + uint32_t magic2; + const uint8_t deviceInfo[9]; + } *devinfo = (struct devinfo *)(0x1000 - 32); + if (devinfo->magic1 != DEVINFO_MAGIC1 || + devinfo->magic2 != DEVINFO_MAGIC2) { + // bootloader does not support this feature, nothing to do + return; + } + // change eeprom_address based on the code in the bootloaders device info + switch (devinfo->deviceInfo[4]) { + case 0x1f: + eeprom_address = 0x08007c00; + break; + case 0x35: + eeprom_address = 0x0800f800; + break; + case 0x2b: + eeprom_address = 0x0801f800; + break; + } + + // TODO: check pin code and reboot to bootloader if incorrect } int main(void) { - initAfterJump(); - - checkDeviceInfo(); + initAfterJump(); - initCorePeripherals(); + checkDeviceInfo(); - enableCorePeripherals(); + initCorePeripherals(); - loadEEpromSettings(); + enableCorePeripherals(); - if (VERSION_MAJOR != eepromBuffer.version.major || VERSION_MINOR != eepromBuffer.version.minor || eeprom_layout_version > eepromBuffer.eeprom_version) { - eepromBuffer.version.major = VERSION_MAJOR; - eepromBuffer.version.minor = VERSION_MINOR; - for (size_t i = 0; i < 12; i++) { - strlen(FIRMWARE_NAME) > i ? eepromBuffer.firmware_name[i] = (uint8_t)FIRMWARE_NAME[i] : 0; - } - saveEEpromSettings(); - } - - // if (eepromBuffer.use_sine_start) { - // min_startup_duty = sin_mode_min_s_d; - // } - - if (eepromBuffer.dir_reversed == 1) { - forward = 0; - } else { - forward = 1; - } - tim1_arr = TIMER1_MAX_ARR; - // startup_max_duty_cycle = startup_max_duty_cycle * TIMER1_MAX_ARR / 2000 + dead_time_override; // adjust for pwm frequency - // throttle_max_at_low_rpm = throttle_max_at_low_rpm * TIMER1_MAX_ARR / 2000; // adjust to new pwm frequency - // throttle_max_at_high_rpm = TIMER1_MAX_ARR; // adjust to new pwm frequency - if (!eepromBuffer.comp_pwm) { - eepromBuffer.use_sine_start = 0; // sine start requires complementary pwm. - } + loadEEpromSettings(); - if (eepromBuffer.rc_car_reverse) { // overrides a whole lot of things! - throttle_max_at_low_rpm = 1000; - eepromBuffer.bi_direction = 1; - eepromBuffer.use_sine_start = 0; - low_rpm_throttle_limit = 1; - eepromBuffer.variable_pwm = 0; - // eepromBuffer.stall_protection = 1; - eepromBuffer.comp_pwm = 0; - eepromBuffer.stuck_rotor_protection = 0; - minimum_duty_cycle = minimum_duty_cycle + 50; - stall_protect_minimum_duty = stall_protect_minimum_duty + 50; - min_startup_duty = min_startup_duty + 50; + if (VERSION_MAJOR != eepromBuffer.version.major || VERSION_MINOR != eepromBuffer.version.minor || eeprom_layout_version > eepromBuffer.eeprom_version) { + eepromBuffer.version.major = VERSION_MAJOR; + eepromBuffer.version.minor = VERSION_MINOR; + for (size_t i = 0; i < 12; i++) { + strlen(FIRMWARE_NAME) > i ? eepromBuffer.firmware_name[i] = (uint8_t)FIRMWARE_NAME[i] : 0; } + saveEEpromSettings(); + } + + // if (eepromBuffer.use_sine_start) { + // min_startup_duty = sin_mode_min_s_d; + // } + + if (eepromBuffer.dir_reversed == 1) { + forward = 0; + } else { + forward = 1; + } + tim1_arr = TIMER1_MAX_ARR; + // startup_max_duty_cycle = startup_max_duty_cycle * TIMER1_MAX_ARR / 2000 + dead_time_override; // adjust for pwm frequency + // throttle_max_at_low_rpm = throttle_max_at_low_rpm * TIMER1_MAX_ARR / 2000; // adjust to new pwm frequency + // throttle_max_at_high_rpm = TIMER1_MAX_ARR; // adjust to new pwm frequency + if (!eepromBuffer.comp_pwm) { + eepromBuffer.use_sine_start = 0; // sine start requires complementary pwm. + } + + if (eepromBuffer.rc_car_reverse) { // overrides a whole lot of things! + throttle_max_at_low_rpm = 1000; + eepromBuffer.bi_direction = 1; + eepromBuffer.use_sine_start = 0; + low_rpm_throttle_limit = 1; + eepromBuffer.variable_pwm = 0; + // eepromBuffer.stall_protection = 1; + eepromBuffer.comp_pwm = 0; + eepromBuffer.stuck_rotor_protection = 0; + minimum_duty_cycle = minimum_duty_cycle + 50; + stall_protect_minimum_duty = stall_protect_minimum_duty + 50; + min_startup_duty = min_startup_duty + 50; + } #ifdef MCU_F031 - GPIOF->BSRR = LL_GPIO_PIN_6; // uncomment to take bridge out of standby mode - // and set oc level - GPIOF->BRR = LL_GPIO_PIN_7; // out of standby mode - GPIOA->BRR = LL_GPIO_PIN_11; + GPIOF->BSRR = LL_GPIO_PIN_6; // uncomment to take bridge out of standby mode + // and set oc level + GPIOF->BRR = LL_GPIO_PIN_7; // out of standby mode + GPIOA->BRR = LL_GPIO_PIN_11; #endif #ifdef USE_LED_STRIP - send_LED_RGB(125, 0, 0); + send_LED_RGB(125, 0, 0); #endif #ifdef USE_CRSF_INPUT - inputSet = 1; - playStartupTune(); - MX_IWDG_Init(); - LL_IWDG_ReloadCounter(IWDG); + inputSet = 1; + playStartupTune(); + MX_IWDG_Init(); + LL_IWDG_ReloadCounter(IWDG); #else #if defined(FIXED_DUTY_MODE) || defined(FIXED_SPEED_MODE) - MX_IWDG_Init(); - RELOAD_WATCHDOG_COUNTER(); - inputSet = 1; - armed = 1; - adjusted_input = 48; - newinput = 48; - comStep(2); + MX_IWDG_Init(); + RELOAD_WATCHDOG_COUNTER(); + inputSet = 1; + armed = 1; + adjusted_input = 48; + newinput = 48; + comStep(2); #ifdef FIXED_SPEED_MODE - use_speed_control_loop = 1; - eepromBuffer.use_sine_start = 0; - target_e_com_time = 60000000 / FIXED_SPEED_MODE_RPM / (eepromBuffer.motor_poles / 2); - input = 48; + use_speed_control_loop = 1; + eepromBuffer.use_sine_start = 0; + target_e_com_time = 60000000 / FIXED_SPEED_MODE_RPM / (eepromBuffer.motor_poles / 2); + input = 48; #endif #else #ifdef BRUSHED_MODE - // bi_direction = 1; - commutation_interval = 5000; - eepromBuffer.use_sine_start = 0; - maskPhaseInterrupts(); - playBrushedStartupTune(); + // bi_direction = 1; + commutation_interval = 5000; + eepromBuffer.use_sine_start = 0; + maskPhaseInterrupts(); + playBrushedStartupTune(); +#else +#ifdef MCU_AT415 + play_tone_flag = 5; #else - #ifdef MCU_AT415 - play_tone_flag = 5; - #else - playStartupTune(); - #endif + playStartupTune(); #endif - zero_input_count = 0; - MX_IWDG_Init(); - RELOAD_WATCHDOG_COUNTER(); +#endif + zero_input_count = 0; + MX_IWDG_Init(); + RELOAD_WATCHDOG_COUNTER(); #ifdef GIMBAL_MODE - eepromBuffer.bi_direction = 1; - eepromBuffer.use_sine_start = 1; + eepromBuffer.bi_direction = 1; + eepromBuffer.use_sine_start = 1; #endif #ifdef USE_ADC_INPUT - armed_count_threshold = 5000; - inputSet = 1; + armed_count_threshold = 5000; + inputSet = 1; #else - // checkForHighSignal(); // will reboot if signal line is high for 10ms + // checkForHighSignal(); // will reboot if signal line is high for 10ms - receiveDshotDma(); - if (drive_by_rpm) { - use_speed_control_loop = 1; - } + receiveDshotDma(); + if (drive_by_rpm) { + use_speed_control_loop = 1; + } #endif #endif // end fixed duty mode ifdef #endif // end crsf input #ifdef MCU_F051 - MCU_Id = DBGMCU->IDCODE &= 0xFFF; - REV_Id = DBGMCU->IDCODE >> 16; + MCU_Id = DBGMCU->IDCODE &= 0xFFF; + REV_Id = DBGMCU->IDCODE >> 16; - if (REV_Id >= 4096) { - temperature_offset = 0; - } else { - temperature_offset = 230; - } + if (REV_Id >= 4096) { + temperature_offset = 0; + } else { + temperature_offset = 230; + } #endif #ifdef NEUTRONRC_G071 - setInputPullDown(); + setInputPullDown(); #else - setInputPullUp(); + setInputPullUp(); #endif #ifdef USE_INVERTED_HIGH @@ -1752,390 +1755,390 @@ int main(void) #endif - while (1) { + while (1) { #if defined(FIXED_DUTY_MODE) || defined(FIXED_SPEED_MODE) - setInput(); + setInput(); #endif #ifdef MCU_F031 - if (input_ready) { - processDshot(); - input_ready = 0; - } + if (input_ready) { + processDshot(); + input_ready = 0; + } #endif -if(zero_crosses < 5){ - min_bemf_counts_up = TARGET_MIN_BEMF_COUNTS * 2; - min_bemf_counts_down = TARGET_MIN_BEMF_COUNTS * 2; -}else{ - min_bemf_counts_up = TARGET_MIN_BEMF_COUNTS; - min_bemf_counts_down = TARGET_MIN_BEMF_COUNTS; -} - RELOAD_WATCHDOG_COUNTER(); - e_com_time = ((commutation_intervals[0] + commutation_intervals[1] + commutation_intervals[2] + commutation_intervals[3] + commutation_intervals[4] + commutation_intervals[5]) + 4) >> 1; // COMMUTATION INTERVAL IS 0.5US INCREMENTS - if (eepromBuffer.variable_pwm) { - tim1_arr = map(commutation_interval, 96, 200, TIMER1_MAX_ARR / 2, - TIMER1_MAX_ARR); + if (zero_crosses < 5) { + min_bemf_counts_up = TARGET_MIN_BEMF_COUNTS * 2; + min_bemf_counts_down = TARGET_MIN_BEMF_COUNTS * 2; + } else { + min_bemf_counts_up = TARGET_MIN_BEMF_COUNTS; + min_bemf_counts_down = TARGET_MIN_BEMF_COUNTS; + } + RELOAD_WATCHDOG_COUNTER(); + e_com_time = ((commutation_intervals[0] + commutation_intervals[1] + commutation_intervals[2] + commutation_intervals[3] + commutation_intervals[4] + commutation_intervals[5]) + 4) >> 1; // COMMUTATION INTERVAL IS 0.5US INCREMENTS + if (eepromBuffer.variable_pwm) { + tim1_arr = map(commutation_interval, 96, 200, TIMER1_MAX_ARR / 2, + TIMER1_MAX_ARR); + } + if (signaltimeout > (LOOP_FREQUENCY_HZ >> 1)) { // half second timeout when armed; + if (armed) { + allOff(); + armed = 0; + input = 0; + inputSet = 0; + zero_input_count = 0; + SET_DUTY_CYCLE_ALL(0); + resetInputCaptureTimer(); + for (int i = 0; i < 64; i++) { + dma_buffer[i] = 0; } - if (signaltimeout > (LOOP_FREQUENCY_HZ >> 1)) { // half second timeout when armed; - if (armed) { - allOff(); - armed = 0; - input = 0; - inputSet = 0; - zero_input_count = 0; - SET_DUTY_CYCLE_ALL(0); - resetInputCaptureTimer(); - for (int i = 0; i < 64; i++) { - dma_buffer[i] = 0; - } - NVIC_SystemReset(); - } - if (signaltimeout > LOOP_FREQUENCY_HZ << 1) { // 2 second when not armed - allOff(); - armed = 0; - input = 0; - inputSet = 0; - zero_input_count = 0; - SET_DUTY_CYCLE_ALL(0); - resetInputCaptureTimer(); - for (int i = 0; i < 64; i++) { - dma_buffer[i] = 0; - } - NVIC_SystemReset(); - } + NVIC_SystemReset(); + } + if (signaltimeout > LOOP_FREQUENCY_HZ << 1) { // 2 second when not armed + allOff(); + armed = 0; + input = 0; + inputSet = 0; + zero_input_count = 0; + SET_DUTY_CYCLE_ALL(0); + resetInputCaptureTimer(); + for (int i = 0; i < 64; i++) { + dma_buffer[i] = 0; } + NVIC_SystemReset(); + } + } #ifdef USE_CUSTOM_LED - if ((input >= 47) && (input < 1947)) { - if (ledcounter > (2000 >> forward)) { - GPIOB->BSRR = LL_GPIO_PIN_3; - } else { - GPIOB->BRR = LL_GPIO_PIN_3; - } - if (ledcounter > (4000 >> forward)) { - ledcounter = 0; - } - } - if (input > 1947) { - GPIOB->BSRR = LL_GPIO_PIN_3; - } - if (input < 47) { - GPIOB->BRR = LL_GPIO_PIN_3; - } + if ((input >= 47) && (input < 1947)) { + if (ledcounter > (2000 >> forward)) { + GPIOB->BSRR = LL_GPIO_PIN_3; + } else { + GPIOB->BRR = LL_GPIO_PIN_3; + } + if (ledcounter > (4000 >> forward)) { + ledcounter = 0; + } + } + if (input > 1947) { + GPIOB->BSRR = LL_GPIO_PIN_3; + } + if (input < 47) { + GPIOB->BRR = LL_GPIO_PIN_3; + } #endif - if (tenkhzcounter > LOOP_FREQUENCY_HZ) { // 1s sample interval 10000 - consumed_current = (float)actual_current / 360 + consumed_current; - switch (dshot_extended_telemetry) { - - case 1: - send_extended_dshot = 0b0010 << 8 | degrees_celsius; - dshot_extended_telemetry = 2; - break; - case 2: - send_extended_dshot = 0b0110 << 8 | (uint8_t)actual_current / 50; - dshot_extended_telemetry = 3; - break; - case 3: - send_extended_dshot = 0b0100 << 8 | (uint8_t)(battery_voltage / 25); - dshot_extended_telemetry = 1; - break; - } - tenkhzcounter = 0; - } + if (tenkhzcounter > LOOP_FREQUENCY_HZ) { // 1s sample interval 10000 + consumed_current = (float)actual_current / 360 + consumed_current; + switch (dshot_extended_telemetry) { + + case 1: + send_extended_dshot = 0b0010 << 8 | degrees_celsius; + dshot_extended_telemetry = 2; + break; + case 2: + send_extended_dshot = 0b0110 << 8 | (uint8_t)actual_current / 50; + dshot_extended_telemetry = 3; + break; + case 3: + send_extended_dshot = 0b0100 << 8 | (uint8_t)(battery_voltage / 25); + dshot_extended_telemetry = 1; + break; + } + tenkhzcounter = 0; + } #ifndef BRUSHED_MODE - if ((zero_crosses > 1000) || (adjusted_input == 0)) { - bemf_timeout_happened = 0; - } - if (zero_crosses > 100 && adjusted_input < 200) { - bemf_timeout_happened = 0; - } - if (eepromBuffer.use_sine_start && adjusted_input < 160) { - bemf_timeout_happened = 0; - } + if ((zero_crosses > 1000) || (adjusted_input == 0)) { + bemf_timeout_happened = 0; + } + if (zero_crosses > 100 && adjusted_input < 200) { + bemf_timeout_happened = 0; + } + if (eepromBuffer.use_sine_start && adjusted_input < 160) { + bemf_timeout_happened = 0; + } - if (crawler_mode) { - if (adjusted_input < 400) { - bemf_timeout_happened = 0; - } - } else { - if (adjusted_input < 150) { // startup duty cycle should be low enough to not burn motor - bemf_timeout = 100; - } else { - bemf_timeout = 10; - } - } + if (crawler_mode) { + if (adjusted_input < 400) { + bemf_timeout_happened = 0; + } + } else { + if (adjusted_input < 150) { // startup duty cycle should be low enough to not burn motor + bemf_timeout = 100; + } else { + bemf_timeout = 10; + } + } #endif - average_interval = e_com_time / 3; - if (desync_check && zero_crosses > 10) { - if ((getAbsDif(last_average_interval, average_interval) > average_interval >> 1) && (average_interval < 2000)) { // throttle resitricted before zc 20. - zero_crosses = 0; - desync_happened++; - if ((!eepromBuffer.bi_direction && (input > 47)) || commutation_interval > 1000) { - running = 0; - } - old_routine = 1; - if (zero_crosses > 100) { - average_interval = 5000; - } - last_duty_cycle = min_startup_duty / 2; - } - desync_check = 0; - // } - last_average_interval = average_interval; + average_interval = e_com_time / 3; + if (desync_check && zero_crosses > 10) { + if ((getAbsDif(last_average_interval, average_interval) > average_interval >> 1) && (average_interval < 2000)) { // throttle resitricted before zc 20. + zero_crosses = 0; + desync_happened++; + if ((!eepromBuffer.bi_direction && (input > 47)) || commutation_interval > 1000) { + running = 0; + } + old_routine = 1; + if (zero_crosses > 100) { + average_interval = 5000; } + last_duty_cycle = min_startup_duty / 2; + } + desync_check = 0; + // } + last_average_interval = average_interval; + } #ifndef MCU_F031 - if (dshot_telemetry && (commutation_interval > DSHOT_PRIORITY_THRESHOLD)) { - NVIC_SetPriority(IC_DMA_IRQ_NAME, 0); - NVIC_SetPriority(COM_TIMER_IRQ, 1); - NVIC_SetPriority(COMPARATOR_IRQ, 1); - } else { - NVIC_SetPriority(IC_DMA_IRQ_NAME, 1); - NVIC_SetPriority(COM_TIMER_IRQ, 0); - NVIC_SetPriority(COMPARATOR_IRQ, 0); - } + if (dshot_telemetry && (commutation_interval > DSHOT_PRIORITY_THRESHOLD)) { + NVIC_SetPriority(IC_DMA_IRQ_NAME, 0); + NVIC_SetPriority(COM_TIMER_IRQ, 1); + NVIC_SetPriority(COMPARATOR_IRQ, 1); + } else { + NVIC_SetPriority(IC_DMA_IRQ_NAME, 1); + NVIC_SetPriority(COM_TIMER_IRQ, 0); + NVIC_SetPriority(COMPARATOR_IRQ, 0); + } #endif - if (send_telemetry) { + if (send_telemetry) { #ifdef USE_SERIAL_TELEMETRY - makeTelemPackage(degrees_celsius, battery_voltage, actual_current, - (uint16_t)consumed_current, e_rpm); - send_telem_DMA(); - send_telemetry = 0; + makeTelemPackage(degrees_celsius, battery_voltage, actual_current, + (uint16_t)consumed_current, e_rpm); + send_telem_DMA(); + send_telemetry = 0; #endif - } - adc_counter++; - if (adc_counter > 200) { // for adc and telemetry + } + adc_counter++; + if (adc_counter > 200) { // for adc and telemetry #if defined(STMICRO) - ADC_DMA_Callback(); - LL_ADC_REG_StartConversion(ADC1); - converted_degrees = __LL_ADC_CALC_TEMPERATURE(3300, ADC_raw_temp, LL_ADC_RESOLUTION_12B); + ADC_DMA_Callback(); + LL_ADC_REG_StartConversion(ADC1); + converted_degrees = __LL_ADC_CALC_TEMPERATURE(3300, ADC_raw_temp, LL_ADC_RESOLUTION_12B); #endif #ifdef MCU_GDE23 - ADC_DMA_Callback(); - converted_degrees = (1.43 - ADC_raw_temp * 3.3 / 4096) * 1000 / 4.3 + 25; - adc_software_trigger_enable(ADC_REGULAR_CHANNEL); + ADC_DMA_Callback(); + converted_degrees = (1.43 - ADC_raw_temp * 3.3 / 4096) * 1000 / 4.3 + 25; + adc_software_trigger_enable(ADC_REGULAR_CHANNEL); #endif #ifdef ARTERY - ADC_DMA_Callback(); - adc_ordinary_software_trigger_enable(ADC1, TRUE); - converted_degrees = getConvertedDegrees(ADC_raw_temp); + ADC_DMA_Callback(); + adc_ordinary_software_trigger_enable(ADC1, TRUE); + converted_degrees = getConvertedDegrees(ADC_raw_temp); #endif - degrees_celsius = converted_degrees; - battery_voltage = ((7 * battery_voltage) + ((ADC_raw_volts * 3300 / 4095 * VOLTAGE_DIVIDER) / 100)) >> 3; - smoothed_raw_current = getSmoothedCurrent(); - actual_current = ((smoothed_raw_current * 3300 / 41) - (CURRENT_OFFSET * 100)) / (MILLIVOLT_PER_AMP); - if (actual_current < 0) { - actual_current = 0; - } - if (LOW_VOLTAGE_CUTOFF) { - if (battery_voltage < (cell_count * low_cell_volt_cutoff)) { - low_voltage_count++; - if (low_voltage_count > (20000 - (stepper_sine * 900))) { - input = 0; - allOff(); - maskPhaseInterrupts(); - running = 0; - zero_input_count = 0; - armed = 0; - } - } else { - low_voltage_count = 0; - } - } - adc_counter = 0; + degrees_celsius = converted_degrees; + battery_voltage = ((7 * battery_voltage) + ((ADC_raw_volts * 3300 / 4095 * VOLTAGE_DIVIDER) / 100)) >> 3; + smoothed_raw_current = getSmoothedCurrent(); + actual_current = ((smoothed_raw_current * 3300 / 41) - (CURRENT_OFFSET * 100)) / (MILLIVOLT_PER_AMP); + if (actual_current < 0) { + actual_current = 0; + } + if (LOW_VOLTAGE_CUTOFF) { + if (battery_voltage < (cell_count * low_cell_volt_cutoff)) { + low_voltage_count++; + if (low_voltage_count > (20000 - (stepper_sine * 900))) { + input = 0; + allOff(); + maskPhaseInterrupts(); + running = 0; + zero_input_count = 0; + armed = 0; + } + } else { + low_voltage_count = 0; + } + } + adc_counter = 0; #ifdef USE_ADC_INPUT - if (ADC_raw_input < 10) { - zero_input_count++; - } else { - zero_input_count = 0; - } + if (ADC_raw_input < 10) { + zero_input_count++; + } else { + zero_input_count = 0; + } #endif - } + } #ifdef USE_ADC_INPUT - signaltimeout = 0; - ADC_smoothed_input = (((10 * ADC_smoothed_input) + ADC_raw_input) / 11); - newinput = ADC_smoothed_input / 2; - if (newinput > 2000) { - newinput = 2000; - } + signaltimeout = 0; + ADC_smoothed_input = (((10 * ADC_smoothed_input) + ADC_raw_input) / 11); + newinput = ADC_smoothed_input / 2; + if (newinput > 2000) { + newinput = 2000; + } #endif - stuckcounter = 0; - if (stepper_sine == 0) { - - e_rpm = running * (600000 / e_com_time); // in tens of rpm - k_erpm = e_rpm / 10; // ecom time is time for one electrical revolution in microseconds - - if (low_rpm_throttle_limit) { // some hardware doesn't need this, its on - // by default to keep hardware / motors - // protected but can slow down the response - // in the very low end a little. - duty_cycle_maximum = map(k_erpm, low_rpm_level, high_rpm_level, throttle_max_at_low_rpm, - throttle_max_at_high_rpm); // for more performance lower the - // high_rpm_level, set to a - // consvervative number in source. - }else{ - duty_cycle_maximum = 2000; - } - - if (degrees_celsius > eepromBuffer.limits.temperature) { - duty_cycle_maximum = map(degrees_celsius, eepromBuffer.limits.temperature - 10, eepromBuffer.limits.temperature + 10, - throttle_max_at_high_rpm / 2, 1); - } - if (zero_crosses < 100 && commutation_interval > 500) { + stuckcounter = 0; + if (stepper_sine == 0) { + + e_rpm = running * (600000 / e_com_time); // in tens of rpm + k_erpm = e_rpm / 10; // ecom time is time for one electrical revolution in microseconds + + if (low_rpm_throttle_limit) { // some hardware doesn't need this, its on + // by default to keep hardware / motors + // protected but can slow down the response + // in the very low end a little. + duty_cycle_maximum = map(k_erpm, low_rpm_level, high_rpm_level, throttle_max_at_low_rpm, + throttle_max_at_high_rpm); // for more performance lower the + // high_rpm_level, set to a + // consvervative number in source. + } else { + duty_cycle_maximum = 2000; + } + + if (degrees_celsius > eepromBuffer.limits.temperature) { + duty_cycle_maximum = map(degrees_celsius, eepromBuffer.limits.temperature - 10, eepromBuffer.limits.temperature + 10, + throttle_max_at_high_rpm / 2, 1); + } + if (zero_crosses < 100 && commutation_interval > 500) { #ifdef MCU_G071 - TIM1->CCR5 = 1; // comparator blanking - filter_level = 8; + TIM1->CCR5 = 1; // comparator blanking + filter_level = 8; #else - filter_level = 12; + filter_level = 12; #endif - } else { + } else { #ifdef MCU_G071 - TIM1->CCR5 = 10; + TIM1->CCR5 = 10; #endif - filter_level = map(average_interval, 100, 500, 3, 12); - } - if (commutation_interval < 50) { - filter_level = 2; - } + filter_level = map(average_interval, 100, 500, 3, 12); + } + if (commutation_interval < 50) { + filter_level = 2; + } - if (eepromBuffer.auto_advance) { - auto_advance_level = map(duty_cycle, 100, 2000, 13, 23); - } + if (eepromBuffer.auto_advance) { + auto_advance_level = map(duty_cycle, 100, 2000, 13, 23); + } - /**************** old routine*********************/ + /**************** old routine*********************/ #ifdef CUSTOM_RAMP - if (old_routine && running) { - maskPhaseInterrupts(); - getBemfState(); - if (!zcfound) { - if (rising) { - if (bemfcounter > min_bemf_counts_up) { - zcfound = 1; - zcfoundroutine(); - } - } else { - if (bemfcounter > min_bemf_counts_down) { - zcfound = 1; - zcfoundroutine(); - } - } - } + if (old_routine && running) { + maskPhaseInterrupts(); + getBemfState(); + if (!zcfound) { + if (rising) { + if (bemfcounter > min_bemf_counts_up) { + zcfound = 1; + zcfoundroutine(); } -#endif - if (INTERVAL_TIMER_COUNT > 45000 && running == 1) { - bemf_timeout_happened++; - - maskPhaseInterrupts(); - old_routine = 1; - if (input < 48) { - running = 0; - commutation_interval = 5000; - } - zero_crosses = 0; - zcfoundroutine(); + } else { + if (bemfcounter > min_bemf_counts_down) { + zcfound = 1; + zcfoundroutine(); } - } else { // stepper sine + } + } + } +#endif + if (INTERVAL_TIMER_COUNT > 45000 && running == 1) { + bemf_timeout_happened++; + + maskPhaseInterrupts(); + old_routine = 1; + if (input < 48) { + running = 0; + commutation_interval = 5000; + } + zero_crosses = 0; + zcfoundroutine(); + } + } else { // stepper sine #ifdef GIMBAL_MODE - step_delay = 300; + step_delay = 300; + maskPhaseInterrupts(); + allpwm(); + if (newinput > 1000) { + desired_angle = map(newinput, 1000, 2000, 180, 360); + } else { + desired_angle = map(newinput, 0, 1000, 0, 180); + } + if (current_angle > desired_angle) { + forward = 1; + advanceincrement(); + delayMicros(step_delay); + current_angle--; + } + if (current_angle < desired_angle) { + forward = 0; + advanceincrement(); + delayMicros(step_delay); + current_angle++; + } +#else + + if (input > 48 && armed) { + + if (input > 48 && input < 137) { // sine wave stepper + + if (do_once_sinemode) { + // disable commutation interrupt in case set + DISABLE_COM_TIMER_INT(); maskPhaseInterrupts(); + SET_DUTY_CYCLE_ALL(0); allpwm(); - if (newinput > 1000) { - desired_angle = map(newinput, 1000, 2000, 180, 360); - } else { - desired_angle = map(newinput, 0, 1000, 0, 180); - } - if (current_angle > desired_angle) { - forward = 1; - advanceincrement(); - delayMicros(step_delay); - current_angle--; - } - if (current_angle < desired_angle) { - forward = 0; - advanceincrement(); - delayMicros(step_delay); - current_angle++; - } -#else + do_once_sinemode = 0; + } + advanceincrement(); + step_delay = map(input, 48, 120, 7000 / eepromBuffer.motor_poles, 810 / eepromBuffer.motor_poles); + delayMicros(step_delay); + e_rpm = 600 / step_delay; // in hundreds so 33 e_rpm is 3300 actual erpm - if (input > 48 && armed) { - - if (input > 48 && input < 137) { // sine wave stepper - - if (do_once_sinemode) { - // disable commutation interrupt in case set - DISABLE_COM_TIMER_INT(); - maskPhaseInterrupts(); - SET_DUTY_CYCLE_ALL(0); - allpwm(); - do_once_sinemode = 0; - } - advanceincrement(); - step_delay = map(input, 48, 120, 7000 / eepromBuffer.motor_poles, 810 / eepromBuffer.motor_poles); - delayMicros(step_delay); - e_rpm = 600 / step_delay; // in hundreds so 33 e_rpm is 3300 actual erpm - - } else { - do_once_sinemode = 1; - advanceincrement(); - if (input > 200) { - phase_A_position = 0; - step_delay = 80; - } - - delayMicros(step_delay); - if (phase_A_position == 0) { - stepper_sine = 0; - running = 1; - old_routine = 1; - commutation_interval = 9000; - average_interval = 9000; - last_average_interval = average_interval; - SET_INTERVAL_TIMER_COUNT(9000); - zero_crosses = 20; - prop_brake_active = 0; - step = changeover_step; - // comStep(step);// rising bemf on a same as position 0. - if (eepromBuffer.stall_protection) { - last_duty_cycle = stall_protect_minimum_duty; - } - commutate(); - generatePwmTimerEvent(); - } - } + } else { + do_once_sinemode = 1; + advanceincrement(); + if (input > 200) { + phase_A_position = 0; + step_delay = 80; + } + + delayMicros(step_delay); + if (phase_A_position == 0) { + stepper_sine = 0; + running = 1; + old_routine = 1; + commutation_interval = 9000; + average_interval = 9000; + last_average_interval = average_interval; + SET_INTERVAL_TIMER_COUNT(9000); + zero_crosses = 20; + prop_brake_active = 0; + step = changeover_step; + // comStep(step);// rising bemf on a same as position 0. + if (eepromBuffer.stall_protection) { + last_duty_cycle = stall_protect_minimum_duty; + } + commutate(); + generatePwmTimerEvent(); + } + } - } else { - do_once_sinemode = 1; - if (eepromBuffer.brake_on_stop) { + } else { + do_once_sinemode = 1; + if (eepromBuffer.brake_on_stop) { #ifndef PWM_ENABLE_BRIDGE - duty_cycle = (TIMER1_MAX_ARR - 19) + eepromBuffer.drag_brake_strength * 2; - adjusted_duty_cycle = TIMER1_MAX_ARR - ((duty_cycle * tim1_arr) / TIMER1_MAX_ARR) + 1; - proportionalBrake(); - SET_DUTY_CYCLE_ALL(adjusted_duty_cycle); - prop_brake_active = 1; + duty_cycle = (TIMER1_MAX_ARR - 19) + eepromBuffer.drag_brake_strength * 2; + adjusted_duty_cycle = TIMER1_MAX_ARR - ((duty_cycle * tim1_arr) / TIMER1_MAX_ARR) + 1; + proportionalBrake(); + SET_DUTY_CYCLE_ALL(adjusted_duty_cycle); + prop_brake_active = 1; #else - // todo add braking for PWM /enable style bridges. + // todo add braking for PWM /enable style bridges. #endif - } else { - SET_DUTY_CYCLE_ALL(0); - allOff(); - } - e_rpm = 0; - } + } else { + SET_DUTY_CYCLE_ALL(0); + allOff(); + } + e_rpm = 0; + } #endif // gimbal mode - } // stepper/sine mode end + } // stepper/sine mode end #ifdef BRUSHED_MODE - runBrushedLoop(); + runBrushedLoop(); #endif #if DRONECAN_SUPPORT - DroneCAN_update(); + DroneCAN_update(); #endif - } + } } #ifdef USE_FULL_ASSERT @@ -2148,10 +2151,10 @@ if(zero_crosses < 5){ */ void assert_failed(uint8_t* file, uint32_t line) { - /* USER CODE BEGIN 6 */ - /* User can add his own implementation to report the file name and line - number, tex: printf("Wrong parameters value: file %s on line %d\r\n", file, - line) */ - /* USER CODE END 6 */ + /* USER CODE BEGIN 6 */ + /* User can add his own implementation to report the file name and line + number, tex: printf("Wrong parameters value: file %s on line %d\r\n", file, + line) */ + /* USER CODE END 6 */ } #endif /* USE_FULL_ASSERT */ diff --git a/Src/signal.c b/Src/signal.c index 4f96bcaf..e7e9a70c 100644 --- a/Src/signal.c +++ b/Src/signal.c @@ -34,229 +34,229 @@ uint16_t dshot_frametime_low = 0; void computeMSInput() { - int lastnumber = dma_buffer[0]; - for (int j = 1; j < 2; j++) { + int lastnumber = dma_buffer[0]; + for (int j = 1; j < 2; j++) { - if (((dma_buffer[j] - lastnumber) < 1500) && ((dma_buffer[j] - lastnumber) > 0)) { // blank space + if (((dma_buffer[j] - lastnumber) < 1500) && ((dma_buffer[j] - lastnumber) > 0)) { // blank space - newinput = map((dma_buffer[j] - lastnumber), 243, 1200, 0, 2000); - break; - } - lastnumber = dma_buffer[j]; + newinput = map((dma_buffer[j] - lastnumber), 243, 1200, 0, 2000); + break; } + lastnumber = dma_buffer[j]; + } } void computeServoInput() { - if (((dma_buffer[1] - dma_buffer[0]) > 800) && ((dma_buffer[1] - dma_buffer[0]) < 2200)) { - signaltimeout = 0; - if (calibration_required) { - if (!high_calibration_set) { - if (high_calibration_counts == 0) { - last_high_threshold = dma_buffer[1] - dma_buffer[0]; - } - high_calibration_counts++; - if (getAbsDif(last_high_threshold, servo_high_threshold) > 50) { - calibration_required = 0; - } else { - servo_high_threshold = ((7 * servo_high_threshold + (dma_buffer[1] - dma_buffer[0])) >> 3); - if (high_calibration_counts > 50) { - servo_high_threshold = servo_high_threshold - 25; - eepromBuffer.servo.high_threshold = (servo_high_threshold - 1750) / 2; - high_calibration_set = 1; - playDefaultTone(); - } - } - last_high_threshold = servo_high_threshold; - } - if (high_calibration_set) { - if (dma_buffer[1] - dma_buffer[0] < 1250) { - low_calibration_counts++; - servo_low_threshold = ((7 * servo_low_threshold + (dma_buffer[1] - dma_buffer[0])) >> 3); - } - if (low_calibration_counts > 75) { - servo_low_threshold = servo_low_threshold + 25; - eepromBuffer.servo.low_threshold = (servo_low_threshold - 750) / 2; - calibration_required = 0; - saveEEpromSettings(); - low_calibration_counts = 0; - playChangedTone(); - } - } - signaltimeout = 0; + if (((dma_buffer[1] - dma_buffer[0]) > 800) && ((dma_buffer[1] - dma_buffer[0]) < 2200)) { + signaltimeout = 0; + if (calibration_required) { + if (!high_calibration_set) { + if (high_calibration_counts == 0) { + last_high_threshold = dma_buffer[1] - dma_buffer[0]; + } + high_calibration_counts++; + if (getAbsDif(last_high_threshold, servo_high_threshold) > 50) { + calibration_required = 0; } else { - if (eepromBuffer.bi_direction) { - if (dma_buffer[1] - dma_buffer[0] <= servo_neutral) { - servorawinput = map((dma_buffer[1] - dma_buffer[0]), - servo_low_threshold, servo_neutral, 0, 1000); - } else { - servorawinput = map((dma_buffer[1] - dma_buffer[0]), servo_neutral + 1, - servo_high_threshold, 1001, 2000); - } - } else { - servorawinput = map((dma_buffer[1] - dma_buffer[0]), servo_low_threshold, - servo_high_threshold, 47, 2047); - if (servorawinput <= 48) { - servorawinput = 0; - } - } - signaltimeout = 0; + servo_high_threshold = ((7 * servo_high_threshold + (dma_buffer[1] - dma_buffer[0])) >> 3); + if (high_calibration_counts > 50) { + servo_high_threshold = servo_high_threshold - 25; + eepromBuffer.servo.high_threshold = (servo_high_threshold - 1750) / 2; + high_calibration_set = 1; + playDefaultTone(); + } + } + last_high_threshold = servo_high_threshold; + } + if (high_calibration_set) { + if (dma_buffer[1] - dma_buffer[0] < 1250) { + low_calibration_counts++; + servo_low_threshold = ((7 * servo_low_threshold + (dma_buffer[1] - dma_buffer[0])) >> 3); + } + if (low_calibration_counts > 75) { + servo_low_threshold = servo_low_threshold + 25; + eepromBuffer.servo.low_threshold = (servo_low_threshold - 750) / 2; + calibration_required = 0; + saveEEpromSettings(); + low_calibration_counts = 0; + playChangedTone(); } + } + signaltimeout = 0; } else { - zero_input_count = 0; // reset if out of range + if (eepromBuffer.bi_direction) { + if (dma_buffer[1] - dma_buffer[0] <= servo_neutral) { + servorawinput = map((dma_buffer[1] - dma_buffer[0]), + servo_low_threshold, servo_neutral, 0, 1000); + } else { + servorawinput = map((dma_buffer[1] - dma_buffer[0]), servo_neutral + 1, + servo_high_threshold, 1001, 2000); + } + } else { + servorawinput = map((dma_buffer[1] - dma_buffer[0]), servo_low_threshold, + servo_high_threshold, 47, 2047); + if (servorawinput <= 48) { + servorawinput = 0; + } + } + signaltimeout = 0; } + } else { + zero_input_count = 0; // reset if out of range + } - if (servorawinput - newinput > max_servo_deviation) { - newinput += max_servo_deviation; - } else if (newinput - servorawinput > max_servo_deviation) { - newinput -= max_servo_deviation; - } else { - newinput = servorawinput; - } + if (servorawinput - newinput > max_servo_deviation) { + newinput += max_servo_deviation; + } else if (newinput - servorawinput > max_servo_deviation) { + newinput -= max_servo_deviation; + } else { + newinput = servorawinput; + } } void transfercomplete() { - if (armed && dshot_telemetry) { - if (out_put) { - receiveDshotDma(); - compute_dshot_flag = 2; - return; - } else { - sendDshotDma(); - compute_dshot_flag = 1; - return; - } + if (armed && dshot_telemetry) { + if (out_put) { + receiveDshotDma(); + compute_dshot_flag = 2; + return; + } else { + sendDshotDma(); + compute_dshot_flag = 1; + return; } - if (inputSet == 0) { - detectInput(); + } + if (inputSet == 0) { + detectInput(); + receiveDshotDma(); + return; + } + if (inputSet == 1) { + + if (dshot_telemetry) { + if (out_put) { + make_dshot_package(e_com_time); + computeDshotDMA(); receiveDshotDma(); return; - } - if (inputSet == 1) { + } else { + sendDshotDma(); + return; + } + } else { - if (dshot_telemetry) { - if (out_put) { - make_dshot_package(e_com_time); - computeDshotDMA(); - receiveDshotDma(); - return; - } else { - sendDshotDma(); - return; - } + if (dshot == 1) { + computeDshotDMA(); + receiveDshotDma(); + } + if (servoPwm == 1) { + if (getInputPinState()) { + buffersize = 3; } else { - - if (dshot == 1) { - computeDshotDMA(); - receiveDshotDma(); - } - if (servoPwm == 1) { - if (getInputPinState()) { - buffersize = 3; - } else { - buffersize = 2; - computeServoInput(); - } - receiveDshotDma(); - } + buffersize = 2; + computeServoInput(); + } + receiveDshotDma(); + } + } + if (!armed) { + if (dshot && (average_count < 8) && (zero_input_count > 5)) { + average_count++; + average_packet_length = average_packet_length + (dma_buffer[31] - dma_buffer[0]); + if (average_count == 8) { + dshot_frametime_high = (average_packet_length >> 3) + (average_packet_length >> 7); + dshot_frametime_low = (average_packet_length >> 3) - (average_packet_length >> 7); } - if (!armed) { - if (dshot && (average_count < 8) && (zero_input_count > 5)) { - average_count++; - average_packet_length = average_packet_length + (dma_buffer[31] - dma_buffer[0]); - if (average_count == 8) { - dshot_frametime_high = (average_packet_length >> 3) + (average_packet_length >> 7); - dshot_frametime_low = (average_packet_length >> 3) - (average_packet_length >> 7); - } - } - if (adjusted_input == 0 && calibration_required == 0) { // note this in input..not newinput so it - // will be adjusted be main loop - zero_input_count++; - } else { - zero_input_count = 0; - if (adjusted_input > 1500) { - if (getAbsDif(adjusted_input, last_input) > 50) { - enter_calibration_count = 0; - } else { - enter_calibration_count++; - } + } + if (adjusted_input == 0 && calibration_required == 0) { // note this in input..not newinput so it + // will be adjusted be main loop + zero_input_count++; + } else { + zero_input_count = 0; + if (adjusted_input > 1500) { + if (getAbsDif(adjusted_input, last_input) > 50) { + enter_calibration_count = 0; + } else { + enter_calibration_count++; + } - if (enter_calibration_count > 50 && (!high_calibration_set)) { - playBeaconTune3(); - calibration_required = 1; - enter_calibration_count = 0; - } - last_input = adjusted_input; - } - } + if (enter_calibration_count > 50 && (!high_calibration_set)) { + playBeaconTune3(); + calibration_required = 1; + enter_calibration_count = 0; + } + last_input = adjusted_input; } + } } + } } void checkDshot() { - if ((smallestnumber >= 1) && (smallestnumber < 4) && (average_signal_pulse < 60)) { - ic_timer_prescaler = 0; - if (CPU_FREQUENCY_MHZ > 100) { - output_timer_prescaler = 1; - } else { - output_timer_prescaler = 0; - } - // dshot_runout_timer = 1000; - dshot = 1; - buffer_padding = 14; - buffersize = 32; - inputSet = 1; + if ((smallestnumber >= 1) && (smallestnumber < 4) && (average_signal_pulse < 60)) { + ic_timer_prescaler = 0; + if (CPU_FREQUENCY_MHZ > 100) { + output_timer_prescaler = 1; + } else { + output_timer_prescaler = 0; } - if ((smallestnumber >= 4) && (smallestnumber <= 8) && (average_signal_pulse < 100)) { - dshot = 1; - ic_timer_prescaler = 1; - if (CPU_FREQUENCY_MHZ > 100) { - output_timer_prescaler = 3; - } else { - output_timer_prescaler = 1; - } - buffer_padding = 7; - buffersize = 32; - inputSet = 1; + // dshot_runout_timer = 1000; + dshot = 1; + buffer_padding = 14; + buffersize = 32; + inputSet = 1; + } + if ((smallestnumber >= 4) && (smallestnumber <= 8) && (average_signal_pulse < 100)) { + dshot = 1; + ic_timer_prescaler = 1; + if (CPU_FREQUENCY_MHZ > 100) { + output_timer_prescaler = 3; + } else { + output_timer_prescaler = 1; } + buffer_padding = 7; + buffersize = 32; + inputSet = 1; + } } void checkServo() { - if (smallestnumber > 200 && smallestnumber < 20000) { - servoPwm = 1; - ic_timer_prescaler = CPU_FREQUENCY_MHZ - 1; - buffersize = 2; - inputSet = 1; - } + if (smallestnumber > 200 && smallestnumber < 20000) { + servoPwm = 1; + ic_timer_prescaler = CPU_FREQUENCY_MHZ - 1; + buffersize = 2; + inputSet = 1; + } } void detectInput() { - smallestnumber = 20000; - average_signal_pulse = 0; - int lastnumber = dma_buffer[0]; - for (int j = 1; j < 31; j++) { - if (dma_buffer[j] - lastnumber > 0) { - if ((dma_buffer[j] - lastnumber) < smallestnumber) { - smallestnumber = dma_buffer[j] - lastnumber; - } - average_signal_pulse += (dma_buffer[j] - lastnumber); - } - lastnumber = dma_buffer[j]; + smallestnumber = 20000; + average_signal_pulse = 0; + int lastnumber = dma_buffer[0]; + for (int j = 1; j < 31; j++) { + if (dma_buffer[j] - lastnumber > 0) { + if ((dma_buffer[j] - lastnumber) < smallestnumber) { + smallestnumber = dma_buffer[j] - lastnumber; + } + average_signal_pulse += (dma_buffer[j] - lastnumber); } - average_signal_pulse = average_signal_pulse / 32; + lastnumber = dma_buffer[j]; + } + average_signal_pulse = average_signal_pulse / 32; - if (dshot == 1) { - checkDshot(); - } - if (servoPwm == 1) { - checkServo(); - } + if (dshot == 1) { + checkDshot(); + } + if (servoPwm == 1) { + checkServo(); + } - if (!dshot && !servoPwm) { - checkDshot(); - checkServo(); - } + if (!dshot && !servoPwm) { + checkDshot(); + checkServo(); + } } diff --git a/Src/sounds.c b/Src/sounds.c index 14fde893..fdb8fd80 100644 --- a/Src/sounds.c +++ b/Src/sounds.c @@ -17,246 +17,247 @@ uint8_t beep_volume; void pause(uint16_t ms) { - SET_DUTY_CYCLE_ALL(0); - delayMillis(ms); - SET_DUTY_CYCLE_ALL(beep_volume); // volume of the beep, (duty cycle) don't go - // above 25 out of 2000 + SET_DUTY_CYCLE_ALL(0); + delayMillis(ms); + SET_DUTY_CYCLE_ALL(beep_volume); // volume of the beep, (duty cycle) don't go + // above 25 out of 2000 } void setVolume(uint8_t volume) { - if (volume > 11) { - volume = 11; - } - beep_volume = volume * 2; // volume variable from 0 - 11 equates to CCR value of 0-22 + if (volume > 11) { + volume = 11; + } + beep_volume = volume * 2; // volume variable from 0 - 11 equates to CCR value of 0-22 } void setCaptureCompare() { - SET_DUTY_CYCLE_ALL(beep_volume); // volume of the beep, (duty cycle) don't go - // above 25 out of 2000 + SET_DUTY_CYCLE_ALL(beep_volume); // volume of the beep, (duty cycle) don't go + // above 25 out of 2000 } void playBJNote(uint16_t freq, uint16_t bduration) -{ // hz and ms - uint16_t timerOne_reload = TIM1_AUTORELOAD; +{ + // hz and ms + uint16_t timerOne_reload = TIM1_AUTORELOAD; - SET_PRESCALER_PWM(10); - timerOne_reload = CPU_FREQUENCY_MHZ*100000 / freq; + SET_PRESCALER_PWM(10); + timerOne_reload = CPU_FREQUENCY_MHZ*100000 / freq; - SET_AUTO_RELOAD_PWM(timerOne_reload); - SET_DUTY_CYCLE_ALL(beep_volume * timerOne_reload / TIM1_AUTORELOAD); // volume of the beep, (duty cycle) don't - // go above 25 out of 2000 - delayMillis(bduration); + SET_AUTO_RELOAD_PWM(timerOne_reload); + SET_DUTY_CYCLE_ALL(beep_volume * timerOne_reload / TIM1_AUTORELOAD); // volume of the beep, (duty cycle) don't + // go above 25 out of 2000 + delayMillis(bduration); } uint16_t getBlueJayNoteFrequency(uint8_t bjarrayfreq) { - return 11000000 / (bjarrayfreq * 247 + 4000); + return 11000000 / (bjarrayfreq * 247 + 4000); } void playBlueJayTune() { - uint8_t full_time_count = 0; - uint16_t duration; - uint16_t frequency; - comStep(3); - // read_flash_bin(blueJayTuneBuffer , eeprom_address + 48 , 128); - for (int i = 0; i < 124; i += 2) { - RELOAD_WATCHDOG_COUNTER(); - signaltimeout = 0; + uint8_t full_time_count = 0; + uint16_t duration; + uint16_t frequency; + comStep(3); + // read_flash_bin(blueJayTuneBuffer , eeprom_address + 48 , 128); + for (int i = 0; i < 124; i += 2) { + RELOAD_WATCHDOG_COUNTER(); + signaltimeout = 0; - if (eepromBuffer.tune[i] == 255) { - full_time_count++; + if (eepromBuffer.tune[i] == 255) { + full_time_count++; - } else { - if (eepromBuffer.tune[i + 1] == 0) { - duration = full_time_count * 254 + eepromBuffer.tune[i]; - SET_DUTY_CYCLE_ALL(0); - delayMillis(duration); - } else { - frequency = getBlueJayNoteFrequency(eepromBuffer.tune[i + 1]); - duration = ((full_time_count * 254 + eepromBuffer.tune[i]) * (100000 / frequency)) / 100; - playBJNote(frequency, duration); - } - full_time_count = 0; - } + } else { + if (eepromBuffer.tune[i + 1] == 0) { + duration = full_time_count * 254 + eepromBuffer.tune[i]; + SET_DUTY_CYCLE_ALL(0); + delayMillis(duration); + } else { + frequency = getBlueJayNoteFrequency(eepromBuffer.tune[i + 1]); + duration = ((full_time_count * 254 + eepromBuffer.tune[i]) * (100000 / frequency)) / 100; + playBJNote(frequency, duration); + } + full_time_count = 0; } - allOff(); // turn all channels low again - SET_PRESCALER_PWM(0); // set prescaler back to 0. - SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR); - signaltimeout = 0; - RELOAD_WATCHDOG_COUNTER(); + } + allOff(); // turn all channels low again + SET_PRESCALER_PWM(0); // set prescaler back to 0. + SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR); + signaltimeout = 0; + RELOAD_WATCHDOG_COUNTER(); } void playStartupTune() { - __disable_irq(); + __disable_irq(); - uint8_t value = *(uint8_t*)(eeprom_address + 48); - if (value != 0xFF) { - playBlueJayTune(); - } else { - SET_AUTO_RELOAD_PWM(TIM1_AUTORELOAD); - setCaptureCompare(); - comStep(3); // activate a pwm channel - SET_PRESCALER_PWM(55); // frequency of beep - delayMillis(200); // duration of beep + uint8_t value = *(uint8_t*)(eeprom_address + 48); + if (value != 0xFF) { + playBlueJayTune(); + } else { + SET_AUTO_RELOAD_PWM(TIM1_AUTORELOAD); + setCaptureCompare(); + comStep(3); // activate a pwm channel + SET_PRESCALER_PWM(55); // frequency of beep + delayMillis(200); // duration of beep - comStep(5); - SET_PRESCALER_PWM(40); // next beep is higher frequency - delayMillis(200); + comStep(5); + SET_PRESCALER_PWM(40); // next beep is higher frequency + delayMillis(200); - comStep(6); - SET_PRESCALER_PWM(25); // higher again.. - delayMillis(200); + comStep(6); + SET_PRESCALER_PWM(25); // higher again.. + delayMillis(200); - allOff(); // turn all channels low again - SET_PRESCALER_PWM(0); // set prescaler back to 0. - signaltimeout = 0; - } + allOff(); // turn all channels low again + SET_PRESCALER_PWM(0); // set prescaler back to 0. + signaltimeout = 0; + } - SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR); - __enable_irq(); + SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR); + __enable_irq(); } void playBrushedStartupTune() { - __disable_irq(); - SET_AUTO_RELOAD_PWM(TIM1_AUTORELOAD); - setCaptureCompare(); - comStep(1); // activate a pwm channel - SET_PRESCALER_PWM(40); // frequency of beep - delayMillis(300); // duration of beep - comStep(2); // activate a pwm channel - SET_PRESCALER_PWM(30); // frequency of beep - delayMillis(300); // duration of beep - comStep(3); // activate a pwm channel - SET_PRESCALER_PWM(25); // frequency of beep - delayMillis(300); // duration of beep - comStep(4); - SET_PRESCALER_PWM(20); // higher again.. - delayMillis(300); - allOff(); // turn all channels low again - SET_PRESCALER_PWM(0); // set prescaler back to 0. - signaltimeout = 0; - SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR); - __enable_irq(); + __disable_irq(); + SET_AUTO_RELOAD_PWM(TIM1_AUTORELOAD); + setCaptureCompare(); + comStep(1); // activate a pwm channel + SET_PRESCALER_PWM(40); // frequency of beep + delayMillis(300); // duration of beep + comStep(2); // activate a pwm channel + SET_PRESCALER_PWM(30); // frequency of beep + delayMillis(300); // duration of beep + comStep(3); // activate a pwm channel + SET_PRESCALER_PWM(25); // frequency of beep + delayMillis(300); // duration of beep + comStep(4); + SET_PRESCALER_PWM(20); // higher again.. + delayMillis(300); + allOff(); // turn all channels low again + SET_PRESCALER_PWM(0); // set prescaler back to 0. + signaltimeout = 0; + SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR); + __enable_irq(); } void playDuskingTune() { - setCaptureCompare(); - SET_AUTO_RELOAD_PWM(TIM1_AUTORELOAD); - comStep(2); // activate a pwm channel - SET_PRESCALER_PWM(60); // frequency of beep - delayMillis(200); // duration of beep - SET_PRESCALER_PWM(55); // next beep is higher frequency - delayMillis(150); - SET_PRESCALER_PWM(50); // higher again.. - delayMillis(150); - SET_PRESCALER_PWM(45); // frequency of beep - delayMillis(100); // duration of beep - SET_PRESCALER_PWM(50); // next beep is higher frequency - delayMillis(100); - SET_PRESCALER_PWM(55); // higher again.. - delayMillis(100); - SET_PRESCALER_PWM(25); // higher again.. - delayMillis(200); - SET_PRESCALER_PWM(55); // higher again.. - delayMillis(150); - allOff(); // turn all channels low again - SET_PRESCALER_PWM(0); // set prescaler back to 0. - SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR); + setCaptureCompare(); + SET_AUTO_RELOAD_PWM(TIM1_AUTORELOAD); + comStep(2); // activate a pwm channel + SET_PRESCALER_PWM(60); // frequency of beep + delayMillis(200); // duration of beep + SET_PRESCALER_PWM(55); // next beep is higher frequency + delayMillis(150); + SET_PRESCALER_PWM(50); // higher again.. + delayMillis(150); + SET_PRESCALER_PWM(45); // frequency of beep + delayMillis(100); // duration of beep + SET_PRESCALER_PWM(50); // next beep is higher frequency + delayMillis(100); + SET_PRESCALER_PWM(55); // higher again.. + delayMillis(100); + SET_PRESCALER_PWM(25); // higher again.. + delayMillis(200); + SET_PRESCALER_PWM(55); // higher again.. + delayMillis(150); + allOff(); // turn all channels low again + SET_PRESCALER_PWM(0); // set prescaler back to 0. + SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR); } void playInputTune2() { - SET_AUTO_RELOAD_PWM(TIM1_AUTORELOAD); - __disable_irq(); - RELOAD_WATCHDOG_COUNTER(); - SET_PRESCALER_PWM(60); - setCaptureCompare(); - comStep(1); - delayMillis(75); - SET_PRESCALER_PWM(80); - delayMillis(75); - SET_PRESCALER_PWM(90); - RELOAD_WATCHDOG_COUNTER(); - delayMillis(75); - allOff(); - SET_PRESCALER_PWM(0); - signaltimeout = 0; - SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR); - __enable_irq(); + SET_AUTO_RELOAD_PWM(TIM1_AUTORELOAD); + __disable_irq(); + RELOAD_WATCHDOG_COUNTER(); + SET_PRESCALER_PWM(60); + setCaptureCompare(); + comStep(1); + delayMillis(75); + SET_PRESCALER_PWM(80); + delayMillis(75); + SET_PRESCALER_PWM(90); + RELOAD_WATCHDOG_COUNTER(); + delayMillis(75); + allOff(); + SET_PRESCALER_PWM(0); + signaltimeout = 0; + SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR); + __enable_irq(); } void playInputTune() { - __disable_irq(); - SET_AUTO_RELOAD_PWM(TIM1_AUTORELOAD); - RELOAD_WATCHDOG_COUNTER(); - SET_PRESCALER_PWM(80); - setCaptureCompare(); - comStep(3); - delayMillis(100); - SET_PRESCALER_PWM(70); - delayMillis(100); - SET_PRESCALER_PWM(40); - delayMillis(100); - allOff(); - SET_PRESCALER_PWM(0); - signaltimeout = 0; - SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR); - __enable_irq(); + __disable_irq(); + SET_AUTO_RELOAD_PWM(TIM1_AUTORELOAD); + RELOAD_WATCHDOG_COUNTER(); + SET_PRESCALER_PWM(80); + setCaptureCompare(); + comStep(3); + delayMillis(100); + SET_PRESCALER_PWM(70); + delayMillis(100); + SET_PRESCALER_PWM(40); + delayMillis(100); + allOff(); + SET_PRESCALER_PWM(0); + signaltimeout = 0; + SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR); + __enable_irq(); } void playDefaultTone() { - SET_AUTO_RELOAD_PWM(TIM1_AUTORELOAD); - SET_PRESCALER_PWM(50); - setCaptureCompare(); - comStep(2); - delayMillis(150); - RELOAD_WATCHDOG_COUNTER(); - SET_PRESCALER_PWM(30); - delayMillis(150); - allOff(); - SET_PRESCALER_PWM(0); - signaltimeout = 0; - SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR); + SET_AUTO_RELOAD_PWM(TIM1_AUTORELOAD); + SET_PRESCALER_PWM(50); + setCaptureCompare(); + comStep(2); + delayMillis(150); + RELOAD_WATCHDOG_COUNTER(); + SET_PRESCALER_PWM(30); + delayMillis(150); + allOff(); + SET_PRESCALER_PWM(0); + signaltimeout = 0; + SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR); } void playChangedTone() { - SET_AUTO_RELOAD_PWM(TIM1_AUTORELOAD); - SET_PRESCALER_PWM(40); - setCaptureCompare(); - comStep(2); - delayMillis(150); - RELOAD_WATCHDOG_COUNTER(); - SET_PRESCALER_PWM(80); - delayMillis(150); - allOff(); - SET_PRESCALER_PWM(0); - signaltimeout = 0; - SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR); + SET_AUTO_RELOAD_PWM(TIM1_AUTORELOAD); + SET_PRESCALER_PWM(40); + setCaptureCompare(); + comStep(2); + delayMillis(150); + RELOAD_WATCHDOG_COUNTER(); + SET_PRESCALER_PWM(80); + delayMillis(150); + allOff(); + SET_PRESCALER_PWM(0); + signaltimeout = 0; + SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR); } void playBeaconTune3() { - SET_AUTO_RELOAD_PWM(TIM1_AUTORELOAD); - __disable_irq(); - setCaptureCompare(); - for (int i = 119; i > 0; i = i - 2) { - RELOAD_WATCHDOG_COUNTER(); - comStep(i / 20); - SET_PRESCALER_PWM(10 + (i / 2)); - delayMillis(10); - } - allOff(); - SET_PRESCALER_PWM(0); - signaltimeout = 0; - SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR); - __enable_irq(); + SET_AUTO_RELOAD_PWM(TIM1_AUTORELOAD); + __disable_irq(); + setCaptureCompare(); + for (int i = 119; i > 0; i = i - 2) { + RELOAD_WATCHDOG_COUNTER(); + comStep(i / 20); + SET_PRESCALER_PWM(10 + (i / 2)); + delayMillis(10); + } + allOff(); + SET_PRESCALER_PWM(0); + signaltimeout = 0; + SET_AUTO_RELOAD_PWM(TIMER1_MAX_ARR); + __enable_irq(); }