/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "stm32g4xx_ll_hrtim.h"
#include "stm32g4xx_ll_fmac.h"
#include <stdio.h>
#include <string.h>
#include <math.h>
#include "cc_controller.h"
#include "mppt.h"
#include "debug_protocol.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
void adjustIINREF (int adjust);
void adjustVREF(uint16_t VREF);
void activateALARM();
void deactivateALARM();
void ADC3_config_polling();

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* 1Mhz
#define B0 ((int16_t)(0x1950))
#define B1 ((int16_t)(0xEE22))
#define B2 ((int16_t)(0xE733))
#define B3 ((int16_t)(0x1261))
#define A1 ((int16_t)(0x4729))
#define A2 ((int16_t)(0x3288))
#define A3 ((int16_t)(0x64F))
#define pre_shift (+3)
#define post_shift (+0)
#define DUTY_TICKS_MIN (272)
#define DUTY_TICKS_MAX (2448)
*/
// 500kHz A
/*
#define B0 (int16_t)(0x2C07)
#define B1 (int16_t)(0xE77B)
#define B2 (int16_t)(0xD5EC)
#define B3 (int16_t)(0x1A78)
#define A1 (int16_t)(0xFBA0)
#define A2 (int16_t)(0x6227)
#define A3 (int16_t)(0x223A)
#define pre_shift (+3)
#define post_shift (+0)
#define REF (3208)
#define DUTY_TICKS_MIN (544)
#define DUTY_TICKS_MAX (4896)
*/
/* 500khz B
#define B0 (int16_t)(0x2C1C)
#define B1 (int16_t)(0xE6B4)
#define B2 (int16_t)(0xD5A1)
#define B3 (int16_t)(0x1B09)
#define A1 (int16_t)(0xFBA0)
#define A2 (int16_t)(0x6227)
#define A3 (int16_t)(0x223A)
#define pre_shift (+3)
#define post_shift (+0)
#define REF (3208)
#define DUTY_TICKS_MIN (544)
#define DUTY_TICKS_MAX (4896)
*/
// 29uF 3.6uH 30khz 65phase 1000khz compensator
/*
#define B0 (int16_t)(0x6E3D)
#define B1 (int16_t)(0xA6AA)
#define B2 (int16_t)(0x91FF)
#define B3 (int16_t)(0x5992)
#define A1 (int16_t)(0xFBA0)
#define A2 (int16_t)(0x6227)
#define A3 (int16_t)(0x223A)
#define pre_shift (+3)
#define post_shift (+0)
#define REF (3208)
#define DUTY_TICKS_MIN (544)
#define DUTY_TICKS_MAX (4896)
*/
// 29uF 3.6uH 25khz 60phase 900khz compensator
/*
#define B0 (int16_t)(0x5179)
#define B1 (int16_t)(0xC702)
#define B2 (int16_t)(0xB033)
#define B3 (int16_t)(0x3AAB)
#define A1 (int16_t)(0xFBA0)
#define A2 (int16_t)(0x6227)
#define A3 (int16_t)(0x223A)
#define pre_shift (+3)
#define post_shift (+0)
#define REF (3208)*/

// 33uF 3.6uH 30khz 55 phase 800khz compensator
#define B0 (int16_t)(0x448A)
#define B1 (int16_t)(0xCB7A)
#define B2 (int16_t)(0xBBE6)
#define B3 (int16_t)(0x34F6)
#define A1 (int16_t)(0xFDD0)
#define A2 (int16_t)(0x3113)
#define A3 (int16_t)(0x111D)
#define pre_shift (+3)
#define post_shift (+1)
#define REF (3208)


#define COEFF_VECTOR_A_SIZE 3
#define COEFF_VECTOR_B_SIZE 4
#define INPUT_BUFFER_SIZE 5
#define FMAC_WDATA (0x40021418)

#define VOUT_COMP_REF 3851 // 2.361V (2.351V = 28.8V)
#define I_IN_COMP_REF 4000 // todo
#define I_LOAD_COMP_REF 4000 // todo


#define ADC_12BIT_2_5V_RESOLUTION 0.6105f // mV/count
#define VOUT_MULTIPLICATOR 12.25f
#define VIN_MULTIPLICATOR 52.282f //mv/V
#define VFLY_MULTIPLICATOR 33.902f // mV/V
#define IIN_MULTIPLICATOR 30.303 // mA/mV
#define IOUT_MULTIPLICATOR 21.82215 // mA/mV
#define ETEMP_MULTIPLICATOR 0.0354167f
#define ETEMP_SUBTRACTION -1.7f


#define VOUT_MAX 28300
#define VOUT_MIN 21000
#define VIN_MAX 120000
#define VIN_MIN 36000
#define IIN_MAX 40000
#define IIN_MIN -40000
#define IOUT_MAX 55000
#define IOUT_MIN -55000
#define VFLY_MAX 90000
#define ETEMP_MAX 74
#define ETEMP_MIN -9

#define VREF_MIN 3100
#define VREF_MAX 3700

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
ADC_HandleTypeDef hadc1;
ADC_HandleTypeDef hadc2;
ADC_HandleTypeDef hadc3;
ADC_HandleTypeDef hadc4;
ADC_HandleTypeDef hadc5;
DMA_HandleTypeDef hdma_adc1;
DMA_HandleTypeDef hdma_adc2;
DMA_HandleTypeDef hdma_adc3;
DMA_HandleTypeDef hdma_adc4;
DMA_HandleTypeDef hdma_adc5;

COMP_HandleTypeDef hcomp1;
COMP_HandleTypeDef hcomp3;
COMP_HandleTypeDef hcomp4;

DAC_HandleTypeDef hdac1;
DAC_HandleTypeDef hdac3;

FDCAN_HandleTypeDef hfdcan2;

FMAC_HandleTypeDef hfmac;

HRTIM_HandleTypeDef hhrtim1;

I2C_HandleTypeDef hi2c1;

OPAMP_HandleTypeDef hopamp1;
OPAMP_HandleTypeDef hopamp2;

TIM_HandleTypeDef htim1;
TIM_HandleTypeDef htim6;
TIM_HandleTypeDef htim7;
TIM_HandleTypeDef htim16;

UART_HandleTypeDef huart4;

/* USER CODE BEGIN PV */
__attribute__((aligned(32))) uint16_t DMA1BUF1; //adc4
volatile uint16_t dummy1 = 0x0;
__attribute__((aligned(32))) uint16_t DMA1BUF2[3]; //adc1
volatile uint16_t dummy2 = 0x0;
__attribute__((aligned(32))) uint16_t DMA1BUF3[2]; //adc2
volatile uint16_t dummy3 = 0x0;
__attribute__((aligned(32))) uint16_t DMA1BUF4; //adc5
volatile uint16_t dummy4 = 0x0;

__attribute__((aligned(32))) uint8_t DMA1BUF5[16]; // uart rx buffer

volatile int16_t last_tmp = 0;
volatile int16_t vfly_correction = 0;
float vfly_kp = 0.005f;
float vfly_ki = 0.0000f;
uint16_t vfly_clamp = 100;
volatile uint8_t vfly_active = 1;
float vfly_accumulator = 0;
float vfly_integral = 0;
uint16_t vfly_loop_counter = 0;
uint16_t vfly_loop_counter_trigger = 50; // 50kHz/100 = 500Hz
volatile float vin,iin,vout,iout,vfly,etemp;
float itemp,vbat;
uint16_t *retemp,*rvin,*riout,*rvfly,*ritemp,*rvbat;
int16_t *riin,*rvout;

CCController cc;
volatile float cc_target = 500.0;
volatile int cc_active = 0;
float cc_gain = 0.002;
float CC_MIN_STEP = -5;
float CC_MAX_STEP = 5;
uint16_t cc_loop_counter = 0;
uint16_t cc_loop_counter_trigger = 50; // 50KHz/50 = 1KHz
float cc_iout_accumulator = 0;

uint16_t VREF = 3208;

MPPTController mppt;
volatile int mppt_active = 0;
uint16_t mppt_loop_counter = 0;
uint16_t mppt_loop_counter_trigger = 150; // 100Hz/10 = 10Hz
float mppt_initial_iref = 2000.0;
float mppt_step = 25.0;
float mppt_iref_min = 500.0;
float mppt_iref_max = 20000.0;
float mppt_dv_threshold =1.0;
float mppt_deadband = 0.005f; // relative dP deadband (0.005 = 0.5%)

float vin_min_ctrl = 36000.0f; // soft vin floor for CC/MPPT

float avg_iin = 0.0;
float avg_vin = 0.0;
volatile float vfly_avg_debug = 0; // last averaged vfly the controller saw
int skip_vfly_check = 1; // set to 1 to bypass startup vfly sanity check

#define DT_NUM_SEGMENTS 6
static const float dt_breakpoints[DT_NUM_SEGMENTS + 1] = {0, 3000, 5000, 10000, 20000, 30000, 45000};
uint8_t dt_values[DT_NUM_SEGMENTS] = {25, 20, 18, 15, 15, 15};

/* Array of filter coefficients A (feedback taps) in Q1.15 format */
static int16_t aFilterCoeffA[COEFF_VECTOR_A_SIZE] = {A1,A2,A3};
/* Array of filter coefficients B (feed-forward taps) in Q1.15 format */
static int16_t aFilterCoeffB[COEFF_VECTOR_B_SIZE] = {-B0,-B1,-B2,-B3};

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_DMA_Init(void);
static void MX_ADC3_Init(void);
static void MX_ADC5_Init(void);
static void MX_FDCAN2_Init(void);
static void MX_HRTIM1_Init(void);
static void MX_I2C1_Init(void);
static void MX_UART4_Init(void);
static void MX_ADC1_Init(void);
static void MX_ADC2_Init(void);
static void MX_OPAMP2_Init(void);
static void MX_COMP1_Init(void);
static void MX_COMP3_Init(void);
static void MX_COMP4_Init(void);
static void MX_DAC1_Init(void);
static void MX_DAC3_Init(void);
static void MX_OPAMP1_Init(void);
static void MX_TIM1_Init(void);
static void MX_FMAC_Init(void);
static void MX_TIM6_Init(void);
static void MX_TIM7_Init(void);
static void MX_TIM16_Init(void);
static void MX_ADC4_Init(void);
/* USER CODE BEGIN PFP */

