Cockpit-cpp/ZjQt/EgoDLL/car_sim.cpp

#include <stdint.h>
#include "../common/comm.h"
#include "Protocol.pb.h"
#include "car_sim.h"
#include "EgoClient.h"

#include <random>
#include "control_sensor.h"
#include <stdio.h>
#include <stdarg.h>

CCarSim::CCarSim(CEgoClient* car) :_car(car)
{

}
void CCarSim::Start()
{

}
void CCarSim::Stop()
{
	
}

//接收下位机数据
void CCarSim::OnPeerMessage(int16_t cmd, int16_t length, const void* data)
{
	switch (cmd)
	{
		case RemoNet::CC_Ping:
		{
			RemoNet::CCPing Req;
			Req.ParseFromArray(data, length);
			int64_t tick = GetTickCount64();
			int32_t diff = tick - Req.tick();
			double temp = Req.temp();
			_car->OnPingValue(diff,temp);
		}
		break;
		case RemoNet::CC_StopACK:
		{
			_car->OnStopAck();
		}

		//接收下位机传上来的数据
		case RemoNet::CC_STATE:
		{
			RemoNet::State msg;
			msg.ParseFromArray(data, length);
			/*
			RemoNet::CCCanMsg msg;
			msg.ParseFromArray(data, length);
			int32_t count = msg.frams_size();
			std::vector<cannet_frame> vec;
			for (int32_t i = 0; i < count; i++)
			{
				const RemoNet::can_net_frame& frame = msg.frams(i);
				if (frame.canid() == 0x189)
				{
					cannet_frame node;
					node.canid = frame.canid();
					node.dlc = frame.dlc();
					memcpy(node.data, frame.data().data(), node.dlc);
				}
				switch (_car->GetCurrentPage())
				{
				case PageProp::WorkPage1:
				{
					switch (frame.canid())
					{
					case 0x1F0:case 0x2F0:case 0x3F0:case 0x4F0:case 0x1F1:case 0x1F2:case 0x2F1:
					case 0x2F2:
					{
						cannet_frame node;
						node.canid = frame.canid();
						node.dlc = frame.dlc();
						memcpy(node.data, frame.data().data(), node.dlc);
						vec.push_back(node);




					}
					break;
					}
				}
				break;
				case PageProp::WorkPage2:
				{
					switch (frame.canid())
					{
					case 0x3F2:
					case 0x4F2:
					{
						cannet_frame node;
						node.canid = frame.canid();
						node.dlc = frame.dlc();
						memcpy(node.data, frame.data().data(), node.dlc);
						vec.push_back(node);
					}
					break;

					}
				}
				break;
				case PageProp::CalibrationPage:
				{
					switch (frame.canid())
					{
					case 0x2F6:case 0x3F6:case 0x4F6:
					case 0x1F9:case 0x2F9:case 0x4F9:
					case 0x380 + 0x81: //0x401
					{
						cannet_frame node;
						node.canid = frame.canid();
						node.dlc = frame.dlc();
						memcpy(node.data, frame.data().data(), node.dlc);
						vec.push_back(node);
					}
					break;
					}
				}
				break;
				case PageProp::EngineParameterPage:
				{
					switch (frame.canid())
					{
					case 0x1F3: case 0x2F3:case 0x3F3:case 0x4F3:
					case 0x1F4:case 0x2F4:case 0x3F4: case 0x4F4:
					case 0x1F5:
					{
						cannet_frame node;
						node.canid = frame.canid();
						node.dlc = frame.dlc();
						memcpy(node.data, frame.data().data(), node.dlc);
						vec.push_back(node);
					}
					break;
					}
				}
				break;


				case PageProp::WarningPage:
				{
					switch (frame.canid())
					{
					case 0x1F6:
					{
						cannet_frame node;
						node.canid = frame.canid();
						node.dlc = frame.dlc();
						memcpy(node.data, frame.data().data(), node.dlc);
						vec.push_back(node);
					}
					break;
					}
				}
				break;
				}

			}
			*/

			/*
			FeedData data;
			data.brake_pressure = msg.brake_pressure();
			data.engine_rpm = msg.engine_rpm();
			data.engine_pressure = msg.engine_pressure();
			//data.gear = msg.gear();
			data.gearbox_oil_pressure = msg.gearbox_oil_pressure();
			data.gearbox_oil_temp = msg.gearbox_oil_temp();
			data.speed = msg.speed();
			data.work_pressure = msg.work_pressure();
			data.cold_water = msg.cold_water();
			data.steer_angle = msg.steer_angle();
			data.left_lock_status = msg.left_lock();
			data.right_lock_status = msg.right_lock();
			*/

			//定义临时变量用于接收
			FeedData data;
			data.engine_speed = msg.engine_speed();
			data.travel_speed = msg.travel_speed();
			data.fuel_level = msg.fuel_level();
			data.engine_temperature = msg.engine_temperature();
			data.hydraulic_oil_temperature = msg.hydraulic_oil_temperature();
			data.main_pump_1_pressure = msg.main_pump_1_pressure();
			data.main_pump_2_pressure = msg.main_pump_2_pressure();
			data.hand_gear = msg.hand_gear();
			data.actual_gear = msg.actual_gear();
			data.gripper_height = msg.gripper_height();
			data.amplitude = msg.amplitude();
			data.boom_angle = msg.boom_angle();
			data.stick_angle = msg.stick_angle();
			data.idle_protection = msg.idle_protection();
			data.front_toggle = msg.front_toggle();
			data.back_toggle = msg.back_toggle();
			data.interlock = msg.interlock();
			data.safety_switch = msg.safety_switch();
			data.arm_lift_pilot = msg.arm_lift_pilot();
			data.arm_lowering_pilot = msg.arm_lowering_pilot();
			data.lever_lifting_pilot = msg.lever_lifting_pilot();
			data.rod_lowering_pilot = msg.rod_lowering_pilot();
			data.left_rotary_pilot = msg.left_rotary_pilot();
			data.right_rotary_pilot = msg.right_rotary_pilot();
			data.grab_open_pilot = msg.grab_open_pilot();
			data.grab_close_pilot = msg.grab_close_pilot();
			data.safety_valves = msg.safety_valves();

			int jjj = msg.error_buff().size();
			for (int i = 0; i < msg.error_buff().size(); ++i)
			{
				data.warningData[i] = msg.error_buff()[i];
			}
			//发送信息给PCAN（下位机传的0x280透传）
			//动态分配TPCANMsg结构体大小的内存,将其地址赋给 CANMsg 指针
			TPCANMsg* CANMsg = (TPCANMsg*)malloc(sizeof(TPCANMsg));
			CANMsg->ID = 0x280;
			CANMsg->MSGTYPE = PCAN_MESSAGE_STANDARD;
			CANMsg->LEN = msg.error_buff().size();
			//将 data.warningData 中的数据复制到 CANMsg->DATA 中
			memcpy(CANMsg->DATA, data.warningData, CANMsg->LEN);
			//SendCANMessage(CANMsg);
			_car->SendCanDate(CANMsg);
			//释放之前通过 malloc 分配的 CANMsg 内存空间，确保不会发生内存泄漏
			free(CANMsg);
			
			//将报警信息解读出来赋值给data中的变量
			data.liftpilotpressuresensor_f = (data.warningData[0] & 0x1) == 0x1 ? 1 : 0;
			data.loweringpilotpressuresensor_f = (data.warningData[0] & 0x2) == 0x2 ? 1 : 0;
			data.leverliftpilotpressuresensor_f = (data.warningData[0] & 0x4) == 0x4 ? 1 : 0;
			data.rodloweringpilotpressurepensor_f = (data.warningData[0] & 0x8) == 0x8 ? 1 : 0;
			data.leftturnpilotpressuresensor_f = (data.warningData[0] & 0x10) == 0x10 ? 1 : 0;
			data.rightturnpilotpressuresensor_f = (data.warningData[0] & 0x20) == 0x20 ? 1 : 0;
			data.legextensionpilotpressuresensor_f = (data.warningData[0] & 0x40) == 0x40 ? 1 : 0;
			data.legretractpilotpressuresensor_f = (data.warningData[0] & 0x80) == 0x80 ? 1 : 0;
			data.grabopenpilotpressuresensor_f = (data.warningData[1] & 0x1) == 0x1 ? 1 : 0;
			data.grabclosurepilotpressuresensor_f = (data.warningData[1] & 0x2) == 0x2 ? 1 : 0;
			data.mainpump1pressuresensor_f = (data.warningData[1] & 0x4) == 0x4 ? 1 : 0;
			data.mainpump2pressuresensor_f = (data.warningData[1] & 0x8) == 0x8 ? 1 : 0;
			data.chamberpressuresensor_f = (data.warningData[1] & 0x10) == 0x10 ? 1 : 0;
			data.bucketrodpressuresensor_f = (data.warningData[1] & 0x20) == 0x20 ? 1 : 0;
			data.parkingpressuresensor_f = (data.warningData[1] & 0x40) == 0x40 ? 1 : 0;
			data.accumulatorpressuresensor_f = (data.warningData[1] & 0x80) == 0x80 ? 1 : 0;
			data.gaspedal_f = (data.warningData[2] & 0x1) == 0x1 ? 1 : 0;
			data.throttleknob_f = (data.warningData[2] & 0x2) == 0x2 ? 1 : 0;
			data.leghandle_f = (data.warningData[2] & 0x4) == 0x4 ? 1 : 0;
			data.gripknob_f = (data.warningData[2] & 0x8) == 0x8 ? 1 : 0;
			data.hydraulicoiltemperaturesensor_f = (data.warningData[2] & 0x10) == 0x10 ? 1 : 0;
			data.fuellevelsensor_f = (data.warningData[2] & 0x20) == 0x20 ? 1 : 0;
			data.boomanglesensor_f = (data.warningData[2] & 0x40) == 0x40 ? 1 : 0;
			data.bucketlevertiltsensor_f = (data.warningData[2] & 0x80) == 0x80 ? 1 : 0;
			data.expansionmodule_f = (data.warningData[3] & 0x1) == 0x1 ? 1 : 0;
			data.workingdistanceoverrun_f = (data.warningData[3] & 0x2) == 0x2 ? 1 : 0;
			data.lowoillevel_f = (data.warningData[3] & 0x4) == 0x4 ? 1 : 0;
			data.batterylowvoltage_f = (data.warningData[3] & 0x8) == 0x8 ? 1 : 0;
			data.batteryhighvoltage_f = (data.warningData[3] & 0x10) == 0x10 ? 1 : 0;
			data.accumulatorpressure_f = (data.warningData[3] & 0x20) == 0x20 ? 1 : 0;
			data.highhydraulicoiltemperature_f = (data.warningData[3] & 0x40) == 0x40 ? 1 : 0;
			data.lowoilpressure_f = (data.warningData[3] & 0x80) == 0x80 ? 1 : 0;
			data.coolantlevellow_f = (data.warningData[4] & 0x1) == 0x1 ? 1 : 0;
			data.coolanttemperaturehigh_f = (data.warningData[4] & 0x2) == 0x2 ? 1 : 0;
			data.systemoverload_f = (data.warningData[4] & 0x4) == 0x4 ? 1 : 0;
			data.safebypasspressdown_f = (data.warningData[4] & 0x8) == 0x8 ? 1 : 0;
			data.enginespn_f = (data.warningData[4] & 0x10) == 0x10 ? 1 : 0;
			data.enginewarmup_f = (data.warningData[4] & 0x20) == 0x20 ? 1 : 0;
			data.enginewarmuptimeout_f = (data.warningData[4] & 0x40) == 0x40 ? 1 : 0;
			data.emergencystoppress_f = (data.warningData[4] & 0x80) == 0x80 ? 1 : 0;
			data.leftfrontlegchosed = (data.warningData[6] & 0x1) == 0x1 ? 1 : 0;
			data.rightfrontlegchosed = (data.warningData[6] & 0x2) == 0x2 ? 1 : 0;
			data.leftrearlegchosed = (data.warningData[6] & 0x4) == 0x4 ? 1 : 0;
			data.rightrearlegchosed = (data.warningData[6] & 0x8) == 0x8 ? 1 : 0;


			//发送信息给PCAN（下位机传的0x384透传）
			TPCANMsg* CANMsg384 = (TPCANMsg*)malloc(sizeof(TPCANMsg));
			CANMsg384->ID = 0x384;
			CANMsg384->MSGTYPE = PCAN_MESSAGE_STANDARD;
			CANMsg384->LEN = msg.error_buff().size();
			//将 新增数据添加到 CANMsg384->DATA 中
			CANMsg384->DATA[0] = static_cast<BYTE>((msg.function_code() >> 8) & 0xFF);
			CANMsg384->DATA[1] = static_cast<BYTE>(msg.function_code() & 0xFF);
			CANMsg384->DATA[2] = static_cast<BYTE>((msg.main_add() >> 8) & 0xFF);
			CANMsg384->DATA[3] = static_cast<BYTE>(msg.main_add() & 0xFF);
			CANMsg384->DATA[4] = static_cast<BYTE>((msg.sub_add() >> 8) & 0xFF);
			CANMsg384->DATA[5] = static_cast<BYTE>(msg.sub_add() & 0xFF);
			CANMsg384->DATA[6] = static_cast<BYTE>((msg.paramter() >> 8) & 0xFF);
			CANMsg384->DATA[7] = static_cast<BYTE>(msg.paramter() & 0xFF);
			_car->SendCanDate(CANMsg384);
			//释放之前通过 malloc 分配的 CANMsg384 内存空间，确保不会发生内存泄漏
			free(CANMsg384);

			//发送信息给PCAN（下位机传的0x28B透传）
			TPCANMsg* CANMsg28B = (TPCANMsg*)malloc(sizeof(TPCANMsg));
			CANMsg28B->ID = 0x28B;
			CANMsg28B->MSGTYPE = PCAN_MESSAGE_STANDARD;
			CANMsg28B->LEN = msg.error_buff().size();
			//将 新增数据添加到 CANMsg28B->DATA 中
			CANMsg28B->DATA[0] = static_cast<BYTE>(msg.engine_speed() & 0xFF);
			CANMsg28B->DATA[1] = static_cast<BYTE>((msg.engine_speed() >> 8) & 0xFF);
			CANMsg28B->DATA[2] = static_cast<BYTE>(msg.engine_spn() & 0xFF);
			CANMsg28B->DATA[3] = static_cast<BYTE>((msg.engine_spn() >> 8) & 0xFF);
			CANMsg28B->DATA[4] = static_cast<BYTE>(msg.engine_fmi() & 0xFF);
			CANMsg28B->DATA[5] = static_cast<BYTE>((msg.engine_fmi() >> 8) & 0xFF);
			CANMsg28B->DATA[6] = static_cast<BYTE>(msg.engine_num() & 0xFF);
			CANMsg28B->DATA[7] = static_cast<BYTE>((msg.engine_num() >> 8) & 0xFF);
			_car->SendCanDate(CANMsg28B);
			//释放之前通过 malloc 分配的 CANMsg28B 内存空间，确保不会发生内存泄漏
			free(CANMsg28B);

			//发送信息给PCAN（下位机传的0x28F透传）
			TPCANMsg* CANMsg28F = (TPCANMsg*)malloc(sizeof(TPCANMsg));
			CANMsg28F->ID = 0x28F;
			CANMsg28F->MSGTYPE = PCAN_MESSAGE_STANDARD;
			CANMsg28F->LEN = msg.error_buff().size();
			//将 新增数据添加到 CANMsg28F->DATA 中
			int8_t data28F[8];
			for (int i = 0; i < msg.all_buff().size();i++)
			{
				data28F[i] = msg.error_buff()[i];
			}
			memcpy(CANMsg28F->DATA, data28F, CANMsg28F->LEN);
			_car->SendCanDate(CANMsg28F);
			//释放之前通过 malloc 分配的 CANMsg28F 内存空间，确保不会发生内存泄漏
			free(CANMsg28F);

			///**报警信息打印数据*/
			char buffer1[64];
			std::string ret1;
			for (int i = 0; i < 1; i++)
			{
				int16_t t = int(data.boomanglesensor_f);
				sprintf_s(buffer1, "%x 动臂倾角传感器故障", t);
				ret1 += buffer1;
				ret1 += "\n";
			}
			OutputDebugStringA(ret1.c_str());

			///**报警信息打印数据*/
			char buffer2[64];
			std::string ret2;
			for (int i = 0; i < 1; i++)
			{
				int16_t t = int(data.bucketlevertiltsensor_f);
				sprintf_s(buffer2, "%x 铲斗倾角传感器故障", t);
				ret2 += buffer2;
				ret2 += "\n";
			}
			OutputDebugStringA(ret2.c_str());
			//发送信息给qt界面
			_car->OnFeedPage(data);
		}
		break;

		
		case RemoNet::CC_NDTPOS:
		{
			RemoNet::NDTPos Req;
			Req.ParseFromArray(data, length);
			Position pos;
			pos.x = Req.x();
			pos.y = Req.y();
			pos.z = Req.z();
			pos.rx = Req.rx();
			pos.ry = Req.ry();
			pos.rz = Req.rz();
			pos.rw = Req.rw();
			_car->OnNDTPos(&pos);
		}
		break;
		

	}
}