/* 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 "dma.h"
#include "spi.h"
#include "queue.h"
#include "kv_flash.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 */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
bool DB_init_flag = false; // 数据库初始化标志位
	
extern uint8_t dataReceive485[BUFFER_SIZE485];	// 485串口接收缓冲区
extern uint8_t dataReceivep[BUFFER_SIZEP]; // 调试串口接收buffer
extern uint32_t uartIRQ_rx_len ;	// 485串口接收数据长度

sysmode Systemmode;

extern bool erase_APP2;
extern bool erase_APPdata_buffer;
extern bool save_JsRoot_0100_flag;
extern uint32_t expected_file_size;	// 升级文件大小值
extern uint16_t upgrade_mode_subcode; // 记录使用的subcode（0x0100 或 0x0200）

/*控制台打印信息控制符*/
volatile uint8_t debug_print_enabled = 0;  // 默认关闭调试打印
volatile uint8_t fdc_print_enabled = 0;  // 默认关闭电容打印
volatile uint8_t ads_print_enabled = 0;  // 默认关闭ADS打印
volatile uint8_t kv_print_enabled = 0;  // 默认关闭KV数据库打印

/*阀杆相关参数定义*/
volatile float ADS_v0 = 0.0f; // ads1256 通道0原始电压值
volatile float ADS_v0_offset = 0.008f; // ads1256 通道0电压偏移 0.008
volatile float filtered_ADS_v0 = 0.0f; // 滤波之后的阀杆压力值（kpa）
volatile float MPXV_P0 = 0.0f; // 电压转换后的阀杆压力值（kpa）
volatile float Value_open_mm = 0.0f; // 阀杆行程毫米
volatile float Value_open_percent = 0.0f; // 阀杆开行程百分比
//volatile float // 阀杆压力值滤波滑动窗口大小

/*油水相关参数定义*/ // 多线程环境修改的共享变量
volatile float oil_para_indu = 18.0f; // 并联电感值
volatile float oil_para_cap = 33.0f; // 并联电容值
volatile float oil_cap = 0.0f;// 软件IIC 1号（PB10PB11）测得电容值
volatile float oil_cap_offset = 0.0f;// 油水电容偏移值 43.1f
volatile float filtered_oil_cap = 0.0f; // 滤波之后的油水电容值
volatile float oil_percent = 0.0f; // 油水百分比
//volatile float // 油水电容值滤波滑动窗口大小

/*余油相关参数定义*/
volatile float res_oil_para_indu = 18.0f; // 并联电感值
volatile float res_oil_para_cap = 33.0f; // 并联电容值
volatile float res_oil_cap = 0.0f; // // 软件IIC 2号（PB10PB11）测得电容值
volatile float res_oil_cap_offset = 0.0f;// 余油电容偏移值  43.1f
volatile float filtered_res_oil_cap = 0.0f; // 滤波之后的电容值

volatile float ADS_v1 = 0.0f; // ads1256 通道1原始电压值
volatile float ADS_v1_offset = 0.008f; // ads1256 通道1电压偏移值
volatile float filtered_ADS_v1 = 0.0f; // 滤波之后的余油电压值
volatile float MPXV_P1 = 0.0f; // 余油压力值（kpa）
volatile float res_oil_height = 0.0f; // 余油高度值（mm）
//volatile float // 余油压力值滤波滑动窗口大小

