/*
 * msp_Force_Sensor.c
 *
 *  Created on: Oct 8, 2024
 *      Author: akeguo
 */
#include "msp_Dynamometer_Sensor.h"
#include "bsp_Error.pb.h"
#include "BHBF_ROBOT.h"


void decode_Dynamometer(uint8_t *buffer, uint16_t length);
void GF_DS_Inquiry();//inqure data from the, in fact this is a modbus 03 function;

int32_t* DynamometerValue_in;
struct UARTHandler *Dynamometer_sensor;

DispacherController *Dynamometer_sensor_dispacherController;
void Dynamometer_sensor_intialize(struct UARTHandler *Handler)
{
	//uartHandler_intialize(&Force_sensor,Handler,10);
	Dynamometer_sensor = Handler;
	Dynamometer_sensor_dispacherController=Handler->dispacherController;

	LOG("Dynamometer_encoder_intialize");

		//log_info("angle_encoder_intialize");
	Dynamometer_sensor_dispacherController->Add_Dispatcher_List(Dynamometer_sensor_dispacherController,GF_DS_Inquiry);
	Dynamometer_sensor_dispacherController->Dispacher_Enable = 1;

	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"Dynamometer_sensor",0,ComError_Dynamometer_sensor);
		//uartHandler->Insert_HardWare_Entry_UART
	 LOG("Dynamometer_intialize");
}

uint8_t Inquiry_Order_Dynamometer[8]={0X01, 0X03, 0X00, 0X04, 0X00, 0X01, 0XC5, 0XCB}; //01 03 00 04 00 01 C5 CB
void GF_DS_Inquiry()
{

	MB_ReadHoldingReg(&Dynamometer_sensor->Tx_Buf,
			&Dynamometer_sensor->TxCount, 1, 0x0004, 1); //03 command ; read 2 registers

	Dynamometer_sensor->AddSendList(Dynamometer_sensor, Dynamometer_sensor->Tx_Buf,
			Dynamometer_sensor->TxCount, OneLineWaitTime, decode_Dynamometer);


}


void decode_Dynamometer(uint8_t *buffer, uint16_t length)
{
	uint8_t data[20];
	memcpy(data,buffer,length);
	LOG("start decoding and the length is %d",length);
	/* CRC 校验 */
	uint16_t crc_check = ((buffer[length - 1] << 8) | buffer[length - 2]);
	/* CRC 校验正确 */
	if (crc_check == MB_CRC16(buffer, length - 2))
	{
		*DynamometerValue_in = (int32_t)((int32_t)buffer[3]<<8|buffer[4]);

		HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"Dynamometer_sensor",1);
		LOG("decode_force  succeeded and the force is %d",*DynamometerValue_in);

	}
	else
	{
		//Decode Error；
		//log_error("wire sensor decoding failed");
		LOGFF(DL_ERROR,"Dynamometer_sensor decoding failed");

	}

}