MotorStep42L/Core/Src/main.c

/* 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"
#include "adc.h"
#include "can.h"
#include "dma.h"
#include "rtc.h"
#include "spi.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

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

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

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

// Sysled ----------------------------------------------------------------------
#include "LED_ATY.h"
void SysLed_1_IO_SET(uint8_t level){
    if(level){
        HAL_GPIO_WritePin(SYSLED_GPIO_Port, SYSLED_Pin, GPIO_PIN_SET);
    }
    else{
        HAL_GPIO_WritePin(SYSLED_GPIO_Port, SYSLED_Pin, GPIO_PIN_RESET);
    }
}
struct LED_ATY_Dev LED_ATY_t_1 = {
    .ledBlinkStep = 0,
    .ledBlinkType = 0x20,
    .ioSet = SysLed_1_IO_SET,
    .longSteps = 20,
    .lock = __ATY_UNLOCKED
};

// Timer loop ------------------------------------------------------------------
#include "HW_TIMER_ATY.h"
void TimerLoopProcess_User_4(void);
void TimerInit_4(){
    HAL_TIM_Base_Start_IT(&htim4);
}
struct HW_TIMER_ATY_Dev HW_TIMER_ATY_Dev_4 = {
    .msN = 0,
    .ms10 = 0,
    .ms100 = 0,
    .s1 = 0,

    .ms1_f = 0,
    .ms10_f = 0,
    .ms100_f = 0,
    .s1_f = 0,

    .timerInitFlag = 0,
    .timerInit = TimerInit_4,

    .timerLoopProcess_User = TimerLoopProcess_User_4,

    .lock = __ATY_UNLOCKED
};

// Uart ------------------------------------------------------------------------
#include "HW_UART_ATY.h"
extern DMA_HandleTypeDef hdma_usart2_rx;
void UartReceiveProcess_User_1(void);
void UartInit_1(uint8_t* rx){
    HAL_UARTEx_ReceiveToIdle_DMA(&huart2, rx, UART_RX_MAX_LEN);
    __HAL_DMA_DISABLE_IT(&hdma_usart2_rx, DMA_IT_HT);
}
void UartSendByte_1(uint8_t byte){
    HAL_UART_Transmit(&huart2, (uint8_t*)&byte, 1, 0xFFFF);
}
struct HW_UART_ATY_Dev HW_UART_ATY_Dev_1 = {
    .rx = {0},
    .rxCount = 0,
    .txCount = 0,
    .rcvOverTimeOutCountNum = 1,
    .rcvOverTimeOutCount = 0,
    .rcvOverFlag = 0,

    .uartInitFlag = 0,
    .uartInit = UartInit_1,
    .uartSendByte = UartSendByte_1,

    .uartReceiveProcess_User = UartReceiveProcess_User_1,

    .lock = __ATY_UNLOCKED
};
void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef* huart, uint16_t Size){
    if(huart == &huart2){
        HAL_UART_DMAStop(&huart2);
        HW_UART_ATY_Dev_1.rcvOverTimeOutCount = 0;
        HW_UART_ATY_Dev_1.rxCount = UART_RX_MAX_LEN - __HAL_DMA_GET_COUNTER(&hdma_usart2_rx);
        HW_UART_ATY_Dev_1.rcvOverFlag = 0;

        HAL_UARTEx_ReceiveToIdle_DMA(&huart2, HW_UART_ATY_Dev_1.rx, UART_RX_MAX_LEN);
        __HAL_DMA_DISABLE_IT(&hdma_usart2_rx, DMA_IT_HT);
    }
}

// Regs ------------------------------------------------------------------------
float rgf[200 / 2] = {0};

// sys
#define RGF_SYS_TIME rgf[0]                         // r  // uint   // sys run time in sec
#define RGF_SYS_ADDR rgf[1]                         // rw // uint8  // sys address
#define RGF_SYS_GROUP_ID rgf[2]                     //  w // uint8  // sys group id,
#define RGF_SYS_ERROR rgf[3]                        // rw // uint8  // sys error
#define RGF_SOFTWARE_RST rgf[4]                     //  w // uint1  // sys software reset
#define RGF_UPDATE_RST rgf[5]                       //  w // uint1  // sys update reset
// ad
#define RGF_VOL_PER_AD rgf[10]                      // r  // float  // ad vref ad
#define RGF_MCU_BASE_VOL rgf[11]                    // r  // float  // ad vref vol
#define RGF_MCU_TEMP rgf[12]                        // r  // float  // mcu T
#define RGF_BOARD_TEMP rgf[13]                      // r  // float  // board ntc T, and for fan T use

// motor fan
#define RGF_MOTOR_FAN_EN rgf[20]                    //  w // uint8  // 0: disable, 1: enable, 2: always full on
#define RGF_MOTOR_FAN_BASE rgf[21]                  //  w // uint8  // min dutycycle for fan running
#define RGF_MOTOR_FAN_LOW_T rgf[22]                 //  w // uint8  // min T for fan start
#define RGF_MOTOR_FAN_HIGH_T rgf[23]                //  w // uint8  // max T for fan full on
#define RGF_MOTOR_FAN_T_SPACE rgf[24]               //  w // uint8  // fan T space to jump speed level
// motor angle base
#define RGF_MOTOR_ANGLE_SET_ZERO rgf[25]            //  w // uint1  // 1: set zero now, auto clear, zero point is current angle
#define RGF_MOTOR_ANGLE_ZERO rgf[26]                // r  // uint16 // current zero point value
#define RGF_MOTOR_ANGLE rgf[27]                     // r  // float  // current angle
#define RGF_MOTOR_ANGLE_ERR rgf[28]                 // r  // uint8  // current angle error
// motor angle correction
#define RGF_MOTOR_ANGLE_ANGLE_START rgf[30]         // r  // float  // angle when motor start
#define RGF_MOTOR_ANGLE_TURN_COUNT rgf[31]          // r  // uint32 // current turn count this start
#define RGF_MOTOR_ANGLE_ANGLE_TOTAL rgf[32]         // r  // float  // last agle within 720 degree for motor compensate
#define RGF_MOTOR_ANGLE_START rgf[33]               // r  // uint1  // MT6816 start flag when motor start

// useless motor params
#define RGF_MOTOR_FREQUENCY rgf[35]                 //  w // float  // motor pwm frequency
#define RGF_MOTOR_DUTY_CYCLE rgf[36]                //  w // float  // motor pwm duty cycle, 0 - 100

#define RGF_MOTOR_FULL_STEPS rgf[38]                //  w // uint32 // motor full steps, = 360 / stpDegree, 1.8 = 200
#define RGF_MOTOR_MICROSTEPPING rgf[39]             //  w // uint8  // controller microstepping

// motor base
#define RGF_MOTOR_ENABLE rgf[40]                    //  w // uint1  // motor power control, influenced by autoPower flag
#define RGF_MOTOR_DIRECTION rgf[41]                 //  w // uint1  // motor run direction
#define RGF_MOTOR_SPEED rgf[42]                     //  w // uint32 // motor speed in rad/s
#define RGF_MOTOR_UNIT_DIVISION rgf[43]             //  w // uint32 // motor params unit division, 1/10/100... like
#define RGF_MOTOR_ACCELERATION rgf[44]              //  w // uint32 // motor acceleration in rad/s^2
#define RGF_MOTOR_DECELERATION rgf[45]              //  w // uint32 // motor deceleration in rad/s^2

#define RGF_MOTOR_MODE rgf[46]                      //  w // uint8  // motor mode, defien in MOTOR_STEP_SPEED_T_C_ATY.h
#define RGF_MOTOR_ANGLE_LOCK_TARGET rgf[47]         //  w // float  // motor angle lock target
#define RGF_MOTOR_AUTO_POWER rgf[48]                //  w // uint1  // motor auto power, if this enable, ENABLE flag is not useful
#define RGF_MOTOR_RUN_STATE rgf[49]                 // rw // uint8  // motor run state, defien in MOTOR_STEP_SPEED_T_C_ATY.h
#define RGF_MOTOR_RUN rgf[50]                       //  w // uint1  // motor run command, set 1 to run
#define RGF_MOTOR_STEP_COUNT rgf[51]                //  w // uint32 // motor run step count, only valid in COUNT mode

#define RGF_MOTOR_CURRENT_STEPS rgf[52]             // r  // uint32 // motor current run steps
#define RGF_MOTOR_ACCEL_STEPS rgf[53]               // r  // uint32 // motor accel steps
#define RGF_MOTOR_ACCEL_LIMIT rgf[54]               // r  // uint32 // motor accel steps when accel to full speed
#define RGF_MOTOR_DECEL_STEPS rgf[55]               // r  // uint32 // motor decel steps
#define RGF_MOTOR_DECEL_START rgf[56]               // r  // uint32 // motor decel start step number

#define RGF_MOTOR_INFO_FROM rgf[60]                 //  w // uint8  // 0: MT6816 SM, 1: MB SF, 2: MB flowmeter
#define RGF_MOTOR_INFO_REG1_TARGET rgf[61]          //  w // uint32 // motor info register 1 target
#define RGF_MOTOR_INFO_REG1_CURRENT rgf[62]         // r  // uint32 // motor info register 1 current
#define RGF_MOTOR_INFO_REG2_TARGET rgf[63]          //  w // uint32 // motor info register 2 target
#define RGF_MOTOR_INFO_REG2_CURRENT rgf[64]         // r  // uint32 // motor info register 2 current

// Modbus ----------------------------------------------------------------------
#include "MODBUS_S_LOW_ATY.h"
uint8_t MODBUS1_UART(uint8_t* buf, uint8_t len){
    HAL_GPIO_WritePin(RS485_EN_GPIO_Port, RS485_EN_Pin, GPIO_PIN_SET);
    UartSendBytes(buf, len, &HW_UART_ATY_Dev_1);
    HAL_GPIO_WritePin(RS485_EN_GPIO_Port, RS485_EN_Pin, GPIO_PIN_RESET);
    return 0;
}
struct MODBUS_S_LOW_ATY_Dev MODBUS_S_LOW_ATY_Dev_1 = {
    .addr = 0x01,
    .mbRegs = {0},
    .uartSendBytes = MODBUS1_UART,
    .bigEndian = 0,
    .lock = __ATY_UNLOCKED
};

// CAN -------------------------------------------------------------------------
#include "CAN_ATY.h"
uint8_t CAN_1_FilterConfig(uint8_t bankId){
    CAN_FilterTypeDef sFilterConfig;

    if(bankId > 14){
        sFilterConfig.FilterBank = CAN_FILTER_BROADCAST;
        sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
        sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
        sFilterConfig.FilterIdHigh = 0x0000;
        sFilterConfig.FilterIdLow = 0x0000;
        sFilterConfig.FilterMaskIdHigh = 0x0000;
        sFilterConfig.FilterMaskIdLow = 0x0000;
        sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0;
        sFilterConfig.FilterActivation = ENABLE;
        sFilterConfig.SlaveStartFilterBank = 0;
        if(HAL_CAN_ConfigFilter(&hcan, &sFilterConfig) != HAL_OK)
            return 9;
    }
    if(bankId == 0 || bankId == CAN_FILTER_BROADCAST){
        sFilterConfig.FilterBank = CAN_FILTER_BROADCAST;
        sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
        sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
        sFilterConfig.FilterIdHigh = ((CAN_STD_ID_BROADCAST << 21) >> 16);
        sFilterConfig.FilterIdLow = (CAN_STD_ID_BROADCAST << 21) & 0xFFFF;
        sFilterConfig.FilterMaskIdHigh = 0xFFFF;
        sFilterConfig.FilterMaskIdLow = 0xFFFF;
        sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO1;
        sFilterConfig.FilterActivation = ENABLE;
        sFilterConfig.SlaveStartFilterBank = 0;
        if(HAL_CAN_ConfigFilter(&hcan, &sFilterConfig) != HAL_OK)
            return 1;
    }

    if(bankId == 0 || bankId == CAN_FILTER_MULTICAST){
        sFilterConfig.FilterBank = CAN_FILTER_MULTICAST;
        sFilterConfig.FilterIdHigh = (((CAN_STD_ID_MULTICAST + (uint8_t)RGF_SYS_GROUP_ID) << 21) >> 16);
        sFilterConfig.FilterIdLow = ((CAN_STD_ID_MULTICAST + (uint8_t)RGF_SYS_GROUP_ID) << 21) & 0xFFFF;
        sFilterConfig.FilterMaskIdHigh = 0xFFFF;
        sFilterConfig.FilterMaskIdLow = 0xFFFF;
        sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO1;
        sFilterConfig.FilterActivation = ENABLE;
        if(HAL_CAN_ConfigFilter(&hcan, &sFilterConfig) != HAL_OK)
            return 2;
    }

    if(bankId == 0 || bankId == CAN_FILTER_P2P){
        sFilterConfig.FilterBank = CAN_FILTER_P2P;
        sFilterConfig.FilterIdHigh = (((CAN_STD_ID_P2P + (uint8_t)RGF_SYS_ADDR) << 21) >> 16);
        sFilterConfig.FilterIdLow = ((CAN_STD_ID_P2P + (uint8_t)RGF_SYS_ADDR) << 21) & 0xFFFF;
        sFilterConfig.FilterMaskIdHigh = 0xFFFF;
        sFilterConfig.FilterMaskIdLow = 0xFFFF;
        sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO1;
        sFilterConfig.FilterActivation = ENABLE;
        if(HAL_CAN_ConfigFilter(&hcan, &sFilterConfig) != HAL_OK)
            return 3;
    }
    return 0;
}
uint8_t CAN_1_InitConfigStart(void){
    uint8_t err = 0;

    err = CAN_1_FilterConfig(0);
    if(err != 0) return err;

    if(HAL_CAN_ActivateNotification(&hcan, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK)
        err = 10;
    if(HAL_CAN_ActivateNotification(&hcan, CAN_IT_RX_FIFO1_MSG_PENDING) != HAL_OK)
        err = 11;

    if(HAL_CAN_Start(&hcan) != HAL_OK)
        err = 12;

    return err;
}
uint8_t CAN_1_AddTxMessage(uint32_t stdId, uint8_t* data, uint8_t len){
    CAN_TxHeaderTypeDef txHeader;
    uint32_t mailBox;

    txHeader.StdId = stdId;
    txHeader.IDE = CAN_ID_STD;
    txHeader.RTR = CAN_RTR_DATA;
    txHeader.DLC = len;
    txHeader.TransmitGlobalTime = DISABLE;

    HAL_CAN_AddTxMessage(&hcan, &txHeader, data, &mailBox);
    return 0;
}
struct CAN_ATY_Dev CAN_ATY_Dev_1 = {
    .addTxMessage = CAN_1_AddTxMessage,

    .addr = 0,
    .groupId = 0,

    .lock = __ATY_UNLOCKED
};
void CAN_1_UpdateFilter(uint8_t addr, uint8_t groupId){
    if(CAN_ATY_Dev_1.addr != addr){
        CAN_ATY_Dev_1.addr = addr;
        CAN_1_FilterConfig(0);
    }
    if(CAN_ATY_Dev_1.groupId != groupId){
        CAN_ATY_Dev_1.groupId = groupId;
        CAN_1_FilterConfig(CAN_FILTER_MULTICAST);
    }
}
void CAN_1_RxProcess(CAN_HandleTypeDef* hcan_t, uint32_t RxFifo){
    CAN_RxHeaderTypeDef rxHeader;
    uint8_t rxData[8] = {0};

    if(HAL_CAN_GetRxMessage(hcan_t, RxFifo, &rxHeader, rxData) != HAL_OK){
        return;
    }
    if(rxHeader.IDE != CAN_ID_STD || rxHeader.DLC < 2 || rxHeader.DLC < 6){
        return;
    }
    CAN_RX_Callback(&CAN_ATY_Dev_1, rxHeader.StdId, rgf, rxData);
}
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* hcan_t){
    if(hcan_t == &hcan){
        CAN_1_RxProcess(hcan_t, CAN_RX_FIFO0);
    }
}
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef* hcan_t){
    if(hcan_t == &hcan){
        CAN_1_RxProcess(hcan_t, CAN_RX_FIFO1);
    }
}

// ADDR_KEY --------------------------------------------------------------------
uint8_t KEY_Filter(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin){
    uint8_t repeat = 0;
    if(HAL_GPIO_ReadPin(GPIOx, GPIO_Pin) == GPIO_PIN_SET) repeat++;
    if(HAL_GPIO_ReadPin(GPIOx, GPIO_Pin) == GPIO_PIN_SET) repeat++;
    if(HAL_GPIO_ReadPin(GPIOx, GPIO_Pin) == GPIO_PIN_SET) repeat++;
    if(repeat >= 2) return 1;
    else return 0;
}
uint8_t ADDR_KEY_Scan(void){
    uint8_t ADDR_KEY = 0;
    if(KEY_Filter(SEL_A_GPIO_Port, SEL_A_Pin) == 0)
        ADDR_KEY |= 0x01;
    if(KEY_Filter(SEL_B_GPIO_Port, SEL_B_Pin) == 0)
        ADDR_KEY |= 0x02;
    if(KEY_Filter(SEL_C_GPIO_Port, SEL_C_Pin) == 0)
        ADDR_KEY |= 0x04;
    if(KEY_Filter(SEL_D_GPIO_Port, SEL_D_Pin) == 0)
        ADDR_KEY |= 0x08;
    return ADDR_KEY;
}

// AlgorithmBase ---------------------------------------------------------------
#include "ALGO_AlgorithmBase_ATY.h"

// ADC -------------------------------------------------------------------------
#include "HW_ADC_ATY.h"
void MCU_CoreAdcGet(void){
    // F405/F407(mV)
    // float VOL_PER_AD_ATY = 1210.0 / ((float)(*(__IO uint16_t*)(0x1FFF7A2A)));
    // float VREFINT_MCU_ATY = ADC_Get(&hadc1, ADC_CHANNEL_VREFINT) * VOL_PER_AD_ATY;
    // float TEMPERATURE_MCU_ATY =
    //     (((ADC_Get(&hadc1, ADC_CHANNEL_TEMPSENSOR) * VOL_PER_AD_ATY) - 760.0) / 2.5) + 25.0;
    // F103(mV)
    float VOL_PER_AD_ATY = 1200.0 / ADC_Get(&hadc1, ADC_CHANNEL_VREFINT);
    float VREFINT_MCU_ATY = ADC_Get(&hadc1, ADC_CHANNEL_VREFINT) * VOL_PER_AD_ATY;
    float TEMPERATURE_MCU_ATY = ((1430.0 -
        (ADC_Get(&hadc1, ADC_CHANNEL_TEMPSENSOR) * VOL_PER_AD_ATY)) / 4.3) + 25.0;

    float MCU_BASE_VOL_D = 1200;//VREFINT_MCU_ATY;
    float MCU_BASE_VOL = VREFINT_MCU_ATY;
    float MCU_TEMP = TEMPERATURE_MCU_ATY;
    printf_ATY_D("\r\nCAD: %f, CV: %f, CT: %f\r\n", VOL_PER_AD_ATY, MCU_BASE_VOL, MCU_TEMP);
    RGF_VOL_PER_AD = VOL_PER_AD_ATY;
    RGF_MCU_BASE_VOL = MCU_BASE_VOL;
    RGF_MCU_TEMP = MCU_TEMP;
}

// T ---------------------------------------------------------------------------
#include "ALGO_Temperature_ATY.h"
void BoardTempGet(void){
    float MCU_BASE_VOL_D = 1200;//VREFINT_MCU_ATY;
    float BOARD_TEMP = ALGO_VolToKelvinTemp((1000.0 *
        ADC_Get(&hadc1, ADC_CHANNEL_9) / MCU_BASE_VOL_D),
        3300.0, 10.0, 10.0, 3950, 1);
    printf_ATY_D("\r\nT: %f\r\n", BOARD_TEMP);
    RGF_BOARD_TEMP = BOARD_TEMP;
}

// FAN -------------------------------------------------------------------------
uint8_t fanLevel = 0;
uint8_t fanLevelLast = 0;
float currentPwm = 0;
#define FAN_PWM_FULL 10000
void FAN_ControlCycle(void){
    if((uint8_t)RGF_MOTOR_FAN_EN == 2){
        if(RGF_BOARD_TEMP > RGF_MOTOR_FAN_LOW_T){
            fanLevel = 0xFF;
        }
        else if(RGF_BOARD_TEMP < RGF_MOTOR_FAN_LOW_T - RGF_MOTOR_FAN_T_SPACE){
            fanLevel = 0;
        }
    }
    else if((uint8_t)RGF_MOTOR_FAN_EN == 10
        || ((uint8_t)RGF_MOTOR_FAN_EN == 1 && RGF_BOARD_TEMP >= (RGF_MOTOR_FAN_HIGH_T - RGF_MOTOR_FAN_T_SPACE))){
        fanLevel = 0xFF;
    }
    else if((uint8_t)RGF_MOTOR_FAN_EN == 1 && RGF_BOARD_TEMP > RGF_MOTOR_FAN_LOW_T){
        if((uint8_t)((RGF_BOARD_TEMP - RGF_MOTOR_FAN_LOW_T) / RGF_MOTOR_FAN_T_SPACE) + 1 != fanLevel){
            fanLevel = (uint8_t)(((RGF_BOARD_TEMP - RGF_MOTOR_FAN_LOW_T) / RGF_MOTOR_FAN_T_SPACE) + 1);
            currentPwm = (RGF_MOTOR_FAN_BASE / 100.0 * FAN_PWM_FULL)
                + ((FAN_PWM_FULL - (RGF_MOTOR_FAN_BASE / 100.0 * FAN_PWM_FULL)) * fanLevel
                    / ((RGF_MOTOR_FAN_HIGH_T - RGF_MOTOR_FAN_LOW_T) / RGF_MOTOR_FAN_T_SPACE));
            if(currentPwm >= FAN_PWM_FULL){
                currentPwm = FAN_PWM_FULL;
                fanLevel = ((RGF_MOTOR_FAN_HIGH_T - RGF_MOTOR_FAN_LOW_T) / RGF_MOTOR_FAN_T_SPACE) + 1;
            }
        }
    }
    else{
        fanLevel = 0;
    }

    if(fanLevel == fanLevelLast) return;
    fanLevelLast = fanLevel;
    if(fanLevel == 0xFF){
        HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_1);
        __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, FAN_PWM_FULL);
        HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
    }
    else if(fanLevel == 0){
        __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, 0);
        HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_1);
    }
    else{
        HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_1);
        __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, currentPwm);
        HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
    }
}
void FAN_Init(void){
    RGF_MOTOR_FAN_EN = 2;
    RGF_MOTOR_FAN_BASE = 0;
    RGF_MOTOR_FAN_LOW_T = 40;
    RGF_MOTOR_FAN_HIGH_T = 80;
    RGF_MOTOR_FAN_T_SPACE = 5;
}



// MT6816 ---------------------------------------------------------------------
#include "MT6816_ATY.h"
void MT6816_1_NSS_SET(uint8_t level){
    if(level == __ATY_HL_L)
        HAL_GPIO_WritePin(SPI1_NSS_GPIO_Port, SPI1_NSS_Pin, GPIO_PIN_RESET);
    else if(level == __ATY_HL_H)
        HAL_GPIO_WritePin(SPI1_NSS_GPIO_Port, SPI1_NSS_Pin, GPIO_PIN_SET);
}
uint8_t MT6816_1_SPI(uint8_t* data_t, uint16_t len, uint8_t rw){
    if(rw == __ATY_RW_R){
        return HAL_SPI_Receive(&hspi1, (uint8_t*)data_t, len, 1000);
    }
    else{
        return HAL_SPI_Transmit(&hspi1, (uint8_t*)data_t, len, 1000);
    }
}
struct MT6816_ATY_Dev MT6816_ATY_Dev_1 = {
    .nssSet = MT6816_1_NSS_SET,
    .spiProcess = MT6816_1_SPI,

    .angleValue = 0,
    .angle = 0,
    .angleLast = 0,
    .angleStart = 0,
    .angleTotal = 0,
    .turnCount = 0,
    .start = 0,

    .error = 0,
    .zeroPoint = 0,
    .angleReverseDir = 1,

    .lock = __ATY_UNLOCKED
};
void MT6816_1_Init(void){
    MT6816_ReadAngleFull(&MT6816_ATY_Dev_1);
    HAL_Delay(100);
    MT6816_ReadAngleFull(&MT6816_ATY_Dev_1);

    RGF_MOTOR_ANGLE_SET_ZERO = 0;
    RGF_MOTOR_ANGLE_ZERO = MT6816_ATY_Dev_1.zeroPoint;
    RGF_MOTOR_ANGLE = MT6816_ATY_Dev_1.angle;
    RGF_MOTOR_ANGLE_ERR = MT6816_ATY_Dev_1.error;

    RGF_MOTOR_ANGLE_ANGLE_START = MT6816_ATY_Dev_1.angleStart;
    RGF_MOTOR_ANGLE_TURN_COUNT = MT6816_ATY_Dev_1.turnCount;
    RGF_MOTOR_ANGLE_ANGLE_TOTAL = MT6816_ATY_Dev_1.angleTotal;
    RGF_MOTOR_ANGLE_START = MT6816_ATY_Dev_1.start;
}
void MT6816_1_UpdateCycle(void){
    MT6816_ReadAngleFull(&MT6816_ATY_Dev_1);
    MT6816_TurnCountOffset(&MT6816_ATY_Dev_1);
    if((uint8_t)RGF_MOTOR_ANGLE_SET_ZERO == 1){
        RGF_MOTOR_ANGLE_SET_ZERO = 0;
        MT6816_SetZeroPoint(&MT6816_ATY_Dev_1);
    }
    RGF_MOTOR_ANGLE_ZERO = MT6816_ATY_Dev_1.zeroPoint;
    RGF_MOTOR_ANGLE = MT6816_ATY_Dev_1.angle;
    RGF_MOTOR_ANGLE_ERR = MT6816_ATY_Dev_1.error;
    RGF_MOTOR_ANGLE_ANGLE_START = MT6816_ATY_Dev_1.angleStart;
    RGF_MOTOR_ANGLE_TURN_COUNT = MT6816_ATY_Dev_1.turnCount;
    RGF_MOTOR_ANGLE_ANGLE_TOTAL = MT6816_ATY_Dev_1.angleTotal;
    RGF_MOTOR_ANGLE_START = MT6816_ATY_Dev_1.start;
}

#define MS_1_TIM htim1
#define MS_1_TIM_CHANNEL TIM_CHANNEL_1
#define MS_1_TIM_ACTIVE_CHANNEL HAL_TIM_ACTIVE_CHANNEL_1
#define MS_1_TIM_IT_CC TIM_IT_CC1

// MSSTC -----------------------------------------------------------------------
#include "MOTOR_STEP_SPEED_T_C_ATY.h"
uint8_t MSSTC_1_EnSet(uint8_t en){
    HAL_GPIO_WritePin(MS_EN_GPIO_Port, MS_EN_Pin,
        (en == 0) ? GPIO_PIN_SET : GPIO_PIN_RESET);
    return 0;
}
uint8_t MSSTC_1_DirSet(uint8_t dir){
    HAL_GPIO_WritePin(MS_DIR_GPIO_Port, MS_DIR_Pin,
        (dir == __ATY_PN_P) ? GPIO_PIN_SET : GPIO_PIN_RESET);
    return 0;
}
void MSSTC_1_SetTimerCompare(uint32_t value){
    if(value > 65535) value = 65535;
    __HAL_TIM_SET_COMPARE(&MS_1_TIM, MS_1_TIM_CHANNEL, value);
    __HAL_TIM_SET_AUTORELOAD(&MS_1_TIM, value);
    __HAL_TIM_SET_COUNTER(&MS_1_TIM, 0);
}
void MSSTC_1_Start(void){
    __HAL_TIM_SET_COUNTER(&MS_1_TIM, 0);
    HAL_TIM_OC_Start_IT(&MS_1_TIM, MS_1_TIM_CHANNEL);
}
void MSSTC_1_Stop(void){
    HAL_TIM_OC_Stop_IT(&MS_1_TIM, MS_1_TIM_CHANNEL);
}
struct MOTOR_STEP_SPEED_T_C_ATY_Dev MSSTC_Dev_1 = {
    .enSet = MSSTC_1_EnSet,
    .dirSet = MSSTC_1_DirSet,
    .start = MSSTC_1_Start,
    .stop = MSSTC_1_Stop,
    .setTimerCompare = MSSTC_1_SetTimerCompare,

    .timerFreq = 500000,
    .fullSteps = 200,
    .microstepping = 8,

    .enable = 0,
    .direction = __ATY_PN_P,
    .fullSpeed = 20,
    .unitDivision = 1,
    .acceleration = 100,
    .deceleration = 200,

    .mode = MSSTC_MODE_DISTANCE_CLOSE,
    .angleLock = 0,
    .powerOnlyRun = 1,
    .turnBackAllowed = 1,
    .runState = MSSTC_STATE_STANDBY,

    .lock = __ATY_UNLOCKED
};
void HAL_TIM_OC_DelayElapsedCallback(TIM_HandleTypeDef* htim){
    if(htim->Instance == TIM1 && htim->Channel == MS_1_TIM_ACTIVE_CHANNEL){
        if(__HAL_TIM_GET_IT_SOURCE(&MS_1_TIM, MS_1_TIM_IT_CC) != RESET){
            __HAL_TIM_CLEAR_IT(&MS_1_TIM, MS_1_TIM_IT_CC);
            if((MSSTC_Dev_1.mode == MSSTC_MODE_SPEED_OPEN
                && MSSTC_Dev_1.runState == MSSTC_STATE_FULL)){
                __HAL_TIM_DISABLE_IT(&MS_1_TIM, MS_1_TIM_IT_CC);
            }
            else{
                MSSTC_IRQHandler(&MSSTC_Dev_1);
            }
        }
    }
}
void MSSTC_1_Init(void){
    MT6816_1_Init();
    HAL_GPIO_WritePin(MS_SP_GPIO_Port, MS_SP_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(MS_MS1_GPIO_Port, MS_MS1_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(MS_MS2_GPIO_Port, MS_MS2_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(MS_EN_GPIO_Port, MS_EN_Pin, GPIO_PIN_SET);

    RGF_MOTOR_FREQUENCY = (1600.0 / 60.0);
    RGF_MOTOR_DUTY_CYCLE = 50.0;

    RGF_MOTOR_FULL_STEPS = MSSTC_Dev_1.fullSteps;
    RGF_MOTOR_MICROSTEPPING = MSSTC_Dev_1.microstepping;

    RGF_MOTOR_ENABLE = MSSTC_Dev_1.enable;
    RGF_MOTOR_DIRECTION = MSSTC_Dev_1.direction;
    RGF_MOTOR_SPEED = MSSTC_Dev_1.fullSpeed;
    RGF_MOTOR_UNIT_DIVISION = MSSTC_Dev_1.unitDivision;
    RGF_MOTOR_ACCELERATION = MSSTC_Dev_1.acceleration;
    RGF_MOTOR_DECELERATION = MSSTC_Dev_1.deceleration;

    RGF_MOTOR_MODE = MSSTC_Dev_1.mode;
    RGF_MOTOR_ANGLE_LOCK_TARGET = MSSTC_Dev_1.angleLock;
    RGF_MOTOR_AUTO_POWER = MSSTC_Dev_1.powerOnlyRun;
    RGF_MOTOR_RUN_STATE = MSSTC_Dev_1.runState;
    RGF_MOTOR_RUN = 1;
    RGF_MOTOR_STEP_COUNT = 1600;
}
void MSSTC_1_Cycle(void){
    if(MSSTC_IsRunning(&MSSTC_Dev_1) != 1){
        MSSTC_Dev_1.fullSteps = (uint32_t)RGF_MOTOR_FULL_STEPS;
        if(MSSTC_Dev_1.microstepping != (uint32_t)RGF_MOTOR_MICROSTEPPING){
            MSSTC_Dev_1.microstepping = (uint32_t)RGF_MOTOR_MICROSTEPPING;
            MSSTC_Dev_1.enSet(0);
            if(MSSTC_Dev_1.microstepping == 8){
                HAL_GPIO_WritePin(MS_MS1_GPIO_Port, MS_MS1_Pin, GPIO_PIN_RESET);
                HAL_GPIO_WritePin(MS_MS2_GPIO_Port, MS_MS2_Pin, GPIO_PIN_RESET);
            }
            else if(MSSTC_Dev_1.microstepping == 16){
                HAL_GPIO_WritePin(MS_MS1_GPIO_Port, MS_MS1_Pin, GPIO_PIN_SET);
                HAL_GPIO_WritePin(MS_MS2_GPIO_Port, MS_MS2_Pin, GPIO_PIN_SET);
            }
            else if(MSSTC_Dev_1.microstepping == 32){
                HAL_GPIO_WritePin(MS_MS1_GPIO_Port, MS_MS1_Pin, GPIO_PIN_RESET);
                HAL_GPIO_WritePin(MS_MS2_GPIO_Port, MS_MS2_Pin, GPIO_PIN_SET);
            }
            else if(MSSTC_Dev_1.microstepping == 64){
                HAL_GPIO_WritePin(MS_MS1_GPIO_Port, MS_MS1_Pin, GPIO_PIN_SET);
                HAL_GPIO_WritePin(MS_MS2_GPIO_Port, MS_MS2_Pin, GPIO_PIN_RESET);
            }
            if(MSSTC_Dev_1.powerOnlyRun == 0){
                MSSTC_Dev_1.enSet(MSSTC_Dev_1.enable);
            }
        }

        if(MSSTC_Dev_1.enable != (uint8_t)RGF_MOTOR_ENABLE){
            MSSTC_Dev_1.enable = (uint8_t)RGF_MOTOR_ENABLE;
            if(MSSTC_Dev_1.powerOnlyRun == 0){
                MSSTC_Dev_1.enSet(MSSTC_Dev_1.enable);
            }
        }
        if(MSSTC_Dev_1.direction != (uint8_t)RGF_MOTOR_DIRECTION){
            MSSTC_Dev_1.direction = (uint8_t)RGF_MOTOR_DIRECTION;
            MSSTC_Dev_1.dirSet(MSSTC_Dev_1.direction);
            MT6816_TurnCountOffsetStart(&MT6816_ATY_Dev_1, MSSTC_Dev_1.direction);
            MT6816_TurnCountOffsetStop(&MT6816_ATY_Dev_1);
        }
        MSSTC_Dev_1.fullSpeed = (uint32_t)RGF_MOTOR_SPEED;
        MSSTC_Dev_1.unitDivision = (uint32_t)RGF_MOTOR_UNIT_DIVISION;
        MSSTC_Dev_1.acceleration = (uint32_t)RGF_MOTOR_ACCELERATION;
        MSSTC_Dev_1.deceleration = (uint32_t)RGF_MOTOR_DECELERATION;

        MSSTC_Dev_1.mode = (uint8_t)RGF_MOTOR_MODE;
        if(MSSTC_Dev_1.angleLock != (uint16_t)RGF_MOTOR_ANGLE_LOCK_TARGET){
            MSSTC_Dev_1.angleLock = (uint16_t)RGF_MOTOR_ANGLE_LOCK_TARGET;
            if(MSSTC_Dev_1.mode == 1){

            }
        }
        if(MSSTC_Dev_1.powerOnlyRun != (uint8_t)RGF_MOTOR_AUTO_POWER){
            MSSTC_Dev_1.powerOnlyRun = (uint8_t)RGF_MOTOR_AUTO_POWER;
            if(MSSTC_Dev_1.powerOnlyRun == 0){
                MSSTC_Dev_1.enSet(MSSTC_Dev_1.enable);
            }
            else{
                MSSTC_Dev_1.enSet(0);
            }
        }
    }

    MT6816_1_UpdateCycle();
    MSSTC_UpdateAngle(&MSSTC_Dev_1,
        MT6816_ATY_Dev_1.turnCount,
        MT6816_ATY_Dev_1.angleTotal + MT6816_ATY_Dev_1.angleStart);
    if(MSSTC_Dev_1.mode == MSSTC_MODE_SPEED_OPEN
        && MSSTC_Dev_1.fullSpeed != (uint32_t)RGF_MOTOR_SPEED){
        MSSTC_Dev_1.fullSpeed = (uint32_t)RGF_MOTOR_SPEED;
        MSSTC_UpdateFullSpeed(&MSSTC_Dev_1);
    }

    if((uint8_t)RGF_MOTOR_RUN == 1){
        MT6816_TurnCountOffsetStart(&MT6816_ATY_Dev_1, MSSTC_Dev_1.direction);
        MSSTC_Move(&MSSTC_Dev_1, RGF_MOTOR_STEP_COUNT, MT6816_ATY_Dev_1.angleStart);
    }
    else if((uint8_t)RGF_MOTOR_RUN == 2){
        MSSTC_Scram(&MSSTC_Dev_1);
    }
    else if((uint8_t)RGF_MOTOR_RUN == 3){
        MSSTC_Stop(&MSSTC_Dev_1);
        __HAL_TIM_ENABLE_IT(&MS_1_TIM, MS_1_TIM_IT_CC);
    }
    RGF_MOTOR_RUN = 0;
    if(MSSTC_Dev_1.runState == MSSTC_STATE_STANDBY
        && MT6816_ATY_Dev_1.start == 1){
        MT6816_TurnCountOffsetStop(&MT6816_ATY_Dev_1);
    }


    RGF_MOTOR_ENABLE = MSSTC_Dev_1.enable;
    RGF_MOTOR_DIRECTION = MSSTC_Dev_1.direction;

    RGF_MOTOR_RUN_STATE = MSSTC_Dev_1.runState;

    RGF_MOTOR_CURRENT_STEPS = MSSTC_Dev_1.stepCountCurrent;
    RGF_MOTOR_ACCEL_STEPS = MSSTC_Dev_1.accelSteps;
    RGF_MOTOR_ACCEL_LIMIT = MSSTC_Dev_1.accelLimit;
    RGF_MOTOR_DECEL_STEPS = MSSTC_Dev_1.decelSteps;
    RGF_MOTOR_DECEL_START = MSSTC_Dev_1.decelStart;
}




void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef* htim){
    if(htim == &htim4){
        UserTimerLoop_Cycle1ms(&HW_TIMER_ATY_Dev_4);
    }
    // if((uint8_t)RGF_MOTOR_MODE == 0 && htim == &MS_1_COUNT_TIM){
    //     HAL_TIM_PWM_Stop(&MS_1_TIM, MS_1_TIM_CHANNEL);
    //     HAL_TIM_Base_Stop_IT(&MS_1_COUNT_TIM);
    //     RGF_MOTOR_STEP_COUNT = 0;
    //     MOTOR_STEP_O_ATY_Dev_1.stepCount = 0;
    // }
}

// user ------------------------------------------------------------------------
uint8_t CNA_T_regAddr = 16;
uint8_t CNA_T_regEnable = 0;


uint8_t 


/* ATY Machine ****************************************************************/
void TimerLoopProcess_User_4(void){
    if(HW_TIMER_ATY_Dev_4.ms1_f == 1){
        HW_TIMER_ATY_Dev_4.ms1_f = 0;
    }
    if(HW_TIMER_ATY_Dev_4.ms100_f == 1){
        HW_TIMER_ATY_Dev_4.ms100_f = 0;
        SysLedBlink(&LED_ATY_t_1);

        UpdateMbRegsFromFloat(rgf, &MODBUS_S_LOW_ATY_Dev_1);
        CAN_1_UpdateFilter((uint8_t)RGF_SYS_ADDR, (uint8_t)RGF_SYS_GROUP_ID);

        MSSTC_1_Cycle();

        if(CNA_T_regEnable == 1){
            CNA_T_regEnable = 0;
            CAN_RegRW(&CAN_ATY_Dev_1, CAN_STD_ID_P2P + 8, CAN_CMD_WRITE, CNA_T_regAddr, rgf[CNA_T_regAddr]);
        }
    }
    if(HW_TIMER_ATY_Dev_4.s1_f == 1){
        HW_TIMER_ATY_Dev_4.s1_f = 0;

        MCU_CoreAdcGet();
        BoardTempGet();
        RGF_SYS_TIME = HW_TIMER_ATY_Dev_4.msN / 1000;
        FAN_ControlCycle();

        // CAN_RegRW(&CAN_ATY_Dev_1, CAN_STD_ID_P2P + 8, CAN_CMD_WRITE, 16, 1);
        uint8_t send[8] = {0x01, 0x03, 0x20, 0x1A, 0x00, 0x02, 0xEE, 0x0C};
        MODBUS1_UART(send, 8);
    }
}