void turnOff();

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{

  /* USER CODE BEGIN 1 */
	// CLEAR nSWBOOT0 OptionByte with programmer ! otherwise systick wont work
  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */
  HAL_Delay(1000);

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_ADC3_Init();
  MX_ADC5_Init();
  MX_FDCAN2_Init();
  MX_HRTIM1_Init();
  MX_I2C1_Init();
  MX_UART4_Init();
  MX_ADC1_Init();
  MX_ADC2_Init();
  MX_OPAMP2_Init();
  MX_COMP1_Init();
  MX_COMP3_Init();
  MX_COMP4_Init();
  MX_DAC1_Init();
  MX_DAC3_Init();
  MX_OPAMP1_Init();
  MX_TIM1_Init();
  MX_FMAC_Init();
  MX_TIM6_Init();
  MX_TIM7_Init();
  MX_TIM16_Init();
  MX_ADC4_Init();
  /* USER CODE BEGIN 2 */

  HAL_OPAMP_SelfCalibrate(&hopamp1);
  HAL_OPAMP_SelfCalibrate(&hopamp2);
  HAL_OPAMP_Start(&hopamp1);
  HAL_OPAMP_Start(&hopamp2);
  HAL_OPAMP_Lock(&hopamp1);
  HAL_OPAMP_Lock(&hopamp2);

  HAL_GPIO_WritePin(ENABLE_ISO_FET_GPIO_Port,ENABLE_ISO_FET_Pin,GPIO_PIN_RESET);
  HAL_GPIO_WritePin(DISCONNECT_INPUT_GPIO_Port,DISCONNECT_INPUT_Pin,GPIO_PIN_RESET);
  HAL_GPIO_WritePin(DISCONNECT_OUTPUT_GPIO_Port,DISCONNECT_OUTPUT_Pin,GPIO_PIN_RESET);
  HAL_GPIO_WritePin(PRECHARGE_GPIO_Port,PRECHARGE_Pin,GPIO_PIN_RESET);


  HAL_ADCEx_Calibration_Start(&hadc1, ADC_SINGLE_ENDED);
  HAL_ADCEx_Calibration_Start(&hadc2, ADC_SINGLE_ENDED);
  HAL_ADCEx_Calibration_Start(&hadc3, ADC_SINGLE_ENDED);
  HAL_ADCEx_Calibration_Start(&hadc4, ADC_SINGLE_ENDED);
  HAL_ADCEx_Calibration_Start(&hadc5, ADC_SINGLE_ENDED);

  HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, VOUT_COMP_REF);
  HAL_DAC_SetValue(&hdac3, DAC_CHANNEL_1, DAC_ALIGN_12B_R, I_IN_COMP_REF);
  HAL_DAC_SetValue(&hdac3, DAC_CHANNEL_2, DAC_ALIGN_12B_R, I_LOAD_COMP_REF);
  HAL_DAC_Start(&hdac1, DAC_CHANNEL_1);
  HAL_DAC_Start(&hdac3, DAC_CHANNEL_1);
  HAL_DAC_Start(&hdac3, DAC_CHANNEL_2);


  //HAL_COMP_Start(&hcomp3);
  HAL_COMP_Start(&hcomp4);
  HAL_COMP_Start(&hcomp1);


  //ADC1 values
  retemp = &(DMA1BUF2[0]);
  rvin = &(DMA1BUF2[1]);
  riin = (int16_t *)(&(DMA1BUF2[2]));

  //ADC2 values
  rvfly = &(DMA1BUF3[0]);
  riout = &(DMA1BUF3[1]);


  //ADC5 value
  ritemp = &DMA1BUF4;

  //ADC3 value (not active yet)
  rvout = (int16_t *)&(ADC3->DR);

  //ADC4 value
  rvbat =  &DMA1BUF1;


  HAL_ADC_Start_DMA(&hadc1,(uint32_t *)&DMA1BUF2[0],3);
  HAL_ADC_Start_DMA(&hadc2,(uint32_t *)&DMA1BUF3[0],2);
  HAL_ADC_Start_DMA(&hadc5,(uint32_t *)&DMA1BUF4,1);
  HAL_ADC_Start_DMA(&hadc4,(uint32_t *)&DMA1BUF1,1);


  HAL_Delay(1000);


  vin = ADC_12BIT_2_5V_RESOLUTION * VIN_MULTIPLICATOR * *rvin;
  vbat = ADC_12BIT_2_5V_RESOLUTION * VOUT_MULTIPLICATOR * *rvbat;
  iin = ADC_12BIT_2_5V_RESOLUTION * IIN_MULTIPLICATOR * *riin;
  iout = ADC_12BIT_2_5V_RESOLUTION * IOUT_MULTIPLICATOR * *riout;
  vfly = ADC_12BIT_2_5V_RESOLUTION * VFLY_MULTIPLICATOR * *rvfly;

  etemp = (ADC_12BIT_2_5V_RESOLUTION * ETEMP_MULTIPLICATOR * *retemp) - ETEMP_SUBTRACTION;
  itemp = ADC_12BIT_2_5V_RESOLUTION * *ritemp;

 // __HAL_ADC_CALC_TEMPERATURE()
 //__HAL_ADC_CALC_TEMPERATURE_TYP_PARAMS



  //adjustIINREF(*riin); this bricks the ADC

  Proto_Init(&huart4);

  while (vin > VIN_MAX  || vin < VIN_MIN || iin > IIN_MAX || iin < IIN_MIN || iout > IOUT_MAX || iout < IOUT_MIN || vfly > VFLY_MAX || etemp < ETEMP_MIN || etemp > ETEMP_MAX)
  {
	 const char *guard = "GUARD";
	 if (vin > VIN_MAX) guard = "GUARD: VIN_MAX";
	 else if (vin < VIN_MIN) guard = "GUARD: VIN_MIN";
	 else if (iin > IIN_MAX) guard = "GUARD: IIN_MAX";
	 else if (iin < IIN_MIN) guard = "GUARD: IIN_MIN";
	 else if (iout > IOUT_MAX) guard = "GUARD: IOUT_MAX";
	 else if (iout < IOUT_MIN) guard = "GUARD: IOUT_MIN";
	 else if (vfly > VFLY_MAX) guard = "GUARD: VFLY_MAX";
	 else if (etemp > ETEMP_MAX) guard = "GUARD: ETEMP_MAX";
	 else if (etemp < ETEMP_MIN) guard = "GUARD: ETEMP_MIN";
	 Proto_SendDiagDump(guard);
	 HAL_Delay(5000);

	 vin = ADC_12BIT_2_5V_RESOLUTION * VIN_MULTIPLICATOR * *rvin;
	 vbat = ADC_12BIT_2_5V_RESOLUTION * VOUT_MULTIPLICATOR * *rvbat;
     iin = ADC_12BIT_2_5V_RESOLUTION * IIN_MULTIPLICATOR * *riin;
     iout = ADC_12BIT_2_5V_RESOLUTION * IOUT_MULTIPLICATOR* *riout;
     vfly = ADC_12BIT_2_5V_RESOLUTION * VFLY_MULTIPLICATOR * *rvfly;

     etemp = (ADC_12BIT_2_5V_RESOLUTION * ETEMP_MULTIPLICATOR * *retemp) - ETEMP_SUBTRACTION;
     itemp = ADC_12BIT_2_5V_RESOLUTION * *ritemp;
  }
  Proto_SendError("Startup");
  HAL_Delay(500);

  VREF = *rvbat+70; // add a small offset so that we dont reverse feed
  HAL_ADC_Stop_DMA(&hadc4); // dont have to stop ? maybe continue and keep as monitoring for adc3

  ADC3->OFR1 &=(uint32_t) ~ 0xFFF ;
  ADC3->OFR1 |= (VREF & (uint16_t)0xFFF);

  CC_Init(&cc,cc_gain,CC_MIN_STEP,CC_MAX_STEP,VREF_MIN,VREF_MAX,(float)VREF);
  MPPT_IncCond_Init(&mppt,mppt_initial_iref,mppt_step,mppt_iref_min,mppt_iref_max,mppt_dv_threshold);

  /* declare a filter configuration structure */
  FMAC_FilterConfigTypeDef sFmacConfig;
  /* Set the coefficient buffer base address */
  sFmacConfig.CoeffBaseAddress = 0;
  /* Set the coefficient buffer size to the number of coeffs */
  sFmacConfig.CoeffBufferSize = 7;
  /* Set the Input buffer base address to the next free address */
  sFmacConfig.InputBaseAddress = 7;
  /* Set the input buffer size greater than the number of B coeffs(+1) here */
  sFmacConfig.InputBufferSize = 5;
  /* Set the input watermark to zero since we are using DMA */
  sFmacConfig.InputThreshold = 0;
  /* Set the Output buffer base address to the next free address */
  sFmacConfig.OutputBaseAddress = 12;
  /* Set the output buffer size greater than the number of B coeffs(+1) here */
  sFmacConfig.OutputBufferSize = 5;
  /* Set the output watermark to zero since we are using DMA */
  sFmacConfig.OutputThreshold = 0;
  /* Pointer to A coefficients in memory*/
  sFmacConfig.pCoeffA = aFilterCoeffA;
  /* Number of A coefficients */
  sFmacConfig.CoeffASize = 3;
  /* Pointer to Bcoefficients in memory */
  sFmacConfig.pCoeffB = aFilterCoeffB;
  /* Number of B coefficients */
  sFmacConfig.CoeffBSize = 4;
  /* Select IIR filter function */
  sFmacConfig.Filter = FMAC_FUNC_IIR_DIRECT_FORM_1;
  /* Disable DMA and interrupt requests for input */
  sFmacConfig.InputAccess = FMAC_BUFFER_ACCESS_NONE;
  /* Enable FMAC read interrupt for output transfer */
  sFmacConfig.OutputAccess = FMAC_BUFFER_ACCESS_IT;
  /* Enable clipping of the output at 0x7FFF and 0x8000 */
  sFmacConfig.Clip = FMAC_CLIP_ENABLED;
  /* P parameter contains number of B coefficients */
  sFmacConfig.P = 4;
  /* Q parameter contains number of A coefficients */
  sFmacConfig.Q = 3;
  /* R parameter contains the post-shift value */
  sFmacConfig.R = post_shift;
  /* Configure the FMAC */
  if (HAL_FMAC_FilterConfig(&hfmac, &sFmacConfig) != HAL_OK)
  /* Configuration Error */
  Error_Handler();

  LL_HRTIM_TIM_CounterEnable(HRTIM1, LL_HRTIM_TIMER_E | LL_HRTIM_TIMER_F | LL_HRTIM_TIMER_MASTER);

  HAL_GPIO_WritePin(DISCONNECT_OUTPUT_GPIO_Port,DISCONNECT_OUTPUT_Pin,GPIO_PIN_SET);
  HAL_Delay(200);
  HAL_GPIO_WritePin(PRECHARGE_GPIO_Port,PRECHARGE_Pin,GPIO_PIN_SET);
  HAL_Delay(200);
  HAL_GPIO_WritePin(ENABLE_ISO_FET_GPIO_Port,ENABLE_ISO_FET_Pin,GPIO_PIN_SET);
  HAL_GPIO_WritePin(DISCONNECT_INPUT_GPIO_Port,DISCONNECT_INPUT_Pin,GPIO_PIN_SET);
  HAL_Delay(100);
  HAL_TIM_Base_Start_IT(&htim16);
  HAL_TIM_Base_Start_IT(&htim6);
  HAL_TIM_Base_Start_IT(&htim7);
  HAL_Delay(500);


  // Vfly startup sanity check — wait until vfly is within 20% of vin/2
  // HRTIM counters running (ADC triggers active) but outputs not yet enabled

   if (!skip_vfly_check)
   {
       HAL_Delay(100); // let ADC settle after HRTIM start
       float vfly_expected = vin / 2.0f;
       float vfly_deviation = vfly - vfly_expected;
       if (vfly_deviation < 0) vfly_deviation = -vfly_deviation;
       while (vfly_deviation > 0.20f * vfly_expected)
       {
           char vfly_buf[64];
           snprintf(vfly_buf, sizeof(vfly_buf), "STARTUP WAITING: Vfly=%lu exp=%lu",
                    (unsigned long)(uint32_t)vfly,
                    (unsigned long)(uint32_t)vfly_expected);
           Proto_SendError(vfly_buf);
           HAL_Delay(1000);
           vfly_expected = vin / 2.0f;
           vfly_deviation = vfly - vfly_expected;
           if (vfly_deviation < 0) vfly_deviation = -vfly_deviation;
       }
   }

  float ratio = vout/vin;
  int16_t initial_duty = ratio * (DUTY_TICKS_MID / 2);
  int16_t InputPreload[INPUT_BUFFER_SIZE] = {0,0,0,0,0};
  int16_t aOutputDataToPreload[COEFF_VECTOR_A_SIZE] = {initial_duty,initial_duty,initial_duty};
  if (HAL_FMAC_FilterPreload(&hfmac, InputPreload, INPUT_BUFFER_SIZE,
		  aOutputDataToPreload, COEFF_VECTOR_A_SIZE) != HAL_OK) Error_Handler();


  uint16_t ExpectedCalculatedOutputSize = (uint16_t) 1;
  int16_t Fmac_output = 0;

  if (HAL_FMAC_FilterStart(&hfmac,&Fmac_output,&ExpectedCalculatedOutputSize) != HAL_OK) Error_Handler();
  LL_FMAC_DisableIT_WR(FMAC);
  uint32_t *Fmac_Wdata = (uint32_t *) FMAC_WDATA;
  HAL_ADC_Start_DMA(&hadc3,Fmac_Wdata,1);


  LL_HRTIM_EnableOutput(HRTIM1,LL_HRTIM_OUTPUT_TE1 | LL_HRTIM_OUTPUT_TE2 | LL_HRTIM_OUTPUT_TF1 | LL_HRTIM_OUTPUT_TF2);

  //HRTIM1->sTimerxRegs[HRTIM_TIMERINDEX_TIMER_E].CMP1xR = 2000;
  //HRTIM1->sTimerxRegs[HRTIM_TIMERINDEX_TIMER_F].CMP1xR = 2000;

  //HAL_Delay(2500);





  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */

  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Configure the main internal regulator output voltage
  */
  HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1_BOOST);

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV4;
  RCC_OscInitStruct.PLL.PLLN = 34;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV4;
  RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2;
  RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK)
  {
    Error_Handler();
  }

  /** Enables the Clock Security System
  */
  HAL_RCC_EnableCSS();
}

