Cockpit-cpp/ZjQt/EgoDLL/control_sensor.cpp

#define WIN32_LEAN_AND_MEAN
#include <WinSock2.h>
#include "PCANBasic.h"
#include "./include/EgoInterface.h"
#include "EgoClient.h"
#include "EgoWindow.h"
#include "protocol.pb.h"

#include "../common/iobuffer.h"
#include "protocol.pb.h"
#include "control_sensor.h"
#include <algorithm>
#include <string>
#include<json/json.h>

std::string CControlSensor::m_userID; // 在类外定义静态成员变量
int32_t CControlSensor::m_carID;

#define MAX_DGREE (int32_t)(4096/6)
inline int16_t make_int16(int8_t h, int8_t l)
{
	int16_t hi = (int16_t)(h & 0x00FF);
	int16_t low = (int16_t)(l & 0x00FF);

	return (hi << 8) | low;
}
inline int8_t hi_byte(int16_t value)
{
	int8_t hi =(int8_t)((value & 0xFF00) >> 8);
	return hi;
}
inline int8_t lo_byte(int16_t value)
{
	int8_t lo = (int8_t)(value & 0xff);
	return lo;
}
CControlSensor::CControlSensor(CMessageQueue* q) 
{
	_window = static_cast<CEgoWindow*>(q);
	_collection = Sensor_Collection::Sensor_None;
			 
}
void CControlSensor::Notify(TPCANMsg& canmsg,uint64_t timestamp)
{
	 
	//OutputDebugString(str);
    //OutputDebugString(ret.c_str());
	//发送座椅信号给下位机
	switch (canmsg.ID)
	{

		case 0x181:
		{
			//接收181msg
			frames[0].canid = canmsg.ID;
			frames[0].dlc = canmsg.LEN;
			memcpy(&frames[0].data, canmsg.DATA, sizeof(canmsg.DATA));
			//flag收到181
			_collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_181);
		}
		break;
		case 0x182:
		{
			//接收182msg
			frames[1].canid = canmsg.ID;
			frames[1].dlc = canmsg.LEN;
			memcpy(&frames[1].data, canmsg.DATA, sizeof(canmsg.DATA));
			//flag收到182
			_collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_182);
		}
		break;
		case 0x183:
		{			//接收183msg
			frames[2].canid = canmsg.ID;
			frames[2].dlc = canmsg.LEN;
			memcpy(&frames[2].data, canmsg.DATA, sizeof(canmsg.DATA));
			//flag收到183
			_collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_183);
		}
		break;
		case 0x184:
		//case 0x611:
		{

			//接收184msg
			frames[3].canid = canmsg.ID;
			frames[3].dlc = canmsg.LEN;
			memcpy(&frames[3].data, canmsg.DATA, sizeof(canmsg.DATA));
			//flag收到184
			_collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_184);

			//
			//解读184信号，发送到Qt显示（灯光与档位状态和声音的改变）
			bool cautionLight = frames[3].data[0] & 0x4;
			bool lightL = frames[3].data[0] & 0x8;
			bool lightR = frames[3].data[0] & 0x10;
			bool gearF = frames[3].data[0] & 0x20;
			bool gearR = frames[3].data[0] & 0x40;
			bool carPark = frames[3].data[1] & 0x1;
			bool buzzerPlay = frames[3].data[1] & 0x2;
			bool lightFront = frames[3].data[2] & 0x10;
			bool lightWork = frames[3].data[2] & 0x20;
			bool emergency = frames[3].data[3] & 0x2;

			_window->GetEgoClient()->OnCautionLight(cautionLight);
			_window->GetEgoClient()->OnLightL(lightL);
			_window->GetEgoClient()->OnLightR(lightR);
			_window->GetEgoClient()->OnGearF(gearF);
			_window->GetEgoClient()->OnGearR(gearR);
			_window->GetEgoClient()->OnCarPark(carPark);
			_window->GetEgoClient()->OnBuzzerPlay(buzzerPlay);
			_window->GetEgoClient()->OnLightFront(lightFront);
			_window->GetEgoClient()->OnLightWork(lightWork);
			_window->GetEgoClient()->OnEmergency(emergency);
			

		}
		break;
		case 0x185:
		{
			//接收185msg
			frames[4].canid = canmsg.ID;
			frames[4].dlc = canmsg.LEN;
			memcpy(&frames[4].data, canmsg.DATA, sizeof(canmsg.DATA));
			//flag收到185
			_collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_185);
		}
		break;

	}
	 
		 
	if (_window->GetControlState() == ControlState::Process && _collection == Sensor_Collection::Sensor_All)
	{		
		static DWORD tick = GetTickCount();
		RemoNet::CCCanMsg req;
		for (int32_t i = 0; i < _countof(frames); i++)
		{
			if (frames[i].canid != 0)
			{
				RemoNet::can_net_frame* frame = req.add_frams();
				frame->set_canid(frames[i].canid);
				frame->set_dlc(frames[i].dlc);
				frame->set_data(frames[i].data, frames[i].dlc);
			}
		}
		MessageHead Head;
		CIOBuffer pBuffer;
		Head.Command = RemoNet::CC_CANMSG;
		Head.Length = req.ByteSizeLong();
		Head.Serialize(pBuffer.Buffer);
		auto ptr = pBuffer.Buffer + MessageHead::Size();
		req.SerializeToArray(ptr, Head.Length);
		pBuffer.Length = MessageHead::Size() + Head.Length;
		_window->SendData(pBuffer);
		_collection = Sensor_Collection::Sensor_None;
		/*DWORD diff = GetTickCount() - tick;
		tick = GetTickCount();
		std::string str = std::to_string(diff);
		str += "\n";
		OutputDebugStringA(str.c_str());
		*/

		//mqtt
		//_window->GetEgoClient()->OnMqttData(frames);						
	}
		
}