void UartReceiveProcess_User_1(void){
    if(HW_UART_ATY_Dev_1.rx[0] == 'O'
        && HW_UART_ATY_Dev_1.rx[1] == 'K'
        && HW_UART_ATY_Dev_1.rx[2] == '?')
        printf_ATY("OK!");

    if(HW_UART_ATY_Dev_1.rxCount != 0){
        Modbus_Process(HW_UART_ATY_Dev_1.rx, HW_UART_ATY_Dev_1.rxCount, &MODBUS_S_LOW_ATY_Dev_1);

        if(MODBUS_S_LOW_ATY_Dev_1.setFlag == 1){
            MODBUS_S_LOW_ATY_Dev_1.setFlag = 0;
            TransMbRegsToFloat(rgf, &MODBUS_S_LOW_ATY_Dev_1);
        }
    }
}

#include "iwdg.h"
void MainInit(void){
    // MX_IWDG_Init();
    LED_ATY_t_1.ledBlinkType = 0x02;
    // addr
    RGF_SYS_ADDR = (float)ADDR_KEY_Scan();
    RGF_SYS_GROUP_ID = 0;
    // mb
    MODBUS_S_LOW_ATY_Dev_1.addr = (uint8_t)RGF_SYS_ADDR;
    // can
    CAN_ATY_Dev_1.addr = (uint8_t)RGF_SYS_ADDR;
    CAN_ATY_Dev_1.groupId = (uint8_t)RGF_SYS_GROUP_ID;
    CAN_1_InitConfigStart();
    // fan
    FAN_Init();
    // MSSTC
    MSSTC_1_Init();

    printf_ATY("\r\nHello word!\r\n");

    LED_ATY_t_1.ledBlinkType = 0x20;
}

/* USER CODE END 0 */

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

  /* USER CODE BEGIN 1 */

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

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_TIM4_Init();
  MX_SPI1_Init();
  MX_TIM2_Init();
  MX_TIM1_Init();
  MX_USART2_UART_Init();
  MX_ADC1_Init();
  MX_CAN_Init();
  MX_TIM3_Init();
  MX_RTC_Init();
  /* USER CODE BEGIN 2 */
    MainInit();

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
    while(1){
        UartReceiveProcess(&HW_UART_ATY_Dev_1);
        UserTimerLoopProcess(&HW_TIMER_ATY_Dev_4);
    /* 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};
  RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_LSI|RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.LSIState = RCC_LSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  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_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
  PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_RTC|RCC_PERIPHCLK_ADC;
  PeriphClkInit.RTCClockSelection = RCC_RTCCLKSOURCE_LSI;
  PeriphClkInit.AdcClockSelection = RCC_ADCPCLK2_DIV6;
  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */

/* 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 */
  /* User can add his own implementation to report the HAL error return state */
    __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 */