/*温度相关参数定义*/
volatile float ADS_v2 = 0.0f; // ads1256 通道2原始电压值
volatile float ADS_v2_offset = 0.0f; // ads1256 通道2电压偏移值
volatile float filtered_ADS_v2 = 0.0f; // 滤波之后的电压值
volatile float NTC_temp = 0.0f; // 电压查RT表转换后的温度值（℃）
//volatile float // 温度滤波滑动窗口大小

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
extern IWDG_HandleTypeDef hiwdg;
/* USER CODE END Variables */
/* Definitions for HighestTask */
osThreadId_t HighestTaskHandle;
const osThreadAttr_t HighestTask_attributes = {
  .name = "HighestTask",
  .stack_size = 256 * 4,
  .priority = (osPriority_t) osPriorityRealtime,
};
/* Definitions for ReadTask */
osThreadId_t ReadTaskHandle;
const osThreadAttr_t ReadTask_attributes = {
  .name = "ReadTask",
  .stack_size = 256 * 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 = 512 * 4,
  .priority = (osPriority_t) osPriorityBelowNormal,
};
/* Definitions for ReadTask02 */
osThreadId_t ReadTask02Handle;
const osThreadAttr_t ReadTask02_attributes = {
  .name = "ReadTask02",
  .stack_size = 256 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for FilterTask */
osThreadId_t FilterTaskHandle;
const osThreadAttr_t FilterTask_attributes = {
  .name = "FilterTask",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityAboveNormal,
};
/* Definitions for uart485rxqueue */
osMessageQueueId_t uart485rxqueueHandle;
uint8_t uart485queueBuffer[ 20 * 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)
};
/* Definitions for printqueue */
osMessageQueueId_t printqueueHandle;
uint8_t printqueueBuffer[ 32 * sizeof( Print_msg ) ];
osStaticMessageQDef_t printqueueControlBlock;
const osMessageQueueAttr_t printqueue_attributes = {
  .name = "printqueue",
  .cb_mem = &printqueueControlBlock,
  .cb_size = sizeof(printqueueControlBlock),
  .mq_mem = &printqueueBuffer,
  .mq_size = sizeof(printqueueBuffer)
};
/* Definitions for consolemsgqueue */
osMessageQueueId_t consolemsgqueueHandle;
uint8_t consolemsgqueueBuffer[ 32 * sizeof( Console_msg ) ];
osStaticMessageQDef_t consolemsgqueueControlBlock;
const osMessageQueueAttr_t consolemsgqueue_attributes = {
  .name = "consolemsgqueue",
  .cb_mem = &consolemsgqueueControlBlock,
  .cb_size = sizeof(consolemsgqueueControlBlock),
  .mq_mem = &consolemsgqueueBuffer,
  .mq_size = sizeof(consolemsgqueueBuffer)
};
/* Definitions for filterqueue3 */
osMessageQueueId_t filterqueue3Handle;
uint8_t filterqueue3Buffer[ 30 * sizeof( Filter_msg3 ) ];
osStaticMessageQDef_t filterqueue3ControlBlock;
const osMessageQueueAttr_t filterqueue3_attributes = {
  .name = "filterqueue3",
  .cb_mem = &filterqueue3ControlBlock,
  .cb_size = sizeof(filterqueue3ControlBlock),
  .mq_mem = &filterqueue3Buffer,
  .mq_size = sizeof(filterqueue3Buffer)
};
/* Definitions for filterqueue2 */
osMessageQueueId_t filterqueue2Handle;
uint8_t filterqueue2Buffer[ 30 * sizeof( Filter_msg2 ) ];
osStaticMessageQDef_t filterqueue2ControlBlock;
const osMessageQueueAttr_t filterqueue2_attributes = {
  .name = "filterqueue2",
  .cb_mem = &filterqueue2ControlBlock,
  .cb_size = sizeof(filterqueue2ControlBlock),
  .mq_mem = &filterqueue2Buffer,
  .mq_size = sizeof(filterqueue2Buffer)
};
/* Definitions for filterqueue1 */
osMessageQueueId_t filterqueue1Handle;
uint8_t filterqueue1Buffer[ 30 * sizeof( Filter_msg1 ) ];
osStaticMessageQDef_t filterqueue1ControlBlock;
const osMessageQueueAttr_t filterqueue1_attributes = {
  .name = "filterqueue1",
  .cb_mem = &filterqueue1ControlBlock,
  .cb_size = sizeof(filterqueue1ControlBlock),
  .mq_mem = &filterqueue1Buffer,
  .mq_size = sizeof(filterqueue1Buffer)
};
/* Definitions for filterqueue4 */
osMessageQueueId_t filterqueue4Handle;
uint8_t filterqueue4Buffer[ 30 * sizeof( Filter_msg4 ) ];
osStaticMessageQDef_t filterqueue4ControlBlock;
const osMessageQueueAttr_t filterqueue4_attributes = {
  .name = "filterqueue4",
  .cb_mem = &filterqueue4ControlBlock,
  .cb_size = sizeof(filterqueue4ControlBlock),
  .mq_mem = &filterqueue4Buffer,
  .mq_size = sizeof(filterqueue4Buffer)
};
/* Definitions for filterqueue5 */
osMessageQueueId_t filterqueue5Handle;
uint8_t filterqueue5Buffer[ 30 * sizeof( Filter_msg5 ) ];
osStaticMessageQDef_t filterqueue5ControlBlock;
const osMessageQueueAttr_t filterqueue5_attributes = {
  .name = "filterqueue5",
  .cb_mem = &filterqueue5ControlBlock,
  .cb_size = sizeof(filterqueue5ControlBlock),
  .mq_mem = &filterqueue5Buffer,
  .mq_size = sizeof(filterqueue5Buffer)
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
void pack_filtered_cap(void); //  float filtered_cap --> uint32_t uint_filtered_cap
void UART1_Watchdog_Check(void);
osStatus_t QueuePutOverwrite(osMessageQueueId_t queue_id, const void *msg_ptr, uint8_t priority, uint32_t timeout);
void ProcessFDC2214( uint8_t* init_ready_flag, uint8_t* valid_flag, uint8_t channel, uint8_t inductor, uint8_t par_capacitor, float zeroValue, uint8_t device_index); 
void CheckFDC2214_ID(uint8_t device_index, uint8_t* init_ready_flag, uint8_t* deviceValid);
void TProcessFDC2214( uint8_t* init_ready_flag, uint8_t* valid_flag, uint8_t channel, uint8_t inductor, uint8_t par_capacitor, float zeroValue, uint8_t device_index); 
void TCheckFDC2214_ID(uint8_t device_index, uint8_t* init_ready_flag, uint8_t* deviceValid);
/* USER CODE END FunctionPrototypes */

void HighestLEDTask(void *argument);
void ReaddataTask(void *argument);
void DealcallbackTask(void *argument);
void CommonPrintTask(void *argument);
void ReaddataTask02(void *argument);
void FilterdataTask(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 */
	Systemmode = Debugmode;  // Debugmode    releasemode // 发布模式  IAPbootloader // 升级模式
	
	Set_485_Baudrate(JsRoot.baudrate);
	Restart_UART1_DMA();
	Restart_UART2_DMA();
	JsRoot.work = &JsWork;
//	JsRoot.addr = 0xFF;
	printf ("\r\n API_MUL app1 2025_08_20 \r\n");
	
  /* 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 (20, sizeof(Uart485Rx_msg), &uart485rxqueue_attributes);

  /* creation of printqueue */
  printqueueHandle = osMessageQueueNew (32, sizeof(Print_msg), &printqueue_attributes);

  /* creation of consolemsgqueue */
  consolemsgqueueHandle = osMessageQueueNew (32, sizeof(Console_msg), &consolemsgqueue_attributes);

  /* creation of filterqueue3 */
  filterqueue3Handle = osMessageQueueNew (30, sizeof(Filter_msg3), &filterqueue3_attributes);

  /* creation of filterqueue2 */
  filterqueue2Handle = osMessageQueueNew (30, sizeof(Filter_msg2), &filterqueue2_attributes);

  /* creation of filterqueue1 */
  filterqueue1Handle = osMessageQueueNew (30, sizeof(Filter_msg1), &filterqueue1_attributes);

  /* creation of filterqueue4 */
  filterqueue4Handle = osMessageQueueNew (30, sizeof(Filter_msg4), &filterqueue4_attributes);

  /* creation of filterqueue5 */
  filterqueue5Handle = osMessageQueueNew (30, sizeof(Filter_msg5), &filterqueue5_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 ReadTask02 */
  ReadTask02Handle = osThreadNew(ReaddataTask02, NULL, &ReadTask02_attributes);

  /* creation of FilterTask */
  FilterTaskHandle = osThreadNew(FilterdataTask, NULL, &FilterTask_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(;;)
  {
    HAL_IWDG_Refresh(&hiwdg);	
		
		if(Systemmode == releasemode)
			current_status = LED_COMM;
		if(Systemmode == Debugmode) // Debugmode
			current_status = LED_WARNING_2;
		if(Systemmode == IAPbootloader)
			current_status = LED_IAP;

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

    LED_FlashHandler(&led_red, &led_green);
		LED_FlashHandler(&led_green, &led_red);

		if(runtimecount > 100)
		{
			runtimecount = 0;
			JsRoot.age ++ ;
			if(JsRoot.age > 65535)
				JsRoot.age = 0;				
		}
		runtimecount++;

    osDelay(2);
  }
  /* 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 */
	/*----------------ADS1256读取电压值----------------*/
	Filter_msg3 msg3;
	Filter_msg4 msg4;
	Filter_msg5 msg5;
	static int dropped_cnt1,dropped_cnt2,dropped_cnt3 = 0; // 监测未消费数据丢弃计数器
	ADS1256_Init(&hspi1);
	static uint8_t ads_channel_index = 0;
	
  for(;;)
  {
    if (HAL_GPIO_ReadPin(ADS_DRDY_GPIO_Port, ADS_DRDY_Pin) == GPIO_PIN_RESET)
		{
				int32_t adc_value = ADS1256_ReadChannel(ads_channel_index);
//				int32_t adc_value = ADS1256_Readdata();
			// Vin ---- R1 ----（Vout）---- R2 ---- GND    R1 = 3.3kΩ  R2 = 2.2kΩ
			// Vout = Vin × 2.2k / (3.3k + 2.2k) = Vin × 2.2 / 5.5 = Vin × 0.4
				float voltage = ADS1256_ConvertToVoltage(adc_value, 2.5f, 1); // VREF=2.5V，PGA=1
				
				switch (ads_channel_index)
				{
										
						case 0:
								/*----------通道0用于阀杆压力-------------*/
								voltage = voltage / 0.4f;
								ADS_v0 = voltage + ADS_v0_offset; // ADS_v0_offset = 0.008f;						
								msg3.data = ADS_v0;
								ADS_PRINTF("ADS_v0 %.5f \r\n",ADS_v0);
								if (QueuePutOverwrite(filterqueue3Handle, &msg3, 0, 0) != osOK) { // 将数据放入队列
										dropped_cnt1++;
										ADS_PRINTF("\r\nQueue full, MPXV_P0=%.3f not sent. Dropped=%d", MPXV_P0, dropped_cnt1);
								}							
								break;

						case 1:
								/*----------通道1用于余油压力-------------*/
								voltage = voltage / 0.4f;
								ADS_v1 = voltage - ADS_v1_offset; // ADS_v1_offset = 0.008f;					
								msg4.data = ADS_v1;
								ADS_PRINTF("ADS_v1 %.5f \r\n",ADS_v1);
								if (QueuePutOverwrite(filterqueue4Handle, &msg4, 0, 0) != osOK) { // 将数据放入队列
										dropped_cnt2++;
										ADS_PRINTF("\r\nQueue full, MPXV_P1=%.3f not sent. Dropped=%d", MPXV_P1, dropped_cnt2);
								}
								break;

						case 2:
								/*----------通道2用于温度-------------*/
								ADS_v2 = voltage - ADS_v2_offset ; // ADS_v0_offset = 0.000f;
								msg5.data = ADS_v2;
								ADS_PRINTF("ADS_v2 %.5f \r\n",ADS_v2);
								if (QueuePutOverwrite(filterqueue5Handle, &msg5, 0, 0) != osOK) { // 将数据放入队列
										dropped_cnt3++;
										ADS_PRINTF("\r\nQueue full, ADS_v2=%.3f not sent. Dropped=%d", ADS_v2, dropped_cnt3);
								}					
								break;
				}

				// 切换到下一个通道
				ads_channel_index++;
				if (ads_channel_index > 2)
						ads_channel_index = 0;
		}
    osDelay(10);
  }
  /* 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_Consolecallback(); // 处理控制台命令
    Process_Uart485callback(); // 处理485指令应答
		osDelay(10);
		
		if(erase_APP2){
			Flash_ErasePages(APP2_ADDR, APP_SIZE / 4);  // 擦除整个 APP2 区域（80KB）擦除 40 页
			erase_APP2 = false;
		}
		if(erase_APPdata_buffer){ // 
			Flash_ErasePages(IAPDataBuffer_ADDR, APP_SIZE / 4);  // 擦除整个 IAPDataBuffer 区域（80KB）擦除 40 页
			erase_APPdata_buffer = false;
		}
		if(save_JsRoot_0100_flag){
			
			JsRoot.subcode = upgrade_mode_subcode;
			DB_SaveUInt(DB_ID_SNAPSHOT, KV_KEY_IAP_SUBCODE , JsRoot.subcode ); // 存入到SNAPSHOT数据库
			
			JsRoot.iapSize = expected_file_size; // 文件大小存到缓存区
			DB_SaveUInt(DB_ID_SNAPSHOT, KV_KEY_IAP_SIZE , JsRoot.iapSize ); // 存入到SNAPSHOT数据库
			
			save_JsRoot_0100_flag = false;
		}
		if(JsRoot.iapLoadStatus == 300 && DB_init_flag == true) // 已完成APP数据缓存区的下载，接下来开始复制到APP2区
		{
						
			Flash_ErasePages(APP2_ADDR, APP_SIZE / 4);  // 擦除整个 APP2 区域（80KB）擦除 40 页（2048字节/页）
			osDelay(100);
			Flash_Write64(APP2_ADDR, (uint64_t*)IAPDataBuffer_ADDR, APP_SIZE / 8); // 将APP缓存区的数据复制到APP2
			osDelay(100); 
			printf("\r\n Copy IAPDataBuffer to APP2");
			
			uint8_t APP2_md5_bytes[4]; // 将结构体里 char 型的MD5码转成 uint8_t 
			for (int i = 0; i < 4; i++) {
					sscanf(&JsRoot.iapMd5[i*2], "%2hhx", &APP2_md5_bytes[i]);
			}
			printf("\r\n Read FlashMd5 Complete");
			
			// 再验一遍MD5
			uint8_t cal_app2_md5[16];
			MD5_CTX context;
			MD5Init(&context);
			if( JsRoot.subcode == 0x0200 ){
				MD5Update(&context, (uint8_t*)APP2_ADDR, JsRoot.iapSize); // 0200 -0
			}else{
				MD5Update(&context, (uint8_t*)APP2_ADDR, JsRoot.iapSize - 16); // 0100 -16
			}
			MD5Final(cal_app2_md5, &context); // 计算APP2区的MD5码
			printf("\r\n Calculate Md5 Complete");		
			
			printf("APP2_md5_bytes : ");
			for(int i = 0; i < 4; i++) {
					printf("%02X ", APP2_md5_bytes[i]);
			}
			printf("\n");

			printf("cal_app2_md5 : ");
			for(int i = 0; i < 4; i++) {
					printf("%02X ", cal_app2_md5[i]);
			}
			printf("\n");

			// 比较md5结果
			if (memcmp(cal_app2_md5, APP2_md5_bytes, 4) == 0) //验证复制后的APP2的MD5码和APP缓存区存入到数据库的MD5是否一致
			{
				printf("\r\n Copy IAPDataBuffer to APP2 success ");
				
				JsRoot.iapLoadStatus = 2; // 代表程序在APP2区运行
				JsRoot.ubootback = 0;	// 跟随JsRoot.iapLoadStatus = 2 先置为0
				DB_SaveUInt(DB_ID_SNAPSHOT, KV_KEY_UBOOTBACK , JsRoot.ubootback ); // bootloader 状态码存入到SNAPSHOT数据库
				DB_SaveUInt(DB_ID_SNAPSHOT, KV_KEY_IAP_LOAD , JsRoot.iapLoadStatus ); // 装载状态存入到SNAPSHOT数据库
				osDelay(300);
				NVIC_SystemReset();
			}else{

				printf("\r\nCopy IAPDataBuffer to APP2 Failed!!! ");
				JsRoot.ubootback = 401; // 代表APP2区文件完整性校验错误
				JsRoot.iapLoadStatus = 0; // 代表程序在APP1区运行
				DB_SaveUInt(DB_ID_SNAPSHOT, KV_KEY_UBOOTBACK , JsRoot.ubootback ); // bootloader 状态码存入到SNAPSHOT数据库
				DB_SaveUInt(DB_ID_SNAPSHOT, KV_KEY_IAP_LOAD , JsRoot.iapLoadStatus ); // 装载状态存入到SNAPSHOT数据库
				send_upgrade_end_response(0x42, 0xAABB, 0x05, 0x0000); //MD5校验错误
			}
		}
		// 如果检测到APP2区文件完整性错误，再尝试3次将APP缓存区复制到APP2区
//		if(JsRoot.ubootback == 401)
//		{
//			// 复制flash
//		}
		check_upgrade_data4001_timeout();
  }
  /* 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 */
	Print_msg Printmsg;

  for(;;)
  {
		if(printqueueHandle == NULL){
			printf("Failed to create print queue!\n");			
			osDelay(1);
      continue;
		}
		osStatus_t status = osMessageQueueGet(printqueueHandle, &Printmsg, NULL, 0);
    if ( status== osOK)
		{
			printf("%s", Printmsg.msg);  
		}
		else if (status == osErrorParameter) {
            printf("[CRITICAL] Queue corruption detected! Reinit...\n");
            osMessageQueueDelete(printqueueHandle);
            printqueueHandle = osMessageQueueNew(32, sizeof(Print_msg), NULL);
    }
		else{
//			printf("[ERROR] Failed to read from print queue: %d\n", (int)status);
		}
		if(DB_init_flag == false)
		{
				DB_InitAllConfigs(); // 数据库初始化
				DB_init_flag= true;
			
		}

		osDelay(15);
  }
  /* USER CODE END CommonPrintTask */
}

/* USER CODE BEGIN Header_ReaddataTask02 */
/**
* @brief Function implementing the ReadTask02 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_ReaddataTask02 */
void ReaddataTask02(void *argument)
{
  /* USER CODE BEGIN ReaddataTask02 */
  /* Infinite loop */
	uint8_t fdc2214_initready1, fdc2214_initready2 = 0;
  uint8_t device1IDValid, device2IDValid = 0; // 设备ID是否有效的标志	
	uint32_t lastCheck1Time, lastCheck2Time = 0; // 记录上次检查时间
  const uint32_t checkInterval = 5000; // 5秒检查间隔（毫秒）

  for(;;)
  {
    uint32_t now = HAL_GetTick();
		
		// 检查设备1
    if ((now - lastCheck1Time) >= checkInterval)
    {
      lastCheck1Time = now;
      CheckFDC2214_ID(1, &fdc2214_initready1,&device1IDValid);
    }
		if (device1IDValid)
    {
			/*--------------1号电容用于测量油水电容--------------*/
			ProcessFDC2214(&fdc2214_initready1, &device1IDValid, 0, oil_para_indu, oil_para_cap, oil_cap_offset, 1);
		}
		// 检查设备2
		if ((now - lastCheck2Time) >= checkInterval)
    {
      lastCheck2Time = now;
      TCheckFDC2214_ID(2, &fdc2214_initready2, &device2IDValid);
    }
		if (device2IDValid)
    {
			/*--------------2号电容用于测量余油电容--------------*/
			TProcessFDC2214(&fdc2214_initready2, &device2IDValid, 0, res_oil_para_indu, res_oil_para_cap, res_oil_cap_offset, 2);
		}
		
    osDelay(10);
  }
  /* USER CODE END ReaddataTask02 */
}

/* USER CODE BEGIN Header_FilterdataTask */
/**
* @brief Function implementing the FilterTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_FilterdataTask */
void FilterdataTask(void *argument)
{
  /* USER CODE BEGIN FilterdataTask */
  /* Infinite loop */
	SlidingWindowFilter filter1, filter2, filter3, filter4, filter5;
	SlidingFilter_Init(&filter1); // 初始化滑动窗口
	SlidingFilter_Init(&filter2);
	SlidingFilter_Init(&filter3);
	SlidingFilter_Init(&filter4);
	SlidingFilter_Init(&filter5);
	Filter_msg1 msg1;
	Filter_msg2 msg2;
	Filter_msg3 msg3;
	Filter_msg4 msg4;
	Filter_msg5 msg5;
  for(;;)
  {
				
				if (filterqueue1Handle != NULL) {
					/*---------油水电容值滤波----------*/
					osStatus_t status = osMessageQueueGet(filterqueue1Handle, &msg1, 0, 0);
					if (status == osOK) {
							filtered_oil_cap = SlidingFilter_Update(&filter1, msg1.data);	// 滤波										
							oil_percent = Estimate_Percentage_piecewise(filtered_oil_cap);
//							CommonPrintf("filtered_oil_cap=[%.5f], oil_percent=[%.1f%%]\r\n", filtered_oil_cap, oil_percent);
					}
				}

				if (filterqueue2Handle != NULL) {
					/*---------余油电容值滤波----------*/
					osStatus_t status = osMessageQueueGet(filterqueue2Handle, &msg2, 0, 0);
					if (status == osOK) {
							filtered_res_oil_cap = SlidingFilter_Update(&filter2, msg2.data);	
//							CommonPrintf("filtered_res_oil_cap=[%.5f]\r\n", filtered_res_oil_cap);						
					}
				}
				if (filterqueue3Handle != NULL) {
					/*---------阀杆压力值滤波----------*/
					osStatus_t status = osMessageQueueGet(filterqueue3Handle, &msg3, 0, 0);
					if (status == osOK) {
						
							filtered_ADS_v0 = SlidingFilter_Update(&filter3, msg3.data);
							MPXV_P0 = ((filtered_ADS_v0 - 2.5f) / 2.0f) * 7.0f;  // kPa
//							CommonPrintf("P0 raw=%.3f, filtered=%.3f\r\n", msg3.data, filtered_MPXV_P0);
					}
				}
				if (filterqueue4Handle != NULL) {
					/*---------余油压力值滤波----------*/
					osStatus_t status = osMessageQueueGet(filterqueue4Handle, &msg4, 0, 0);
					if (status == osOK) {
							filtered_ADS_v1 = SlidingFilter_Update(&filter4, msg4.data);
							MPXV_P1 = ((filtered_ADS_v1 - 2.5f) / 2.0f) * 7.0f;  // kPa
//							CommonPrintf("P1 raw=%.3f, filtered=%.3f\r\n", msg4.data, filtered_MPXV_P1);
					}
				}
				if (filterqueue5Handle != NULL) {
					/*---------温度值滤波----------*/
					osStatus_t status = osMessageQueueGet(filterqueue5Handle, &msg5, 0, 0);
					if (status == osOK) {
							filtered_ADS_v2 = SlidingFilter_Update(&filter5, msg5.data); // 滤波
							float r_ntc = NTC_CalculateResistance(filtered_ADS_v2, 3.3f, 49.9f); // 电压, ADC基准, 分压电阻
							NTC_temp = NTC_GetTemperature(r_ntc);  // 查RT表得出温度值
					}
				}
        osDelay(5); 
  }
  /* USER CODE END FilterdataTask */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
void pack_filtered_cap(void) //  float filtered_cap --> uint32_t uint_filtered_cap
{
//		float filtered_value = filtered_cap1;
//    memcpy(&uint_filtered_cap, &filtered_value, sizeof(float));  // float --> uint32_t
}

// 队列安全写入（满时覆盖最旧数据）
osStatus_t QueuePutOverwrite(osMessageQueueId_t queue_id, const void *msg_ptr, uint8_t priority, uint32_t timeout) 
{
    osStatus_t status;

    // 先尝试直接放入
    status = osMessageQueuePut(queue_id, msg_ptr, priority, timeout);
    if (status == osOK) {
        return osOK; // 成功
    }

    // 如果队列已满，先取出最旧数据（非阻塞方式）
    if (status == osErrorResource) {
        uint8_t temp_buffer[MAX_FRAME_SIZE]; // 临时存储被丢弃的数据
        osMessageQueueGet(queue_id, temp_buffer, NULL, 0); // 0 表示立即返回
        // 再尝试放入
				printf("[QueuePutOverwrite] full \r\n");
        return osMessageQueuePut(queue_id, msg_ptr, priority, timeout);
    }
		printf("[QueuePutOverwrite] error \r\n");
    // 其他错误直接返回
    return status;
}
void ProcessFDC2214( uint8_t* init_ready_flag, uint8_t* valid_flag, uint8_t channel, uint8_t inductor, uint8_t par_capacitor, float zeroValue, uint8_t device_index) 
{
		Filter_msg1 msg1;	
		if (!(*init_ready_flag)) {
        *init_ready_flag = (FDC2214_Init() == 0) ? 1 : 0;
        if (!(*init_ready_flag)) {
            FDC_PRINTF("\r\nRetrying %d TFDC2214 init  in 2s ...", device_index);
						msg1.data = 0;
						if (QueuePutOverwrite(filterqueue1Handle, &msg1, 0, 0) != osOK) {
						}
            osDelay(2000);
            return;
        } else {
            *valid_flag = 1;
        }
    }

    if (*valid_flag) {
				float cap = 0.0f ;
        cap = getcap(channel, inductor, par_capacitor) - zeroValue; // 采集数据 通道0,电感，电容 /* 并联 18uh - 13pf : 测量47pf , -30.6 , 并联 18uh - 33pf : 测量47pf , -43.1 */
				
				oil_cap = cap; // 油水检测原始电容值			
				msg1.data = cap;
				FDC_PRINTF("msg1.data [%.5f] oil_cap [%.5f]\r\n",msg1.data, oil_cap);
				if (QueuePutOverwrite(filterqueue1Handle, &msg1, 0, 0) != osOK) {
						FDC_PRINTF("\r\nQueue full, cap=%.3f not sent", cap);
				}
    }
}

void CheckFDC2214_ID(uint8_t device_index, uint8_t* init_ready_flag, uint8_t* deviceValid)
{
		Filter_msg1 msg1;
	
    uint16_t id = 0;
    id = FDC2214_Read16(TFDC2214_Addr, 0x7F);

    if (id == FDC2214_ID) {
        FDC_PRINTF("[TFDC2214-%d] ok: ID=0x%04X\r\n", device_index, id);
        *deviceValid = 1;
    } else {
        *deviceValid = 0;
				*init_ready_flag = 0;
				msg1.data = 0;
				res_oil_cap = 0;
				filtered_oil_cap = 0;
				oil_percent = 0;
				// 循环读取队列直到清空
        while (osMessageQueueGet(filterqueue1Handle, &msg1, NULL, 0) == osOK) {
            // 读取成功，继续循环直到队列为空
        }
				FDC_PRINTF("[TFDC2214-%d] error: ID=0x%04X\r\n", device_index, id);
    }

}
void TProcessFDC2214( uint8_t* init_ready_flag, uint8_t* valid_flag, uint8_t channel, uint8_t inductor, uint8_t par_capacitor, float zeroValue, uint8_t device_index) 
{
		Filter_msg2 msg2;	
		if (!(*init_ready_flag)) {
        *init_ready_flag = (TFDC2214_Init() == 0) ? 1 : 0;
        if (!(*init_ready_flag)) {
            FDC_PRINTF("\r\nRetrying %d TFDC2214 init  in 2s ...", device_index);
            msg2.data = 0;
						if (QueuePutOverwrite(filterqueue2Handle, &msg2, 0, 0) != osOK) {
						}
						osDelay(2000);
            return;
        } else {
            *valid_flag = 1;
        }
    }

    if (*valid_flag) {
				float cap = 0.0f ;
        cap = Tgetcap(channel, inductor, par_capacitor) - zeroValue; // 采集数据 通道0,电感，电容 /* 并联 18uh - 13pf : 测量47pf , -30.6 , 并联 18uh - 33pf : 测量47pf , -43.1 */
				
				res_oil_cap = cap; // 余油检测原始电容值				
				msg2.data = cap;
				FDC_PRINTF("msg2.data [%.5f] res_oil_cap [%.5f]\r\n",msg2.data,res_oil_cap);
				if (QueuePutOverwrite(filterqueue2Handle, &msg2, 0, 0) != osOK) {
						FDC_PRINTF("\r\nQueue full, cap=%.3f not sent", cap);
				}
    }
}

void TCheckFDC2214_ID(uint8_t device_index, uint8_t* init_ready_flag, uint8_t* deviceValid)
{
		Filter_msg2 msg2;

    uint16_t id = 0;
    id = TFDC2214_Read16(TFDC2214_Addr, 0x7F);

    if (id == TFDC2214_ID) {
        FDC_PRINTF("[TFDC2214-%d] ok: ID=0x%04X\r\n", device_index, id);
        *deviceValid = 1;
    } else {
        *deviceValid = 0;
				*init_ready_flag = 0;
				msg2.data = 0;

				if (QueuePutOverwrite(filterqueue2Handle, &msg2, 0, 0) != osOK) {
				}
				// 循环读取队列直到清空
        while (osMessageQueueGet(filterqueue2Handle, &msg2, NULL, 0) == osOK) {
            // 读取成功，继续循环直到队列为空
        }
				FDC_PRINTF("[TFDC2214-%d] error: ID=0x%04X\r\n", device_index, id);
    }

}

/* USER CODE END Application */