void CControlSensor::NotifyMqtt()
{
	std::string userID = m_userID;
	int32_t vehicleID = m_carID;
	auto timeStamp = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();

	int keyStatus = ((frames[3].data[2] >> 7) & 0x01) * 2 + ((frames[3].data[3] >> 0) & 0x01) * 1;
	int gearControl = ((frames[3].data[0] >> 6) & 0x01) * 2 + ((frames[3].data[0] >> 5) & 0x01) * 1;
	int travelLight = ((frames[3].data[0] >> 0) & 0x01) * 2 + ((frames[3].data[0] >> 1) & 0x01) * 1;
	std::string accPedal = std::to_string(frames[0].data[0]) + " AND " + std::to_string(frames[0].data[1]) +
		" || " + std::to_string(frames[0].data[2]) + " AND " + std::to_string(frames[0].data[3]);
	std::string brakePedal = std::to_string(frames[0].data[4]) + " AND " + std::to_string(frames[0].data[5]);
	std::string steeringWheel = std::to_string(frames[2].data[2]) + " AND " + std::to_string(frames[2].data[3]);
	int turnSignal = ((frames[3].data[0] >> 4) & 0x01) * 2 + ((frames[3].data[0] >> 3) & 0x01) * 1;
	int turnMode = ((frames[3].data[3] >> 6) & 0x01) * 4 + ((frames[3].data[3] >> 5) & 0x01) * 3 +
		((frames[3].data[3] >> 4) & 0x01) * 2 + ((frames[3].data[3] >> 3) & 0x01) * 1;
	int workLight = ((frames[3].data[2] >> 5) & 0x01) * 2 + ((frames[3].data[2] >> 4) & 0x01) * 1;
	int baseLegSwitch = ((frames[3].data[1] >> 5) & 0x01) * 8 + ((frames[3].data[1] >> 4) & 0x01) * 4 +
		((frames[3].data[1] >> 3) & 0x01) * 2 + ((frames[3].data[1] >> 2) & 0x01) * 1;
	std::string baseLegControl = std::to_string(frames[0].data[6]) + " AND " + std::to_string(frames[0].data[7]);
	std::string taskJoint_1 = std::to_string(frames[1].data[0]) + " AND " + std::to_string(frames[1].data[1]);
	std::string taskJoint_2 = std::to_string(frames[1].data[6]) + " AND " + std::to_string(frames[1].data[7]);
	std::string taskJoint_3 = std::to_string(frames[1].data[2]) + " AND " + std::to_string(frames[1].data[3]);
	std::string endJoint = std::to_string(frames[2].data[0]) + " AND " + std::to_string(frames[2].data[1]);
	std::string toolControl = std::to_string(frames[1].data[4]) + " AND " + std::to_string(frames[1].data[5]);
	int cabLift = ((frames[3].data[1] >> 7) & 0x01) * 2 + ((frames[3].data[1] >> 6) & 0x01) * 1;
	int coopSignal = ((frames[3].data[4] >> 4) & 0x01) * 2 + ((frames[3].data[4] >> 3) & 0x01) * 1;
	int errAccPedal = ((frames[3].data[5] >> 7) & 0x01) * 2 + ((frames[3].data[5] >> 6) & 0x01) * 1;
	int errHandle = ((frames[3].data[5] >> 4) & 0x01) * 16 + ((frames[3].data[5] >> 3) & 0x01) * 8 + ((frames[3].data[5] >> 2) & 0x01) * 4 +
		((frames[3].data[5] >> 1) & 0x01) * 2 + ((frames[3].data[5] >> 0) & 0x01) * 1;
			
	Json::Value pcanRoot;
	pcanRoot["cockpitID"] = Json::Value("Cop001");
	pcanRoot["userID"] = Json::Value(userID);
	pcanRoot["vehicleID"] = Json::Value(vehicleID);
	pcanRoot["timeStamp"] = Json::Value(timeStamp);

	Json::Value basRoot;
	basRoot["keyStatus"] = Json::Value(keyStatus);
	basRoot["parkControl"] = Json::Value(frames[3].data[1] >> 0 & 0x01);
	basRoot["travelMode"] = Json::Value(frames[3].data[3] >> 2 & 0x01);
	basRoot["eStop"] = Json::Value(frames[3].data[3] >> 1 & 0x01);
	basRoot["directSwitch"] = Json::Value(frames[3].data[4] >> 2 & 0x01);
	basRoot["gearControl"] = Json::Value(gearControl);
	basRoot["hazardLight"] = Json::Value(frames[3].data[0] >> 2 & 0x01);
	basRoot["travelLight"] = Json::Value(travelLight);
	basRoot["vehicleHorn"] = Json::Value(frames[3].data[1] >> 1 & 0x01);
	basRoot["silencedAlarm"] = Json::Value(frames[3].data[2] >> 6 & 0x01);
	pcanRoot["basControl"] = Json::Value(basRoot);
	Json::Value driveRoot;
	driveRoot["accPedal"] = Json::Value(accPedal);
	driveRoot["brakePedal"] = Json::Value(brakePedal);
	driveRoot["steeringWheel"] = Json::Value(steeringWheel);
	driveRoot["turnSignal"] = Json::Value(turnSignal);
	driveRoot["turnMode"] = Json::Value(turnMode);
	pcanRoot["driverControl"] = Json::Value(driveRoot);
	Json::Value taskRoot;
	taskRoot["enableHydraulic"] = Json::Value(frames[3].data[2] >> 0 & 0x01);
	taskRoot["workLight"] = Json::Value(workLight);
	taskRoot["bypassSwitch"] = Json::Value(frames[3].data[2] >> 1 & 0x01);
	taskRoot["baseLegSwitch"] = Json::Value(baseLegSwitch);
	taskRoot["baseLegControl"] = Json::Value(baseLegControl);
	taskRoot["taskJoint_1"] = Json::Value(taskJoint_1);
	taskRoot["taskJoint_2"] = Json::Value(taskJoint_2);
	taskRoot["taskJoint_3"] = Json::Value(taskJoint_3);
	taskRoot["endJoint"] = Json::Value(endJoint);
	taskRoot["toolControl"] = Json::Value(toolControl);
	taskRoot["suckerSelect"] = Json::Value(frames[3].data[2] >> 2 & 0x01);
	taskRoot["cabLift"] = Json::Value(cabLift);
	taskRoot["esCabLift"] = Json::Value(frames[3].data[2] >> 3 & 0x01);
	taskRoot["coopSignal"] = Json::Value(coopSignal);
	pcanRoot["taskControl"] = Json::Value(taskRoot);
	Json::Value errRoot;
	errRoot["errBasOperation"] = Json::Value("");
	errRoot["errAccPedal"] = Json::Value(errAccPedal);
	errRoot["errBrakePedal"] = Json::Value(frames[3].data[6] >> 0 & 0x01);
	errRoot["errSteeringWheel"] = Json::Value(frames[3].data[6] >> 1 & 0x01);
	errRoot["errHandle"] = Json::Value(errHandle);
	errRoot["errEndTool"] = Json::Value(frames[3].data[5] >> 5 & 0x01);
	errRoot["errOther"] = Json::Value("");
	pcanRoot["errCode"] = Json::Value(errRoot);

	// 转换 JSON 对象为字符串
	std::string jsonPcan = Json::writeString(Json::StreamWriterBuilder(), pcanRoot);

	//设置mqtt客户端
	mqtt::async_client client("10.10.60.237:41883", "clientPcan");
	callback cb;
	client.set_callback(cb);

	//配置 MQTT 连接的遗嘱消息
	mqtt::connect_options conopts;    //配置 MQTT 连接的选项
	/*mqtt::message willmsg("Cockpit/CanBus/CanId001/Cop001", "Last will and testament", 1, true);
	mqtt::will_options will(willmsg);
	conopts.set_will(will);*/

	try {
		mqtt::token_ptr conntok = client.connect(conopts);    //使用之前配置的conopts与MQTT 代理连接   
		conntok->wait();         //等待连接的确认消息,确保连接成功建立	

		mqtt::delivery_token_ptr pubtok;    //跟踪消息的发布状态的指针
		pubtok = client.publish("Cockpit/CanBus/CanId001/Cop001", jsonPcan.c_str(), jsonPcan.length(), 1, false);    //将消息发布到 MQTT 代理,设置参数
		//pubtok->wait_for(1);

		//检查MQTT客户端是否有发送失败的消息
		auto toks = client.get_pending_delivery_tokens();    //返回一个包含所有待处理投递令牌的容器
		if (!toks.empty())
			std::cout << "Error: There are pending delivery tokens!" << std::endl;
	}
	//异常处理块,用于捕获和处理 MQTT 相关的异常
	catch (const mqtt::exception& exc) {
		std::cerr << exc.what() << std::endl;
	}
}


