ApiValveLock/Core/Src/freertos.c

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @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 "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "adc.h"
#include "dma.h"
#include "i2c.h"
#include "tim.h"
#include "queue.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
typedef StaticQueue_t osStaticMessageQDef_t;
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
typedef enum
{
	Close_output,
	Close_input,
	Open1_input,
	Open2_input,
	default_Hall
} hallADC;
hallADC Hallmode;

DeviceParams_t g_deviceParams;
/* USER CODE END PD */

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

//ADC变量定义
uint16_t AD_Value[5] = {0};
uint16_t AD1, AD2, AD3, AD4, AD5 = 0;
int i,adc;
float AD1_Vol,AD2_Vol,AD3_Vol,AD4_Vol,AD5_Vol,AD5_I;
extern uint16_t Valve_status;		//	阀状态0000关0001开
float pid_output = 0.0f;
extern uint16_t Running_time;			//	运行时长
extern uint32_t Motor_speed;			//  电机转速
extern uint32_t Motor_current;		//  电机电流
extern uint32_t Magnetic_angle;		//	磁角度
extern uint32_t hall_data[4];			//	霍尔1到4的数据
extern uint8_t dataReceive485[BUFFER_SIZE485];	// 485串口接收缓冲区
extern uint16_t uartIRQ_rx_len ;	// 485串口接收数据长度
float Motor_cur = 0.0f;		//  电机电流
float Motor_spe = 0.0f;		//  电机转速
extern float delta_time; 	//	读取角度间隔时间（毫秒）
extern int32_t freq; // 定时器1频率


uint32_t period_buffer[CAPTURE_DEPTH];    // PWM周期
uint32_t high_buffer[CAPTURE_DEPTH];      // PWM高电平时间
sysmode Systemmode;
/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */

/* USER CODE END Variables */
/* Definitions for HighestTask */
osThreadId_t HighestTaskHandle;
const osThreadAttr_t HighestTask_attributes = {
  .name = "HighestTask",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityRealtime,
};
/* Definitions for ReadTask */
osThreadId_t ReadTaskHandle;
const osThreadAttr_t ReadTask_attributes = {
  .name = "ReadTask",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for DealTask */
osThreadId_t DealTaskHandle;
const osThreadAttr_t DealTask_attributes = {
  .name = "DealTask",
  .stack_size = 512 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for CommonTask */
osThreadId_t CommonTaskHandle;
const osThreadAttr_t CommonTask_attributes = {
  .name = "CommonTask",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityBelowNormal,
};
/* Definitions for MotorTask */
osThreadId_t MotorTaskHandle;
const osThreadAttr_t MotorTask_attributes = {
  .name = "MotorTask",
  .stack_size = 256 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for uart485rxqueue */
osMessageQueueId_t uart485rxqueueHandle;
uint8_t uart485queueBuffer[ 16 * sizeof( Uart485Rx_msg ) ];
osStaticMessageQDef_t uart485queueControlBlock;
const osMessageQueueAttr_t uart485rxqueue_attributes = {
  .name = "uart485rxqueue",
  .cb_mem = &uart485queueControlBlock,
  .cb_size = sizeof(uart485queueControlBlock),
  .mq_mem = &uart485queueBuffer,
  .mq_size = sizeof(uart485queueBuffer)
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
//void uart_enable(void);
//void Restart_UART1_DMA(void);
float Read_MT6813_Angle(void);
void pack_hall_data(void) ;
void pack_Magnetic_angle(void);
void pack_Motor_current(void); //  电机电流
void pack_Motor_speed(void); //  电机转速
/* USER CODE END FunctionPrototypes */

void HighestLEDTask(void *argument);
void ReaddataTask(void *argument);
void DealcallbackTask(void *argument);
void CommonPrintTask(void *argument);
void MotorControlTask(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */
	printf("API2_ELE 0612\r\n");
	Systemmode = Debugmode; // 调试模式
	
	load_params_from_flash(); // 从内部flash读取传感器参数
  Set_485_Baudrate(g_deviceParams.Serial_baud_rate);
	Restart_UART1_DMA();
	
  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* Create the queue(s) */
  /* creation of uart485rxqueue */
  uart485rxqueueHandle = osMessageQueueNew (16, sizeof(Uart485Rx_msg), &uart485rxqueue_attributes);

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* creation of HighestTask */
  HighestTaskHandle = osThreadNew(HighestLEDTask, NULL, &HighestTask_attributes);

  /* creation of ReadTask */
  ReadTaskHandle = osThreadNew(ReaddataTask, NULL, &ReadTask_attributes);

  /* creation of DealTask */
  DealTaskHandle = osThreadNew(DealcallbackTask, NULL, &DealTask_attributes);

  /* creation of CommonTask */
  CommonTaskHandle = osThreadNew(CommonPrintTask, NULL, &CommonTask_attributes);

  /* creation of MotorTask */
  MotorTaskHandle = osThreadNew(MotorControlTask, NULL, &MotorTask_attributes);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_EVENTS */
  /* add events, ... */
  /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_HighestLEDTask */
/**
  * @brief  Function implementing the HighestTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_HighestLEDTask */
void HighestLEDTask(void *argument)
{
  /* USER CODE BEGIN HighestLEDTask */
  /* Infinite loop */
	LEDStatus_t current_status;
	uint8_t last_status = 0;
	uint32_t runtimecount = 0;
  for(;;)
  {
		
    if(Hallmode == Close_output)
      current_status = LED_WARNING_1;
    else if(Hallmode == Close_input)
      current_status = LED_WARNING_2;
    else if(Hallmode == Open1_input)
      current_status = LED_WARNING_3;
    else if(Hallmode == Open2_input)
      current_status = LED_WARNING_4;			
    else
      current_status = LED_NORMAL;

		if(Motormode == MOTOR_OVERCURRENT)
      current_status = LED_WARNING_5;
		
		if(Systemmode == IAPbootloader)
      current_status = LED_IAP;
		
    // 只有当状态发生变化时才调用
    if (current_status != last_status)
    {
      last_status = current_status;
      UpdateLEDStatus(current_status);
    }

    LED_FlashHandler(&led_green);
    LED_FlashHandler(&led_red);
		if(runtimecount > 100)
		{
			runtimecount = 0;
			Running_time ++ ;
			if(Running_time > 65535)
				Running_time = 0;				
		}
		runtimecount++;
    osDelay(10);
  }
  /* USER CODE END HighestLEDTask */
}

/* USER CODE BEGIN Header_ReaddataTask */
/**
* @brief Function implementing the ReadTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_ReaddataTask */
void ReaddataTask(void *argument)
{
  /* USER CODE BEGIN ReaddataTask */
  /* Infinite loop */
	/*角度PWM输入捕获*/
//	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
//	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);		
//	HAL_TIM_IC_Start(&htim1, TIM_CHANNEL_1);
//	HAL_TIM_IC_Start(&htim1, TIM_CHANNEL_2);
	HAL_TIM_IC_Start_DMA(&htim1, TIM_CHANNEL_1, period_buffer, CAPTURE_DEPTH);
	HAL_TIM_IC_Start_DMA(&htim1, TIM_CHANNEL_2, high_buffer, CAPTURE_DEPTH);
	/*ADC多通道读取*/
	ADC_Enable (&hadc1);
	HAL_ADCEx_Calibration_Start(&hadc1, ADC_SINGLE_ENDED); // 启动ADC校准（单端模式）消除ADC的偏移误差 ADC_DIFFERENTIAL_ENDED ADC_SINGLE_ENDED
	HAL_ADC_Start_DMA(&hadc1,(uint32_t *) AD_Value, 5); // 启动ADC并通过DMA搬运数据
	
  for(;;)
  {
//		Read_MT6813_Angle(); // PWM中断读取角度		
//		read_angle();	// 硬件iic读取角度		
		pack_Magnetic_angle(); // 组包到 Magnetic_angle 用于MODBUS发送
		pack_Motor_speed(); 	 // 组包到 Motor_speed 用于MODBUS发送
		
		AD1 = AD_Value[0];
		AD2 = AD_Value[1];
		AD3 = AD_Value[2];
		AD4 = AD_Value[3];
		AD5 = AD_Value[4];
		
		AD1_Vol = (float) AD1 / 4096 * (float) 3.3;
		AD2_Vol = (float) AD2 / 4096 * (float) 3.3;
		AD3_Vol = (float) AD3 / 4096 * (float) 3.3;
		AD4_Vol = (float) AD4 / 4096 * (float) 3.3;
		AD5_Vol = (float) AD5 / 4096 * (float) 3.3;
		
		if( AD5_Vol > 3.3f )
			AD5_Vol = 0.0f;
		AD5_I = AD5_Vol / 0.5f;
		Motor_cur = AD5_I;
		pack_hall_data(); 		// 组包到 hall_data 用于MODBUS发送
		pack_Motor_current(); // 组包到 Motor_current 用于MODBUS发送

		
		if(AD1_Vol > 1.6f && AD1_Vol < 2.0f && AD2_Vol > 2.5f && AD4_Vol > 1.1f && AD4_Vol < 1.5f)
		{
			// 拉杆关 电机伸出到位
			Hallmode = Close_output;
			Valve_status = 0;
		}
		else if(AD1_Vol > 1.6f && AD1_Vol < 2.0f && AD2_Vol > 2.5f && AD4_Vol > 1.5f && AD4_Vol < 2.0f)
		{
			// 拉杆关 电机缩回到位
			Hallmode = Close_input;
			Valve_status = 1;
		}
		else if(AD1_Vol < 1.0f && AD2_Vol > 1.0f && AD2_Vol < 1.4f  && AD4_Vol > 1.6f && AD4_Vol < 2.0f)
		{
			// 拉杆开 电机缩回到位
			Hallmode = Open1_input;
			Valve_status = 1;
		}
		else if(AD1_Vol > 1.2f && AD1_Vol < 1.6f && AD2_Vol > 0.9f && AD2_Vol < 1.3f && AD4_Vol > 1.6f && AD4_Vol < 2.0f)
		{
			// 拉杆开 电机缩回到位
			Hallmode = Open2_input;
			Valve_status = 1;
		}else{
			Hallmode = default_Hall;
			Valve_status = 1;
		}
		
    osDelay(5);
  }
  /* USER CODE END ReaddataTask */
}

/* USER CODE BEGIN Header_DealcallbackTask */
/**
* @brief Function implementing the DealTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_DealcallbackTask */
void DealcallbackTask(void *argument)
{
  /* USER CODE BEGIN DealcallbackTask */
  /* Infinite loop */
	
	
	
  for(;;)
  {
		Process_Uart485callback(); // 处理485指令应答
    osDelay(20);
  }
  /* USER CODE END DealcallbackTask */
}

/* USER CODE BEGIN Header_CommonPrintTask */
/**
* @brief Function implementing the CommonTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_CommonPrintTask */
void CommonPrintTask(void *argument)
{
  /* USER CODE BEGIN CommonPrintTask */
  /* Infinite loop */
  for(;;)
  {
		target_angle_deg = pidMotor1Speed.target_val;	
		diff_Angle = pidMotor1Speed.err;		
		
//		printf("target=[%.2f] cur=[%.2f] diff=[%.2f] pid_output=[%.2f] Magnet:[%s] Motor_spe[%.2f] delta_time[%.2f]\r\n", 
//						target_angle_deg, cur_angle_deg, diff_Angle ,pid_output ,no_mag_flag ? "NO" : "YES" ,Motor_spe ,delta_time);
		printf("cur_angle_deg=[%.2f] Motor_spe[%.2f] Motor_cur[%.3f]\r\n", cur_angle_deg,Motor_spe,Motor_cur);
		osDelay(200);
		
//		printf("AD1_Vol:[%.3f], AD2_Vol:[%.3f], AD3_Vol:[%.3f], AD4_Vol[%.3f], AD5_Vol[%.3f],  Motor_cur[%.3f]\r\n",
//						AD1_Vol, AD2_Vol, AD3_Vol, AD4_Vol, AD5_Vol, Motor_cur);
 

		if(Hallmode == Close_output)
		{
			printf("Close output \r\n"); // 拉杆关 电机伸出到位
		}
		if(Hallmode == Close_input)
		{
			printf("Close input \r\n"); // 拉杆关 电机收回到位
		}
		if(Hallmode == Open1_input)
		{
			printf("Open1 input \r\n"); // 拉杆开一档 电机收回到位
		}
		if(Hallmode == Open2_input)
		{
			printf("Open2 input \r\n"); // 拉杆开二挡 电机收回到位
		}
		if(Hallmode == default_Hall)
		{
			printf("default_Hall \r\n"); // 拉杆开 电机收回到位
		}

  }
  /* USER CODE END CommonPrintTask */
}

/* USER CODE BEGIN Header_MotorControlTask */
/**
* @brief Function implementing the MotorTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_MotorControlTask */
void MotorControlTask(void *argument)
{
  /* USER CODE BEGIN MotorControlTask */
  /* Infinite loop */
	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3);  // 启动电机 CH1 PWM（控制 IN1）
  HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4);  // 启动电机 CH2 PWM（控制 IN2）
	uint16_t overcurrent_timer_ms = 0;       // 记录过流持续时间
	PID_init();
	
  for(;;)
  {
		// 电机角度	cur_angle_deg（0到360度）
		// 电机转速 Motor_spe （-400到+400）
		// 电机电流 Motor_cur （正常是0.01，堵转最高是到0.035）
		// 目标速度 target_speed 
		
//		Motor_Angle_Current_PID_Control();			
		if(use_pid_control)
		{	
			if (Motor_cur > 0.035f )
			{
					if (overcurrent_timer_ms < 1500)
					{
							overcurrent_timer_ms += 10;  // 每10ms调用一次
					}
					else
					{
							// 过流超时，停转
							Motormode = MOTOR_OVERCURRENT;
							__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, 0);
							__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, 0);
					}
			}
			else
			{
					// 电流恢复正常，清零计时器
					overcurrent_timer_ms = 0;
			}
//				Motor_PID_Control();
//			Motor_Angle_Current_PID_Control();
			int16_t pwm_output = PID_Incremental_Calc(&pidMotor1Speed, pidMotor1Speed.target_speed, Motor_spe);

			// 起动门槛限制
			if (abs(pwm_output) < 350 && pidMotor1Speed.target_speed != 0)
					pwm_output = (pwm_output > 0) ? 500 : -500;

			if (Motormode == MOTOR_FORWARD)
					Motor_Forward(pwm_output);
			if (Motormode == MOTOR_REVERSE)
					Motor_Reverse(pwm_output);
			if (Motormode == MOTOR_STOPPED)
			{
				__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, 0);
				__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, 0);
			}
			
		}
		
//			float pwm_Speed = PID_Incremental_Calc(&pidMotor1Speed,1000,Motor_spe);
//			__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, pwm_Speed); 	//电机正转
			
		if(Hallmode != Close_output)
			__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, 0);
		
		if(Hallmode != Open1_input && Hallmode != Open2_input && Hallmode != Close_input)
      __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, 0); 
    osDelay(10);
  }
  /* USER CODE END MotorControlTask */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */


float Read_MT6813_Angle(void)
{
    uint32_t high_time = HAL_TIM_ReadCapturedValue(&htim1, TIM_CHANNEL_2); // 高电平宽度
    uint32_t period = HAL_TIM_ReadCapturedValue(&htim1, TIM_CHANNEL_1)+1;    // 周期

    if (period == 0) return 0.0f;

    float duty = (float)high_time / period;
    cur_angle_deg = duty * 360.0f;
		
    return angle;
}


void pack_hall_data(void)
{
    memcpy(&hall_data[0], &AD1_Vol, sizeof(float));  // float --> uint32_t（IEEE754位一致）
    memcpy(&hall_data[1], &AD2_Vol, sizeof(float));
    memcpy(&hall_data[2], &AD3_Vol, sizeof(float));
    memcpy(&hall_data[3], &AD4_Vol, sizeof(float));
}

void pack_Magnetic_angle(void)
{
    memcpy(&Magnetic_angle, &cur_angle_deg, sizeof(float));  // float --> uint32_t
}

void pack_Motor_speed(void) //  电机转速
{
    memcpy(&Motor_speed, &Motor_spe, sizeof(float));  // float --> uint32_t
}
void pack_Motor_current(void) //  电机电流
{
    memcpy(&Motor_current, &Motor_cur, sizeof(float));  // float --> uint32_t
}
	