/**
  * @brief ADC1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_ADC1_Init(void)
{

  /* USER CODE BEGIN ADC1_Init 0 */

  /* USER CODE END ADC1_Init 0 */

  ADC_MultiModeTypeDef multimode = {0};
  ADC_ChannelConfTypeDef sConfig = {0};

  /* USER CODE BEGIN ADC1_Init 1 */

  /* USER CODE END ADC1_Init 1 */

  /** Common config
  */
  hadc1.Instance = ADC1;
  hadc1.Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV4;
  hadc1.Init.Resolution = ADC_RESOLUTION_12B;
  hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
  hadc1.Init.GainCompensation = 0;
  hadc1.Init.ScanConvMode = ADC_SCAN_ENABLE;
  hadc1.Init.EOCSelection = ADC_EOC_SEQ_CONV;
  hadc1.Init.LowPowerAutoWait = DISABLE;
  hadc1.Init.ContinuousConvMode = ENABLE;
  hadc1.Init.NbrOfConversion = 3;
  hadc1.Init.DiscontinuousConvMode = DISABLE;
  hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
  hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
  hadc1.Init.DMAContinuousRequests = ENABLE;
  hadc1.Init.Overrun = ADC_OVR_DATA_OVERWRITTEN;
  hadc1.Init.OversamplingMode = DISABLE;
  if (HAL_ADC_Init(&hadc1) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure the ADC multi-mode
  */
  multimode.Mode = ADC_MODE_INDEPENDENT;
  if (HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Regular Channel
  */
  sConfig.Channel = ADC_CHANNEL_3;
  sConfig.Rank = ADC_REGULAR_RANK_1;
  sConfig.SamplingTime = ADC_SAMPLETIME_247CYCLES_5;
  sConfig.SingleDiff = ADC_SINGLE_ENDED;
  sConfig.OffsetNumber = ADC_OFFSET_NONE;
  sConfig.Offset = 0;
  if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Regular Channel
  */
  sConfig.Channel = ADC_CHANNEL_6;
  sConfig.Rank = ADC_REGULAR_RANK_2;
  if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Regular Channel
  */
  sConfig.Channel = ADC_CHANNEL_7;
  sConfig.Rank = ADC_REGULAR_RANK_3;
  sConfig.OffsetNumber = ADC_OFFSET_1;
  sConfig.Offset = 2703;
  sConfig.OffsetSign = ADC_OFFSET_SIGN_NEGATIVE;
  sConfig.OffsetSaturation = DISABLE;
  if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN ADC1_Init 2 */

  /* USER CODE END ADC1_Init 2 */

}

/**
  * @brief ADC2 Initialization Function
  * @param None
  * @retval None
  */
static void MX_ADC2_Init(void)
{

  /* USER CODE BEGIN ADC2_Init 0 */

  /* USER CODE END ADC2_Init 0 */

  ADC_ChannelConfTypeDef sConfig = {0};

  /* USER CODE BEGIN ADC2_Init 1 */

  /* USER CODE END ADC2_Init 1 */

  /** Common config
  */
  hadc2.Instance = ADC2;
  hadc2.Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV4;
  hadc2.Init.Resolution = ADC_RESOLUTION_12B;
  hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT;
  hadc2.Init.GainCompensation = 0;
  hadc2.Init.ScanConvMode = ADC_SCAN_ENABLE;
  hadc2.Init.EOCSelection = ADC_EOC_SEQ_CONV;
  hadc2.Init.LowPowerAutoWait = DISABLE;
  hadc2.Init.ContinuousConvMode = DISABLE;
  hadc2.Init.NbrOfConversion = 2;
  hadc2.Init.DiscontinuousConvMode = DISABLE;
  hadc2.Init.ExternalTrigConv = ADC_EXTERNALTRIG_HRTIM_TRG3;
  hadc2.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING;
  hadc2.Init.DMAContinuousRequests = ENABLE;
  hadc2.Init.Overrun = ADC_OVR_DATA_OVERWRITTEN;
  hadc2.Init.OversamplingMode = DISABLE;
  if (HAL_ADC_Init(&hadc2) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Regular Channel
  */
  sConfig.Channel = ADC_CHANNEL_3;
  sConfig.Rank = ADC_REGULAR_RANK_1;
  sConfig.SamplingTime = ADC_SAMPLETIME_6CYCLES_5;
  sConfig.SingleDiff = ADC_SINGLE_ENDED;
  sConfig.OffsetNumber = ADC_OFFSET_NONE;
  sConfig.Offset = 0;
  if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Regular Channel
  */
  sConfig.Channel = ADC_CHANNEL_5;
  sConfig.Rank = ADC_REGULAR_RANK_2;
  sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5;
  if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN ADC2_Init 2 */

  /* USER CODE END ADC2_Init 2 */

}

/**
  * @brief ADC3 Initialization Function
  * @param None
  * @retval None
  */
static void MX_ADC3_Init(void)
{

  /* USER CODE BEGIN ADC3_Init 0 */

  /* USER CODE END ADC3_Init 0 */

  ADC_MultiModeTypeDef multimode = {0};
  ADC_ChannelConfTypeDef sConfig = {0};

  /* USER CODE BEGIN ADC3_Init 1 */

  /* USER CODE END ADC3_Init 1 */

  /** Common config
  */
  hadc3.Instance = ADC3;
  hadc3.Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV4;
  hadc3.Init.Resolution = ADC_RESOLUTION_12B;
  hadc3.Init.DataAlign = ADC_DATAALIGN_LEFT;
  hadc3.Init.GainCompensation = 0;
  hadc3.Init.ScanConvMode = ADC_SCAN_DISABLE;
  hadc3.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
  hadc3.Init.LowPowerAutoWait = DISABLE;
  hadc3.Init.ContinuousConvMode = DISABLE;
  hadc3.Init.NbrOfConversion = 1;
  hadc3.Init.DiscontinuousConvMode = DISABLE;
  hadc3.Init.ExternalTrigConv = ADC_EXTERNALTRIG_HRTIM_TRG1;
  hadc3.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING;
  hadc3.Init.DMAContinuousRequests = ENABLE;
  hadc3.Init.Overrun = ADC_OVR_DATA_OVERWRITTEN;
  hadc3.Init.OversamplingMode = DISABLE;
  if (HAL_ADC_Init(&hadc3) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure the ADC multi-mode
  */
  multimode.Mode = ADC_MODE_INDEPENDENT;
  if (HAL_ADCEx_MultiModeConfigChannel(&hadc3, &multimode) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Regular Channel
  */
  sConfig.Channel = ADC_CHANNEL_1;
  sConfig.Rank = ADC_REGULAR_RANK_1;
  sConfig.SamplingTime = ADC_SAMPLETIME_12CYCLES_5;
  sConfig.SingleDiff = ADC_SINGLE_ENDED;
  sConfig.OffsetNumber = ADC_OFFSET_1;
  sConfig.Offset = 3208;
  sConfig.OffsetSign = ADC_OFFSET_SIGN_NEGATIVE;
  sConfig.OffsetSaturation = DISABLE;
  if (HAL_ADC_ConfigChannel(&hadc3, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN ADC3_Init 2 */
  HAL_NVIC_DisableIRQ(DMA2_Channel1_IRQn);
  /* USER CODE END ADC3_Init 2 */

}

/**
  * @brief ADC4 Initialization Function
  * @param None
  * @retval None
  */
static void MX_ADC4_Init(void)
{

  /* USER CODE BEGIN ADC4_Init 0 */

  /* USER CODE END ADC4_Init 0 */

  ADC_ChannelConfTypeDef sConfig = {0};

  /* USER CODE BEGIN ADC4_Init 1 */

  /* USER CODE END ADC4_Init 1 */

  /** Common config
  */
  hadc4.Instance = ADC4;
  hadc4.Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV4;
  hadc4.Init.Resolution = ADC_RESOLUTION_12B;
  hadc4.Init.DataAlign = ADC_DATAALIGN_RIGHT;
  hadc4.Init.GainCompensation = 0;
  hadc4.Init.ScanConvMode = ADC_SCAN_DISABLE;
  hadc4.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
  hadc4.Init.LowPowerAutoWait = DISABLE;
  hadc4.Init.ContinuousConvMode = ENABLE;
  hadc4.Init.NbrOfConversion = 1;
  hadc4.Init.DiscontinuousConvMode = DISABLE;
  hadc4.Init.ExternalTrigConv = ADC_SOFTWARE_START;
  hadc4.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
  hadc4.Init.DMAContinuousRequests = ENABLE;
  hadc4.Init.Overrun = ADC_OVR_DATA_OVERWRITTEN;
  hadc4.Init.OversamplingMode = DISABLE;
  if (HAL_ADC_Init(&hadc4) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Regular Channel
  */
  sConfig.Channel = ADC_CHANNEL_3;
  sConfig.Rank = ADC_REGULAR_RANK_1;
  sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5;
  sConfig.SingleDiff = ADC_SINGLE_ENDED;
  sConfig.OffsetNumber = ADC_OFFSET_NONE;
  sConfig.Offset = 0;
  if (HAL_ADC_ConfigChannel(&hadc4, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN ADC4_Init 2 */

  /* USER CODE END ADC4_Init 2 */

}

/**
  * @brief ADC5 Initialization Function
  * @param None
  * @retval None
  */
static void MX_ADC5_Init(void)
{

  /* USER CODE BEGIN ADC5_Init 0 */

  /* USER CODE END ADC5_Init 0 */

  ADC_ChannelConfTypeDef sConfig = {0};

  /* USER CODE BEGIN ADC5_Init 1 */

  /* USER CODE END ADC5_Init 1 */

  /** Common config
  */
  hadc5.Instance = ADC5;
  hadc5.Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV4;
  hadc5.Init.Resolution = ADC_RESOLUTION_12B;
  hadc5.Init.DataAlign = ADC_DATAALIGN_RIGHT;
  hadc5.Init.GainCompensation = 0;
  hadc5.Init.ScanConvMode = ADC_SCAN_DISABLE;
  hadc5.Init.EOCSelection = ADC_EOC_SEQ_CONV;
  hadc5.Init.LowPowerAutoWait = DISABLE;
  hadc5.Init.ContinuousConvMode = ENABLE;
  hadc5.Init.NbrOfConversion = 1;
  hadc5.Init.DiscontinuousConvMode = DISABLE;
  hadc5.Init.ExternalTrigConv = ADC_SOFTWARE_START;
  hadc5.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
  hadc5.Init.DMAContinuousRequests = ENABLE;
  hadc5.Init.Overrun = ADC_OVR_DATA_PRESERVED;
  hadc5.Init.OversamplingMode = DISABLE;
  if (HAL_ADC_Init(&hadc5) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Regular Channel
  */
  sConfig.Channel = ADC_CHANNEL_TEMPSENSOR_ADC5;
  sConfig.Rank = ADC_REGULAR_RANK_1;
  sConfig.SamplingTime = ADC_SAMPLETIME_247CYCLES_5;
  sConfig.SingleDiff = ADC_SINGLE_ENDED;
  sConfig.OffsetNumber = ADC_OFFSET_NONE;
  sConfig.Offset = 0;
  if (HAL_ADC_ConfigChannel(&hadc5, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN ADC5_Init 2 */

  /* USER CODE END ADC5_Init 2 */

}

/**
  * @brief COMP1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_COMP1_Init(void)
{

  /* USER CODE BEGIN COMP1_Init 0 */

  /* USER CODE END COMP1_Init 0 */

  /* USER CODE BEGIN COMP1_Init 1 */

  /* USER CODE END COMP1_Init 1 */
  hcomp1.Instance = COMP1;
  hcomp1.Init.InputPlus = COMP_INPUT_PLUS_IO2;
  hcomp1.Init.InputMinus = COMP_INPUT_MINUS_DAC1_CH1;
  hcomp1.Init.OutputPol = COMP_OUTPUTPOL_NONINVERTED;
  hcomp1.Init.Hysteresis = COMP_HYSTERESIS_10MV;
  hcomp1.Init.BlankingSrce = COMP_BLANKINGSRC_NONE;
  hcomp1.Init.TriggerMode = COMP_TRIGGERMODE_IT_RISING;
  if (HAL_COMP_Init(&hcomp1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN COMP1_Init 2 */

  /* USER CODE END COMP1_Init 2 */

}

/**
  * @brief COMP3 Initialization Function
  * @param None
  * @retval None
  */
static void MX_COMP3_Init(void)
{

  /* USER CODE BEGIN COMP3_Init 0 */

  /* USER CODE END COMP3_Init 0 */

  /* USER CODE BEGIN COMP3_Init 1 */

  /* USER CODE END COMP3_Init 1 */
  hcomp3.Instance = COMP3;
  hcomp3.Init.InputPlus = COMP_INPUT_PLUS_IO2;
  hcomp3.Init.InputMinus = COMP_INPUT_MINUS_DAC3_CH1;
  hcomp3.Init.OutputPol = COMP_OUTPUTPOL_NONINVERTED;
  hcomp3.Init.Hysteresis = COMP_HYSTERESIS_10MV;
  hcomp3.Init.BlankingSrce = COMP_BLANKINGSRC_NONE;
  hcomp3.Init.TriggerMode = COMP_TRIGGERMODE_IT_RISING;
  if (HAL_COMP_Init(&hcomp3) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN COMP3_Init 2 */

  /* USER CODE END COMP3_Init 2 */

}

/**
  * @brief COMP4 Initialization Function
  * @param None
  * @retval None
  */
static void MX_COMP4_Init(void)
{

  /* USER CODE BEGIN COMP4_Init 0 */

  /* USER CODE END COMP4_Init 0 */

  /* USER CODE BEGIN COMP4_Init 1 */

  /* USER CODE END COMP4_Init 1 */
  hcomp4.Instance = COMP4;
  hcomp4.Init.InputPlus = COMP_INPUT_PLUS_IO1;
  hcomp4.Init.InputMinus = COMP_INPUT_MINUS_DAC3_CH2;
  hcomp4.Init.OutputPol = COMP_OUTPUTPOL_NONINVERTED;
  hcomp4.Init.Hysteresis = COMP_HYSTERESIS_10MV;
  hcomp4.Init.BlankingSrce = COMP_BLANKINGSRC_NONE;
  hcomp4.Init.TriggerMode = COMP_TRIGGERMODE_IT_RISING;
  if (HAL_COMP_Init(&hcomp4) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN COMP4_Init 2 */

  /* USER CODE END COMP4_Init 2 */

}

/**
  * @brief DAC1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_DAC1_Init(void)
{

  /* USER CODE BEGIN DAC1_Init 0 */

  /* USER CODE END DAC1_Init 0 */

  DAC_ChannelConfTypeDef sConfig = {0};

  /* USER CODE BEGIN DAC1_Init 1 */

  /* USER CODE END DAC1_Init 1 */

  /** DAC Initialization
  */
  hdac1.Instance = DAC1;
  if (HAL_DAC_Init(&hdac1) != HAL_OK)
  {
    Error_Handler();
  }

  /** DAC channel OUT1 config
  */
  sConfig.DAC_HighFrequency = DAC_HIGH_FREQUENCY_INTERFACE_MODE_AUTOMATIC;
  sConfig.DAC_DMADoubleDataMode = DISABLE;
  sConfig.DAC_SignedFormat = DISABLE;
  sConfig.DAC_SampleAndHold = DAC_SAMPLEANDHOLD_DISABLE;
  sConfig.DAC_Trigger = DAC_TRIGGER_NONE;
  sConfig.DAC_Trigger2 = DAC_TRIGGER_NONE;
  sConfig.DAC_OutputBuffer = DAC_OUTPUTBUFFER_DISABLE;
  sConfig.DAC_ConnectOnChipPeripheral = DAC_CHIPCONNECT_INTERNAL;
  sConfig.DAC_UserTrimming = DAC_TRIMMING_FACTORY;
  if (HAL_DAC_ConfigChannel(&hdac1, &sConfig, DAC_CHANNEL_1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN DAC1_Init 2 */

  /* USER CODE END DAC1_Init 2 */

}

/**
  * @brief DAC3 Initialization Function
  * @param None
  * @retval None
  */
static void MX_DAC3_Init(void)
{

  /* USER CODE BEGIN DAC3_Init 0 */

  /* USER CODE END DAC3_Init 0 */

  DAC_ChannelConfTypeDef sConfig = {0};

  /* USER CODE BEGIN DAC3_Init 1 */

  /* USER CODE END DAC3_Init 1 */

  /** DAC Initialization
  */
  hdac3.Instance = DAC3;
  if (HAL_DAC_Init(&hdac3) != HAL_OK)
  {
    Error_Handler();
  }

  /** DAC channel OUT1 config
  */
  sConfig.DAC_HighFrequency = DAC_HIGH_FREQUENCY_INTERFACE_MODE_AUTOMATIC;
  sConfig.DAC_DMADoubleDataMode = DISABLE;
  sConfig.DAC_SignedFormat = DISABLE;
  sConfig.DAC_SampleAndHold = DAC_SAMPLEANDHOLD_DISABLE;
  sConfig.DAC_Trigger = DAC_TRIGGER_NONE;
  sConfig.DAC_Trigger2 = DAC_TRIGGER_NONE;
  sConfig.DAC_OutputBuffer = DAC_OUTPUTBUFFER_DISABLE;
  sConfig.DAC_ConnectOnChipPeripheral = DAC_CHIPCONNECT_INTERNAL;
  sConfig.DAC_UserTrimming = DAC_TRIMMING_FACTORY;
  if (HAL_DAC_ConfigChannel(&hdac3, &sConfig, DAC_CHANNEL_1) != HAL_OK)
  {
    Error_Handler();
  }

  /** DAC channel OUT2 config
  */
  if (HAL_DAC_ConfigChannel(&hdac3, &sConfig, DAC_CHANNEL_2) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN DAC3_Init 2 */

  /* USER CODE END DAC3_Init 2 */

}

/**
  * @brief FDCAN2 Initialization Function
  * @param None
  * @retval None
  */
static void MX_FDCAN2_Init(void)
{

  /* USER CODE BEGIN FDCAN2_Init 0 */

  /* USER CODE END FDCAN2_Init 0 */

  /* USER CODE BEGIN FDCAN2_Init 1 */

  /* USER CODE END FDCAN2_Init 1 */
  hfdcan2.Instance = FDCAN2;
  hfdcan2.Init.ClockDivider = FDCAN_CLOCK_DIV1;
  hfdcan2.Init.FrameFormat = FDCAN_FRAME_CLASSIC;
  hfdcan2.Init.Mode = FDCAN_MODE_NORMAL;
  hfdcan2.Init.AutoRetransmission = DISABLE;
  hfdcan2.Init.TransmitPause = DISABLE;
  hfdcan2.Init.ProtocolException = DISABLE;
  hfdcan2.Init.NominalPrescaler = 16;
  hfdcan2.Init.NominalSyncJumpWidth = 1;
  hfdcan2.Init.NominalTimeSeg1 = 1;
  hfdcan2.Init.NominalTimeSeg2 = 1;
  hfdcan2.Init.DataPrescaler = 1;
  hfdcan2.Init.DataSyncJumpWidth = 1;
  hfdcan2.Init.DataTimeSeg1 = 1;
  hfdcan2.Init.DataTimeSeg2 = 1;
  hfdcan2.Init.StdFiltersNbr = 0;
  hfdcan2.Init.ExtFiltersNbr = 2;
  hfdcan2.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION;
  if (HAL_FDCAN_Init(&hfdcan2) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN FDCAN2_Init 2 */

  /* USER CODE END FDCAN2_Init 2 */

}

/**
  * @brief FMAC Initialization Function
  * @param None
  * @retval None
  */
static void MX_FMAC_Init(void)
{

  /* USER CODE BEGIN FMAC_Init 0 */

  /* USER CODE END FMAC_Init 0 */

  /* USER CODE BEGIN FMAC_Init 1 */

  /* USER CODE END FMAC_Init 1 */
  hfmac.Instance = FMAC;
  if (HAL_FMAC_Init(&hfmac) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN FMAC_Init 2 */

  /* USER CODE END FMAC_Init 2 */

}

/**
  * @brief HRTIM1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_HRTIM1_Init(void)
{

  /* USER CODE BEGIN HRTIM1_Init 0 */

  /* USER CODE END HRTIM1_Init 0 */

  HRTIM_FaultBlankingCfgTypeDef pFaultBlkCfg = {0};
  HRTIM_FaultCfgTypeDef pFaultCfg = {0};
  HRTIM_ADCTriggerCfgTypeDef pADCTriggerCfg = {0};
  HRTIM_TimeBaseCfgTypeDef pTimeBaseCfg = {0};
  HRTIM_TimerCfgTypeDef pTimerCfg = {0};
  HRTIM_CompareCfgTypeDef pCompareCfg = {0};
  HRTIM_TimerCtlTypeDef pTimerCtl = {0};
  HRTIM_DeadTimeCfgTypeDef pDeadTimeCfg = {0};
  HRTIM_OutputCfgTypeDef pOutputCfg = {0};

  /* USER CODE BEGIN HRTIM1_Init 1 */

  /* USER CODE END HRTIM1_Init 1 */
  hhrtim1.Instance = HRTIM1;
  hhrtim1.Init.HRTIMInterruptResquests = HRTIM_IT_NONE;
  hhrtim1.Init.SyncOptions = HRTIM_SYNCOPTION_NONE;
  if (HAL_HRTIM_Init(&hhrtim1) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_HRTIM_DLLCalibrationStart(&hhrtim1, HRTIM_CALIBRATIONRATE_3) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_HRTIM_PollForDLLCalibration(&hhrtim1, 10) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_HRTIM_FaultPrescalerConfig(&hhrtim1, HRTIM_FAULTPRESCALER_DIV1) != HAL_OK)
  {
    Error_Handler();
  }
  pFaultBlkCfg.Threshold = 0;
  pFaultBlkCfg.ResetMode = HRTIM_FAULTCOUNTERRST_UNCONDITIONAL;
  pFaultBlkCfg.BlankingSource = HRTIM_FAULTBLANKINGMODE_RSTALIGNED;
  if (HAL_HRTIM_FaultCounterConfig(&hhrtim1, HRTIM_FAULT_2, &pFaultBlkCfg) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_HRTIM_FaultBlankingConfigAndEnable(&hhrtim1, HRTIM_FAULT_2, &pFaultBlkCfg) != HAL_OK)
  {
    Error_Handler();
  }
  pFaultCfg.Source = HRTIM_FAULTSOURCE_INTERNAL;
  pFaultCfg.Polarity = HRTIM_FAULTPOLARITY_HIGH;
  pFaultCfg.Filter = HRTIM_FAULTFILTER_NONE;
  pFaultCfg.Lock = HRTIM_FAULTLOCK_READWRITE;
  if (HAL_HRTIM_FaultConfig(&hhrtim1, HRTIM_FAULT_2, &pFaultCfg) != HAL_OK)
  {
    Error_Handler();
  }
  HAL_HRTIM_FaultModeCtl(&hhrtim1, HRTIM_FAULT_2, HRTIM_FAULTMODECTL_DISABLED);
  if (HAL_HRTIM_FaultCounterConfig(&hhrtim1, HRTIM_FAULT_4, &pFaultBlkCfg) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_HRTIM_FaultBlankingConfigAndEnable(&hhrtim1, HRTIM_FAULT_4, &pFaultBlkCfg) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_HRTIM_FaultConfig(&hhrtim1, HRTIM_FAULT_4, &pFaultCfg) != HAL_OK)
  {
    Error_Handler();
  }
  HAL_HRTIM_FaultModeCtl(&hhrtim1, HRTIM_FAULT_4, HRTIM_FAULTMODECTL_DISABLED);
  if (HAL_HRTIM_FaultCounterConfig(&hhrtim1, HRTIM_FAULT_5, &pFaultBlkCfg) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_HRTIM_FaultBlankingConfigAndEnable(&hhrtim1, HRTIM_FAULT_5, &pFaultBlkCfg) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_HRTIM_FaultConfig(&hhrtim1, HRTIM_FAULT_5, &pFaultCfg) != HAL_OK)
  {
    Error_Handler();
  }
  HAL_HRTIM_FaultModeCtl(&hhrtim1, HRTIM_FAULT_5, HRTIM_FAULTMODECTL_DISABLED);
  pADCTriggerCfg.UpdateSource = HRTIM_ADCTRIGGERUPDATE_MASTER;
  pADCTriggerCfg.Trigger = HRTIM_ADCTRIGGEREVENT13_MASTER_CMP2;
  if (HAL_HRTIM_ADCTriggerConfig(&hhrtim1, HRTIM_ADCTRIGGER_1, &pADCTriggerCfg) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_HRTIM_ADCPostScalerConfig(&hhrtim1, HRTIM_ADCTRIGGER_1, 0x0) != HAL_OK)
  {
    Error_Handler();
  }
  pADCTriggerCfg.Trigger = HRTIM_ADCTRIGGEREVENT13_MASTER_CMP3;
  if (HAL_HRTIM_ADCTriggerConfig(&hhrtim1, HRTIM_ADCTRIGGER_3, &pADCTriggerCfg) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_HRTIM_ADCPostScalerConfig(&hhrtim1, HRTIM_ADCTRIGGER_3, 0x0) != HAL_OK)
  {
    Error_Handler();
  }
  pTimeBaseCfg.Period = 13600;
  pTimeBaseCfg.RepetitionCounter = 0x00;
  pTimeBaseCfg.PrescalerRatio = HRTIM_PRESCALERRATIO_MUL32;
  pTimeBaseCfg.Mode = HRTIM_MODE_CONTINUOUS;
  if (HAL_HRTIM_TimeBaseConfig(&hhrtim1, HRTIM_TIMERINDEX_MASTER, &pTimeBaseCfg) != HAL_OK)
  {
    Error_Handler();
  }
  pTimerCfg.InterruptRequests = HRTIM_MASTER_IT_NONE;
  pTimerCfg.DMARequests = HRTIM_MASTER_DMA_NONE;
  pTimerCfg.DMASrcAddress = 0x0000;
  pTimerCfg.DMADstAddress = 0x0000;
  pTimerCfg.DMASize = 0x1;
  pTimerCfg.HalfModeEnable = HRTIM_HALFMODE_DISABLED;
  pTimerCfg.InterleavedMode = HRTIM_INTERLEAVED_MODE_DISABLED;
  pTimerCfg.StartOnSync = HRTIM_SYNCSTART_DISABLED;
  pTimerCfg.ResetOnSync = HRTIM_SYNCRESET_DISABLED;
  pTimerCfg.DACSynchro = HRTIM_DACSYNC_NONE;
  pTimerCfg.PreloadEnable = HRTIM_PRELOAD_ENABLED;
  pTimerCfg.UpdateGating = HRTIM_UPDATEGATING_INDEPENDENT;
  pTimerCfg.BurstMode = HRTIM_TIMERBURSTMODE_MAINTAINCLOCK;
  pTimerCfg.RepetitionUpdate = HRTIM_UPDATEONREPETITION_ENABLED;
  pTimerCfg.ReSyncUpdate = HRTIM_TIMERESYNC_UPDATE_UNCONDITIONAL;
  if (HAL_HRTIM_WaveformTimerConfig(&hhrtim1, HRTIM_TIMERINDEX_MASTER, &pTimerCfg) != HAL_OK)
  {
    Error_Handler();
  }
  pCompareCfg.CompareValue = 6800;
  if (HAL_HRTIM_WaveformCompareConfig(&hhrtim1, HRTIM_TIMERINDEX_MASTER, HRTIM_COMPAREUNIT_1, &pCompareCfg) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_HRTIM_WaveformCompareConfig(&hhrtim1, HRTIM_TIMERINDEX_MASTER, HRTIM_COMPAREUNIT_2, &pCompareCfg) != HAL_OK)
  {
    Error_Handler();
  }
  pCompareCfg.CompareValue = 10200;
  if (HAL_HRTIM_WaveformCompareConfig(&hhrtim1, HRTIM_TIMERINDEX_MASTER, HRTIM_COMPAREUNIT_3, &pCompareCfg) != HAL_OK)
  {
    Error_Handler();
  }
  pTimeBaseCfg.Period = 6800;
  if (HAL_HRTIM_TimeBaseConfig(&hhrtim1, HRTIM_TIMERINDEX_TIMER_E, &pTimeBaseCfg) != HAL_OK)
  {
    Error_Handler();
  }
  pTimerCtl.UpDownMode = HRTIM_TIMERUPDOWNMODE_UPDOWN;
  pTimerCtl.GreaterCMP1 = HRTIM_TIMERGTCMP1_GREATER;
  pTimerCtl.DualChannelDacEnable = HRTIM_TIMER_DCDE_DISABLED;
  if (HAL_HRTIM_WaveformTimerControl(&hhrtim1, HRTIM_TIMERINDEX_TIMER_E, &pTimerCtl) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_HRTIM_RollOverModeConfig(&hhrtim1, HRTIM_TIMERINDEX_TIMER_E, HRTIM_TIM_FEROM_VALLEY|HRTIM_TIM_BMROM_VALLEY
                              |HRTIM_TIM_ADROM_VALLEY|HRTIM_TIM_OUTROM_VALLEY
                              |HRTIM_TIM_ROM_VALLEY) != HAL_OK)
  {
    Error_Handler();
  }
  pTimerCfg.InterruptRequests = HRTIM_TIM_IT_NONE;
  pTimerCfg.DMARequests = HRTIM_TIM_DMA_NONE;
  pTimerCfg.PushPull = HRTIM_TIMPUSHPULLMODE_DISABLED;
  pTimerCfg.FaultEnable = HRTIM_TIMFAULTENABLE_NONE;
  pTimerCfg.FaultLock = HRTIM_TIMFAULTLOCK_READWRITE;
  pTimerCfg.DeadTimeInsertion = HRTIM_TIMDEADTIMEINSERTION_ENABLED;
  pTimerCfg.DelayedProtectionMode = HRTIM_TIMER_D_E_DELAYEDPROTECTION_DISABLED;
  pTimerCfg.UpdateTrigger = HRTIM_TIMUPDATETRIGGER_TIMER_E;
  pTimerCfg.ResetTrigger = HRTIM_TIMRESETTRIGGER_MASTER_CMP1;
  pTimerCfg.ResetUpdate = HRTIM_TIMUPDATEONRESET_ENABLED;
  if (HAL_HRTIM_WaveformTimerConfig(&hhrtim1, HRTIM_TIMERINDEX_TIMER_E, &pTimerCfg) != HAL_OK)
  {
    Error_Handler();
  }
  pTimerCfg.DelayedProtectionMode = HRTIM_TIMER_F_DELAYEDPROTECTION_DISABLED;
  pTimerCfg.UpdateTrigger = HRTIM_TIMUPDATETRIGGER_TIMER_F;
  pTimerCfg.ResetTrigger = HRTIM_TIMRESETTRIGGER_MASTER_PER;
  if (HAL_HRTIM_WaveformTimerConfig(&hhrtim1, HRTIM_TIMERINDEX_TIMER_F, &pTimerCfg) != HAL_OK)
  {
    Error_Handler();
  }
  pCompareCfg.CompareValue = 0;
  if (HAL_HRTIM_WaveformCompareConfig(&hhrtim1, HRTIM_TIMERINDEX_TIMER_E, HRTIM_COMPAREUNIT_1, &pCompareCfg) != HAL_OK)
  {
    Error_Handler();
  }
  pDeadTimeCfg.Prescaler = HRTIM_TIMDEADTIME_PRESCALERRATIO_MUL8;
  pDeadTimeCfg.RisingValue = 15;
  pDeadTimeCfg.RisingSign = HRTIM_TIMDEADTIME_RISINGSIGN_POSITIVE;
  pDeadTimeCfg.RisingLock = HRTIM_TIMDEADTIME_RISINGLOCK_WRITE;
  pDeadTimeCfg.RisingSignLock = HRTIM_TIMDEADTIME_RISINGSIGNLOCK_WRITE;
  pDeadTimeCfg.FallingValue = 15;
  pDeadTimeCfg.FallingSign = HRTIM_TIMDEADTIME_FALLINGSIGN_POSITIVE;
  pDeadTimeCfg.FallingLock = HRTIM_TIMDEADTIME_FALLINGLOCK_WRITE;
  pDeadTimeCfg.FallingSignLock = HRTIM_TIMDEADTIME_FALLINGSIGNLOCK_WRITE;
  if (HAL_HRTIM_DeadTimeConfig(&hhrtim1, HRTIM_TIMERINDEX_TIMER_E, &pDeadTimeCfg) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_HRTIM_DeadTimeConfig(&hhrtim1, HRTIM_TIMERINDEX_TIMER_F, &pDeadTimeCfg) != HAL_OK)
  {
    Error_Handler();
  }
  pOutputCfg.Polarity = HRTIM_OUTPUTPOLARITY_HIGH;
  pOutputCfg.SetSource = HRTIM_OUTPUTSET_TIMCMP1;
  pOutputCfg.ResetSource = HRTIM_OUTPUTRESET_NONE;
  pOutputCfg.IdleMode = HRTIM_OUTPUTIDLEMODE_NONE;
  pOutputCfg.IdleLevel = HRTIM_OUTPUTIDLELEVEL_INACTIVE;
  pOutputCfg.FaultLevel = HRTIM_OUTPUTFAULTLEVEL_NONE;
  pOutputCfg.ChopperModeEnable = HRTIM_OUTPUTCHOPPERMODE_DISABLED;
  pOutputCfg.BurstModeEntryDelayed = HRTIM_OUTPUTBURSTMODEENTRY_REGULAR;
  if (HAL_HRTIM_WaveformOutputConfig(&hhrtim1, HRTIM_TIMERINDEX_TIMER_E, HRTIM_OUTPUT_TE1, &pOutputCfg) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_HRTIM_WaveformOutputConfig(&hhrtim1, HRTIM_TIMERINDEX_TIMER_F, HRTIM_OUTPUT_TF1, &pOutputCfg) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_HRTIM_WaveformOutputConfig(&hhrtim1, HRTIM_TIMERINDEX_TIMER_E, HRTIM_OUTPUT_TE2, &pOutputCfg) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_HRTIM_WaveformOutputConfig(&hhrtim1, HRTIM_TIMERINDEX_TIMER_F, HRTIM_OUTPUT_TF2, &pOutputCfg) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_HRTIM_TimeBaseConfig(&hhrtim1, HRTIM_TIMERINDEX_TIMER_F, &pTimeBaseCfg) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_HRTIM_WaveformTimerControl(&hhrtim1, HRTIM_TIMERINDEX_TIMER_F, &pTimerCtl) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_HRTIM_RollOverModeConfig(&hhrtim1, HRTIM_TIMERINDEX_TIMER_F, HRTIM_TIM_FEROM_VALLEY|HRTIM_TIM_BMROM_VALLEY
                              |HRTIM_TIM_ADROM_VALLEY|HRTIM_TIM_OUTROM_VALLEY
                              |HRTIM_TIM_ROM_VALLEY) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_HRTIM_WaveformCompareConfig(&hhrtim1, HRTIM_TIMERINDEX_TIMER_F, HRTIM_COMPAREUNIT_1, &pCompareCfg) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN HRTIM1_Init 2 */

  /* USER CODE END HRTIM1_Init 2 */
  HAL_HRTIM_MspPostInit(&hhrtim1);

}

/**
  * @brief I2C1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_I2C1_Init(void)
{

  /* USER CODE BEGIN I2C1_Init 0 */

  /* USER CODE END I2C1_Init 0 */

  /* USER CODE BEGIN I2C1_Init 1 */

  /* USER CODE END I2C1_Init 1 */
  hi2c1.Instance = I2C1;
  hi2c1.Init.Timing = 0x40B285C2;
  hi2c1.Init.OwnAddress1 = 0;
  hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
  hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
  hi2c1.Init.OwnAddress2 = 0;
  hi2c1.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
  hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
  hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
  if (HAL_I2C_Init(&hi2c1) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Analogue filter
  */
  if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Digital filter
  */
  if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN I2C1_Init 2 */

  /* USER CODE END I2C1_Init 2 */

}

/**
  * @brief OPAMP1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_OPAMP1_Init(void)
{

  /* USER CODE BEGIN OPAMP1_Init 0 */

  /* USER CODE END OPAMP1_Init 0 */

  /* USER CODE BEGIN OPAMP1_Init 1 */

  /* USER CODE END OPAMP1_Init 1 */
  hopamp1.Instance = OPAMP1;
  hopamp1.Init.PowerMode = OPAMP_POWERMODE_HIGHSPEED;
  hopamp1.Init.Mode = OPAMP_STANDALONE_MODE;
  hopamp1.Init.InvertingInput = OPAMP_INVERTINGINPUT_IO0;
  hopamp1.Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO0;
  hopamp1.Init.InternalOutput = DISABLE;
  hopamp1.Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE;
  hopamp1.Init.UserTrimming = OPAMP_TRIMMING_FACTORY;
  if (HAL_OPAMP_Init(&hopamp1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN OPAMP1_Init 2 */

  /* USER CODE END OPAMP1_Init 2 */

}

/**
  * @brief OPAMP2 Initialization Function
  * @param None
  * @retval None
  */
static void MX_OPAMP2_Init(void)
{

  /* USER CODE BEGIN OPAMP2_Init 0 */

  /* USER CODE END OPAMP2_Init 0 */

  /* USER CODE BEGIN OPAMP2_Init 1 */

  /* USER CODE END OPAMP2_Init 1 */
  hopamp2.Instance = OPAMP2;
  hopamp2.Init.PowerMode = OPAMP_POWERMODE_HIGHSPEED;
  hopamp2.Init.Mode = OPAMP_STANDALONE_MODE;
  hopamp2.Init.InvertingInput = OPAMP_INVERTINGINPUT_IO0;
  hopamp2.Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO0;
  hopamp2.Init.InternalOutput = DISABLE;
  hopamp2.Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE;
  hopamp2.Init.UserTrimming = OPAMP_TRIMMING_FACTORY;
  if (HAL_OPAMP_Init(&hopamp2) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN OPAMP2_Init 2 */

  /* USER CODE END OPAMP2_Init 2 */

}

/**
  * @brief TIM1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM1_Init(void)
{

  /* USER CODE BEGIN TIM1_Init 0 */

  /* USER CODE END TIM1_Init 0 */

  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
  TIM_MasterConfigTypeDef sMasterConfig = {0};
  TIM_OC_InitTypeDef sConfigOC = {0};
  TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0};

  /* USER CODE BEGIN TIM1_Init 1 */

  /* USER CODE END TIM1_Init 1 */
  htim1.Instance = TIM1;
  htim1.Init.Prescaler = 20-1;
  htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim1.Init.Period = 3542;
  htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim1.Init.RepetitionCounter = 0;
  htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  if (HAL_TIM_Base_Init(&htim1) != HAL_OK)
  {
    Error_Handler();
  }
  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
  if (HAL_TIM_ConfigClockSource(&htim1, &sClockSourceConfig) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_TIM_PWM_Init(&htim1) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sConfigOC.OCMode = TIM_OCMODE_PWM1;
  sConfigOC.Pulse = 1771;
  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
  sConfigOC.OCFastMode = TIM_OCFAST_ENABLE;
  sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
  sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
  if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
  {
    Error_Handler();
  }
  sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
  sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;
  sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
  sBreakDeadTimeConfig.DeadTime = 0;
  sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE;
  sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH;
  sBreakDeadTimeConfig.BreakFilter = 0;
  sBreakDeadTimeConfig.BreakAFMode = TIM_BREAK_AFMODE_INPUT;
  sBreakDeadTimeConfig.Break2State = TIM_BREAK2_DISABLE;
  sBreakDeadTimeConfig.Break2Polarity = TIM_BREAK2POLARITY_HIGH;
  sBreakDeadTimeConfig.Break2Filter = 0;
  sBreakDeadTimeConfig.Break2AFMode = TIM_BREAK_AFMODE_INPUT;
  sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
  if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM1_Init 2 */

  /* USER CODE END TIM1_Init 2 */
  HAL_TIM_MspPostInit(&htim1);

}

/**
  * @brief TIM6 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM6_Init(void)
{

  /* USER CODE BEGIN TIM6_Init 0 */

  /* USER CODE END TIM6_Init 0 */

  TIM_MasterConfigTypeDef sMasterConfig = {0};

  /* USER CODE BEGIN TIM6_Init 1 */

  /* USER CODE END TIM6_Init 1 */
  htim6.Instance = TIM6;
  htim6.Init.Prescaler = 1700-1;
  htim6.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim6.Init.Period = 100;
  htim6.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  if (HAL_TIM_Base_Init(&htim6) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim6, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM6_Init 2 */

  /* USER CODE END TIM6_Init 2 */

}

/**
  * @brief TIM7 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM7_Init(void)
{

  /* USER CODE BEGIN TIM7_Init 0 */

  /* USER CODE END TIM7_Init 0 */

  TIM_MasterConfigTypeDef sMasterConfig = {0};

  /* USER CODE BEGIN TIM7_Init 1 */

  /* USER CODE END TIM7_Init 1 */
  htim7.Instance = TIM7;
  htim7.Init.Prescaler = 3400-1;
  htim7.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim7.Init.Period = 5000;
  htim7.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  if (HAL_TIM_Base_Init(&htim7) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim7, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM7_Init 2 */

  /* USER CODE END TIM7_Init 2 */

}

/**
  * @brief TIM16 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM16_Init(void)
{

  /* USER CODE BEGIN TIM16_Init 0 */

  /* USER CODE END TIM16_Init 0 */

  /* USER CODE BEGIN TIM16_Init 1 */

  /* USER CODE END TIM16_Init 1 */
  htim16.Instance = TIM16;
  htim16.Init.Prescaler = 170-1;
  htim16.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim16.Init.Period = 20;
  htim16.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim16.Init.RepetitionCounter = 0;
  htim16.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  if (HAL_TIM_Base_Init(&htim16) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM16_Init 2 */

  /* USER CODE END TIM16_Init 2 */

}

/**
  * @brief UART4 Initialization Function
  * @param None
  * @retval None
  */
static void MX_UART4_Init(void)
{

  /* USER CODE BEGIN UART4_Init 0 */

  /* USER CODE END UART4_Init 0 */

  /* USER CODE BEGIN UART4_Init 1 */

  /* USER CODE END UART4_Init 1 */
  huart4.Instance = UART4;
  huart4.Init.BaudRate = 460800;
  huart4.Init.WordLength = UART_WORDLENGTH_8B;
  huart4.Init.StopBits = UART_STOPBITS_1;
  huart4.Init.Parity = UART_PARITY_NONE;
  huart4.Init.Mode = UART_MODE_TX_RX;
  huart4.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart4.Init.OverSampling = UART_OVERSAMPLING_16;
  huart4.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
  huart4.Init.ClockPrescaler = UART_PRESCALER_DIV1;
  huart4.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
  if (HAL_UART_Init(&huart4) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_UARTEx_SetTxFifoThreshold(&huart4, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_UARTEx_SetRxFifoThreshold(&huart4, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_UARTEx_DisableFifoMode(&huart4) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN UART4_Init 2 */

  /* USER CODE END UART4_Init 2 */

}

/**
  * Enable DMA controller clock
  */
static void MX_DMA_Init(void)
{

  /* DMA controller clock enable */
  __HAL_RCC_DMAMUX1_CLK_ENABLE();
  __HAL_RCC_DMA2_CLK_ENABLE();
  __HAL_RCC_DMA1_CLK_ENABLE();

}

/**
  * @brief GPIO Initialization Function
  * @param None
  * @retval None
  */
static void MX_GPIO_Init(void)
{
  GPIO_InitTypeDef GPIO_InitStruct = {0};
  /* USER CODE BEGIN MX_GPIO_Init_1 */

  /* USER CODE END MX_GPIO_Init_1 */

  /* GPIO Ports Clock Enable */
  __HAL_RCC_GPIOC_CLK_ENABLE();
  __HAL_RCC_GPIOF_CLK_ENABLE();
  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();
  __HAL_RCC_GPIOD_CLK_ENABLE();

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOC, TEST_PC5_Pin|GPIO_PIN_12, GPIO_PIN_RESET);

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOB, DISCONNECT_OUTPUT_Pin|ENABLE_ISO_FET_Pin|PRECHARGE_Pin|DISCONNECT_INPUT_Pin, GPIO_PIN_RESET);

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(FDCAN_SILENT_GPIO_Port, FDCAN_SILENT_Pin, GPIO_PIN_RESET);

  /*Configure GPIO pins : TEST_PC5_Pin PC12 */
  GPIO_InitStruct.Pin = TEST_PC5_Pin|GPIO_PIN_12;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);

  /*Configure GPIO pins : DISCONNECT_OUTPUT_Pin ENABLE_ISO_FET_Pin PRECHARGE_Pin DISCONNECT_INPUT_Pin */
  GPIO_InitStruct.Pin = DISCONNECT_OUTPUT_Pin|ENABLE_ISO_FET_Pin|PRECHARGE_Pin|DISCONNECT_INPUT_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

  /*Configure GPIO pin : FDCAN_SILENT_Pin */
  GPIO_InitStruct.Pin = FDCAN_SILENT_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(FDCAN_SILENT_GPIO_Port, &GPIO_InitStruct);

  /* USER CODE BEGIN MX_GPIO_Init_2 */

  /* USER CODE END MX_GPIO_Init_2 */
}

/* USER CODE BEGIN 4 */


void activateALARM()
{
	HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_1);
}
void deactivateALARM()
{
	HAL_TIMEx_PWMN_Stop(&htim1,TIM_CHANNEL_1);
}

void adjustVREF (uint16_t vref)
{
	if (vref == (ADC3->OFR1 & 0xFFF)) return;
	ADC3->OFR1 = (ADC3->OFR1 & ~0xFFFU) | (vref & 0xFFFU);
	VREF = vref;
}

void adjustIINREF (int a)
{
	if ((((ADC1->OFR1 & 0xFFF) + a) > 0xFFF) || (((ADC1->OFR1 & 0xFFF) +a) < 0))
	{
		turnOff();
		Error_Handler();
	}
	uint16_t vref = (ADC1->OFR1 & 0xFFF) + a; // this is called during init, so an offset already exists, this will fine-tune on top of that offset
	ADC1->CR |= ADC_CR_ADSTP;
	while (ADC1->CR & ADC_CR_ADSTART) {}; // todo: do timeout with tickstart
	ADC1->OFR1 &=(uint32_t) ~ 0xFFF ;
	ADC1->OFR1 |= (vref & (uint32_t)0xFFF);
	ADC1->CR |= ADC_CR_ADSTART;
}


void HAL_FMAC_ErrorCallback (FMAC_HandleTypeDef * hfmac)
{
	uint32_t sr = FMAC->SR;
	FMAC->CR |= (1 << 16); // reset FMAC

	char fmac_msg[64];
	snprintf(fmac_msg, sizeof(fmac_msg), "FMAC SR:%08lX%s%s%s%s%s", sr,
			(sr & (1 << 8))  ? " OVF"   : "",
			(sr & (1 << 9))  ? " UNF"   : "",
			(sr & (1 << 10)) ? " SAT"   : "",
			(sr & (1 << 0))  ? " YEMP"  : "",
			(sr & (1 << 1))  ? " X1EMP" : "");
	Proto_SendError(fmac_msg);

	static int16_t aOutputDataToPreload[COEFF_VECTOR_A_SIZE] = {0x0000,0x0000,0x0000};
    if (HAL_FMAC_FilterPreload(hfmac,(int16_t *) NULL, INPUT_BUFFER_SIZE,
			  aOutputDataToPreload, COEFF_VECTOR_A_SIZE) != HAL_OK) Error_Handler();

	uint16_t ExpectedCalculatedOutputSize = (uint16_t) 1;
	if (HAL_FMAC_FilterStart(hfmac,0,&ExpectedCalculatedOutputSize) != HAL_OK) Error_Handler();

}

void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef *hadc)
{

	return;
}

void turnOff()
{

	 LL_HRTIM_DisableOutput(HRTIM1,LL_HRTIM_OUTPUT_TE1 | LL_HRTIM_OUTPUT_TE2 | LL_HRTIM_OUTPUT_TF1 | LL_HRTIM_OUTPUT_TF2);
	 LL_HRTIM_TIM_CounterDisable(HRTIM1, LL_HRTIM_TIMER_E | LL_HRTIM_TIMER_F | LL_HRTIM_TIMER_MASTER);

	 HAL_GPIO_WritePin(ENABLE_ISO_FET_GPIO_Port,ENABLE_ISO_FET_Pin,GPIO_PIN_RESET);
	 HAL_GPIO_WritePin(DISCONNECT_INPUT_GPIO_Port,DISCONNECT_INPUT_Pin,GPIO_PIN_RESET);
	 HAL_GPIO_WritePin(DISCONNECT_OUTPUT_GPIO_Port,DISCONNECT_OUTPUT_Pin,GPIO_PIN_RESET);
	 {
		 char fault_msg[64];
		 snprintf(fault_msg,sizeof(fault_msg),"SHUTOFF V:%lu/%lu I:%i/%lu",
						(uint32_t)roundf(vin),(uint32_t)roundf(vout),(int)roundf(iin),(uint32_t)roundf(iout));
		 Proto_SendError(fault_msg);
	 }

}

void HAL_COMP_TriggerCallback(COMP_HandleTypeDef *hcomp)
{
	turnOff();
	if (hcomp == &hcomp1) Proto_SendError("FAULT: VOUT COMP");
	else if (hcomp == &hcomp3) Proto_SendError("FAULT: I_IN COMP");
	else if (hcomp == &hcomp4) Proto_SendError("FAULT: I_LOAD COMP");
	Error_Handler();
}


void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
	if (htim == &htim16) // this is the 50kHz Loop
	{
		//limit checks, VFLY balancing, CC/CV scheme
		vin = ADC_12BIT_2_5V_RESOLUTION * VIN_MULTIPLICATOR * *rvin;
		vout = ADC_12BIT_2_5V_RESOLUTION * VOUT_MULTIPLICATOR * ((*rvout >> 3)+VREF);
		iin = ADC_12BIT_2_5V_RESOLUTION * IIN_MULTIPLICATOR * *riin;
		iout = ADC_12BIT_2_5V_RESOLUTION * IOUT_MULTIPLICATOR * *riout;
		vfly = ADC_12BIT_2_5V_RESOLUTION * VFLY_MULTIPLICATOR * *rvfly;

		if (vfly_active)
		{
			vfly_accumulator += vfly;
			vfly_loop_counter++;
			if (vfly_loop_counter >= vfly_loop_counter_trigger)
			{
				vfly_accumulator /= vfly_loop_counter_trigger;
				vfly_avg_debug = vfly_accumulator;
				float vfly_error = vin / 2.0f - vfly_accumulator;
				vfly_integral += vfly_error * vfly_ki;
				if (vfly_integral < -(float)vfly_clamp) vfly_integral = -(float)vfly_clamp;
				if (vfly_integral > (float)vfly_clamp) vfly_integral = (float)vfly_clamp;
				float vfly_output = vfly_integral + vfly_error * vfly_kp;
				if (vfly_output < -(float)vfly_clamp) vfly_output = -(float)vfly_clamp;
				if (vfly_output > (float)vfly_clamp) vfly_output = (float)vfly_clamp;
				vfly_correction = (int16_t)roundf(vfly_output);
				vfly_accumulator = 0;
				vfly_loop_counter = 0;
			}
		}
		else { vfly_correction = 0; vfly_integral = 0; }
		etemp = (ADC_12BIT_2_5V_RESOLUTION * ETEMP_MULTIPLICATOR * *retemp) - ETEMP_SUBTRACTION;

		if (vout > VOUT_MAX || vin > VIN_MAX || iin > IIN_MAX || iin < IIN_MIN || iout > IOUT_MAX || iout < IOUT_MIN || vfly > VFLY_MAX || vout > vin || etemp > ETEMP_MAX || etemp < ETEMP_MIN)
		{
			const char *reason = "LIMIT";
			if (vout > VOUT_MAX) reason = "LIMIT: VOUT_MAX";
			else if (vin > VIN_MAX) reason = "LIMIT: VIN_MAX";
			else if (iin > IIN_MAX) reason = "LIMIT: IIN_MAX";
			else if (iin < IIN_MIN) reason = "LIMIT: IIN_MIN";
			else if (iout > IOUT_MAX) reason = "LIMIT: IOUT_MAX";
			else if (iout < IOUT_MIN) reason = "LIMIT: IOUT_MIN";
			else if (vfly > VFLY_MAX) reason = "LIMIT: VFLY_MAX";
			else if (vout > vin) reason = "LIMIT: VOUT>VIN";
			else if (etemp > ETEMP_MAX) reason = "LIMIT: ETEMP_MAX";
			else if (etemp < ETEMP_MIN) reason = "LIMIT: ETEMP_MIN";
			Proto_SendError(reason);
			turnOff();
			Error_Handler();
		}

		// Adaptive deadtime — lookup segment for current iout
		{
			uint8_t dt = dt_values[0];
			for (int i = DT_NUM_SEGMENTS - 1; i >= 0; i--)
			{
				if (iout >= dt_breakpoints[i])
				{
					dt = dt_values[i];
					break;
				}
			}
			if (dt < DT_HARD_MIN) dt = DT_HARD_MIN;
			uint32_t dt_bits = (uint32_t)dt | ((uint32_t)dt << 16);
			MODIFY_REG(HRTIM1->sTimerxRegs[HRTIM_TIMERINDEX_TIMER_E].DTxR,
			           HRTIM_DTR_DTR | HRTIM_DTR_DTF, dt_bits);
			MODIFY_REG(HRTIM1->sTimerxRegs[HRTIM_TIMERINDEX_TIMER_F].DTxR,
			           HRTIM_DTR_DTR | HRTIM_DTR_DTF, dt_bits);
		}

		if (cc_active && cc_target >= 500.0f)
		{
			/* Vin floor protection: clamp internal target without touching cc_target */
			float cc_target_corrected = cc_target;
			if (vin < vin_min_ctrl)
			{
				cc_target_corrected = cc_target * (vin / vin_min_ctrl);
				if (cc_target_corrected < mppt_iref_min) cc_target_corrected = mppt_iref_min;
			}

			cc_iout_accumulator += iout;
			cc_loop_counter++;
			if (cc_loop_counter >= cc_loop_counter_trigger)
			{
				cc_iout_accumulator /= cc_loop_counter_trigger;
				adjustVREF(CC_Update(&cc,cc_target_corrected,cc_iout_accumulator));
				cc_iout_accumulator = 0;
				cc_loop_counter = 0;
			}
		}


		return;
	}
	else if (htim == &htim6) // this is the 100Hz Loop
	{
		if (mppt_active)
		{
			avg_iin += -iin;// inverted because power going in is currently negative
			avg_vin += vin;
			mppt_loop_counter++;
			if (mppt_loop_counter >= mppt_loop_counter_trigger)
			{
				avg_iin /= mppt_loop_counter_trigger;
				avg_vin /= mppt_loop_counter_trigger;
				float old_iref = mppt.iref;
				cc_target = MPPT_IncCond_Update(&mppt, avg_vin, avg_iin);
				/* Vin floor: allow algorithm to decrease iref, but block increases */
				if (avg_vin < vin_min_ctrl && mppt.iref > old_iref)
				{
					mppt.iref = old_iref;
					cc_target = old_iref;
				}
				mppt_loop_counter = 0;
				avg_iin = 0;
				avg_vin = 0;
			}
		}
		return;
	}
	else if (htim == &htim7) // this is the 10Hz Loop (telemetry)
	{
		Proto_SendTelemetry();
		return;
	}
}

void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
{
	Proto_TxCpltCallback(huart);
}

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
	Proto_RxCpltCallback(huart);
}

void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart)
{
	Proto_ErrorCallback(huart);
}

void HAL_RCC_CSSCallback()
{
	Proto_SendError("FAULT: Clock Security System");
	turnOff();
	Error_Handler();
}



/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  Proto_SendDiagDump("ERROR_HANDLER");
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
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,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