ControlStatus CControlSensor::CheckStatus()
{
	return ControlStatus::Ok;
}
bool CControlSensor::Start()
{
	int iBuffer;
	TPCANStatus stsResult;
	TPCANHandle _HandlesArray[16];
	_HandlesArray[0] = PCAN_USBBUS1;
	_HandlesArray[1] = PCAN_USBBUS2;
	_HandlesArray[2] = PCAN_USBBUS3;
	_HandlesArray[3] = PCAN_USBBUS4;
	_HandlesArray[4] = PCAN_USBBUS5;
	_HandlesArray[5] = PCAN_USBBUS6;
	_HandlesArray[6] = PCAN_USBBUS7;
	_HandlesArray[7] = PCAN_USBBUS8;
	_HandlesArray[8] = PCAN_USBBUS9;
	_HandlesArray[9] = PCAN_USBBUS10;
	_HandlesArray[10] = PCAN_USBBUS11;
	_HandlesArray[11] = PCAN_USBBUS12;
	_HandlesArray[12] = PCAN_USBBUS13;
	_HandlesArray[13] = PCAN_USBBUS14;
	_HandlesArray[14] = PCAN_USBBUS15;
	_HandlesArray[15] = PCAN_USBBUS16;
	for (int i = 0; i < (sizeof(_HandlesArray) / sizeof(TPCANHandle)); i++)
	{
		stsResult = CAN_GetValue(_HandlesArray[i], PCAN_CHANNEL_CONDITION, &iBuffer, sizeof(iBuffer));
		if (((stsResult) == PCAN_ERROR_OK) && ((iBuffer & PCAN_CHANNEL_AVAILABLE) == PCAN_CHANNEL_AVAILABLE))
		{
			stsResult = CAN_GetValue((TPCANHandle)_HandlesArray[i], PCAN_CHANNEL_FEATURES, (void*)&iBuffer, sizeof(iBuffer));
			_isFD = (stsResult == PCAN_ERROR_OK) && (iBuffer & FEATURE_FD_CAPABLE);
			_handle = _HandlesArray[i];
			break;
		}
	}
	if (_handle != -1)
	{
		TPCANStatus stsResult = CAN_Initialize(_handle, PCAN_BAUD_250K);
		//使用雷达盒子测试用500k
		//TPCANStatus stsResult = CAN_Initialize(_handle, PCAN_BAUD_500K);
		if (stsResult != PCAN_ERROR_OK)
		{
			return false;
		}
		_thread = std::thread(&CControlSensor::Run, this);
	}
	return _handle != -1;
}
void CControlSensor::Run()
{
	_run = true;
	while (_run)
	{
		for (int32_t i = 0; i < sizeof(frames) / sizeof(cannet_frame); i++)
		{
			frames[i].canid = 0;
		}
		Sleep(10);
		ReadMessages();
	}
	CAN_Uninitialize(_handle);

}
void CControlSensor::ReadMessages()
{
	TPCANStatus stsResult;

	// We read at least one time the queue looking for messages. If a message is found, we look again trying to
	// find more. If the queue is empty or an error occurr, we get out from the dowhile statement.
	do
	{
		ReadMessage();
		if (stsResult != PCAN_ERROR_OK && stsResult != PCAN_ERROR_QRCVEMPTY)
		{
			//ShowStatus(stsResult);
			return;
		}
	} while (!(stsResult & PCAN_ERROR_QRCVEMPTY));
}
 
