/*
 * msp_CMCU.C
 *
 *  Created on: Jun 24, 2025
 *      Author: 13752
 */

#include "msp_CMCU.h"
#include "bsp_Error.pb.h"
#include "BHBF_ROBOT.h"

void GF_CMCU_Inquiry();
void decode_cmcu_sensor(uint8_t *buffer, uint16_t length);
void CMCU_Command_Comp();

struct UARTHandler *cmcu_sensor;



int32_t* CMCUValue_in;



DispacherController *cmcu_sensor_dispacherController;

int32_t Pressure_value = 0;
uint16_t Crc_check = 0;
uint8_t crc_checkH = 0;
uint8_t crc_checkL = 0;
uint8_t crc_checkh = 0;
uint8_t crc_checkl = 0;



void CMCU_sensor_intialize(struct UARTHandler *Handler)
{
	cmcu_sensor = Handler;
	cmcu_sensor->UART_Decode=decode_cmcu_sensor;
	cmcu_sensor->Wait_time = 6; //等待10ms 最低不要低于4；
	cmcu_sensor_dispacherController=Handler->dispacherController;
	cmcu_sensor_dispacherController->Dispacher_Enable=1;
	//不周期性发送
	cmcu_sensor_dispacherController->DispacherCallTime = 100;
	LOG("cmcu_sensor_intialize");

		//log_info("angle_encoder_intialize");
	cmcu_sensor_dispacherController->Add_Dispatcher_List(cmcu_sensor_dispacherController,GF_CMCU_Inquiry);


	//HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"Cmcu_Sensor",0,Cmcu_Sensor);

}

uint8_t Inquiry_Order_CMCU[8]={0X01, 0X03, 0X00, 0X00, 0X00, 0X02, 0XC4, 0X0B};
void GF_CMCU_Inquiry()
{
	memcpy(&cmcu_sensor->Tx_Buf, &Inquiry_Order_CMCU, 8);
	cmcu_sensor->TxCount = 8;
	cmcu_sensor->UART_Tx(cmcu_sensor);
}

void decode_cmcu_sensor(uint8_t *buffer, uint16_t length)
{
	if(buffer[0] == 0X01 && buffer[1] == 0X03 && buffer[2] == 0X04)
	{
		Pressure_value = ((buffer[5] << 24) | (buffer[6] << 16)) | ((buffer[3] << 8) | buffer[4]);
		crc_checkH = buffer[8];
		crc_checkL = buffer[7];
		Crc_check = MB_CRC16(buffer,7);
		crc_checkh = Crc_check >> 8;
		crc_checkl = Crc_check;
		if((crc_checkH == crc_checkh) && (crc_checkL == crc_checkl))
		{
			CMCU_Command_Comp();
		}

	}
}

void CMCU_Command_Comp()
{
	switch(Pressure_value & 0X80000000)
	{
		case 0:
			*CMCUValue_in = Pressure_value;
			break;
		case 0X80000000:
			*CMCUValue_in = Pressure_value - 0XFFFFFFFF - 1;
			break;
	}
}