void JumpToBootloader(void)
{
    uint32_t i;
    void (*SysMemBootJump)(void);
    uint32_t BootAddr = 0x1FFF0000;  // STM32L431 系统 Bootloader 起始地址

    /* 关闭全局中断 */
    __disable_irq();

    /* 关闭 SysTick */
    SysTick->CTRL = 0;
    SysTick->LOAD = 0;
    SysTick->VAL  = 0;

    /* 复位 RCC 时钟设置 */
    HAL_RCC_DeInit();

    /* 禁用所有 NVIC 中断 */
    for (i = 0; i < 8; i++) {
        NVIC->ICER[i] = 0xFFFFFFFF;
        NVIC->ICPR[i] = 0xFFFFFFFF;
    }

    /* 重新允许中断（尽管之后马上跳转） */
    __enable_irq();

    /* 重新映射系统内存到 0x00000000 */
    __HAL_SYSCFG_REMAPMEMORY_SYSTEMFLASH();

    /* 设置 MSP（Main Stack Pointer） */
    __set_MSP(*((uint32_t *)BootAddr));

    /* 设置 CONTROL 寄存器，切换为特权级、使用 MSP */
    __set_CONTROL(0);

    /* 获取 Bootloader 的复位向量地址（入口地址） */
    SysMemBootJump = (void (*)(void)) (*((uint32_t *)(BootAddr + 4)));

    /* 跳转到系统 Bootloader */
    SysMemBootJump();

    /* 此处不应再返回 */
    while (1);
}

/* USER CODE END Application */