TPCANStatus CControlSensor::ReadMessage()
{
	TPCANMsg CANMsg;
	TPCANTimestamp CANTimeStamp;

	// We execute the "Read" function of the PCANBasic
	TPCANStatus stsResult = CAN_Read(_handle, &CANMsg, &CANTimeStamp);
	if (stsResult != PCAN_ERROR_QRCVEMPTY)
	{	// We process the received message
		uint64_t newTimestamp = (CANTimeStamp.micros + 1000 * CANTimeStamp.millis + 0x100000000 * 1000 * CANTimeStamp.millis_overflow);
		Notify(CANMsg, newTimestamp);
		NotifyMqtt();
	}

	return stsResult;
}

//将PCAN信号传回座椅PLC中
TPCANStatus CControlSensor::SendCANMessage(TPCANMsg* CANMsg)
{
	// 执行PCANBasic的“写入”功能
	TPCANStatus stsResult = CAN_Write(_handle, CANMsg);
	return stsResult;
}

void CControlSensor::Stop()
{
	_run = false;
	_thread.join();
}
void CControlSensor::SetEngineRPM(int32_t value)
{
	_rpm = value;
}

std::string CControlSensor::setSCSignUuid(std::string user_uuid)
{
	m_userID = user_uuid;
	return m_userID;
}

int32_t CControlSensor :: setCarID(int32_t CarID)
{
	m_carID = CarID;
	return m_carID;
}