Cockpit-cpp/EgoDLL/control_sensor.cpp

#define WIN32_LEAN_AND_MEAN
#include <WinSock2.h>
#include "PCANBasic.h"
#include "api/video/video_frame.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>

#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:
		{  
			/*
			std::string str;
			 
			char text[32];
			for (int i = 0; i < canmsg.LEN; i++)
			{   
				int16_t t = canmsg.DATA[i];
				sprintf_s(text, "%x ", t);
				str += text;
				 
			}
			str += "\n";
			OutputDebugString(str.c_str());
		    */ 
			isFront = (canmsg.DATA[1] & 0x1) == 1;
			frames[0].canid = canmsg.ID;
			frames[0].dlc = canmsg.LEN;
			
			memcpy(&frames[0].data, canmsg.DATA, sizeof(canmsg.DATA));
			if (_remote_active)
			{
				frames[0].data[0] = 0x10;
				_remote_active = false;
				_active_tick = GetTickCount64();
			}
			_collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_ER);
		 
			/*bool enbling = p->data[0] & 0x1;
			bool motoring = p->data[0] & 0x2;	
			bool gear = p->data[0] & 0x4;
			bool turnL = p->data[0] & 0x8;
			bool turnR = p->data[0] & 0x10;
			_window->GetEgoClient()->OnEmergency(enbling, motoring, gear, turnL, turnR);*/
		}
		break;
		case 0x281:
		{
	 
		/*	if (canmsg.DATA[2] == 0x03)
			{
				if (_rpm >= 300)
				{
					canmsg.DATA[2] = 0x01;
				}
				if (_startup == 0)
				{
					_startup= std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count();
				}
				else
				{
					 auto tick= std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count();
					 if (tick - _startup > 15)
					 {
						 canmsg.DATA[2] = 0x01;
					 }

				}
			}
			else
			{
				_startup = 0;
				_rpm = 0;
			}
			*/
			frames[2].canid = canmsg.ID;
			frames[2].dlc = canmsg.LEN;
			memcpy(&frames[2].data, canmsg.DATA, sizeof(canmsg.DATA));

			_collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_Signal);
			 
 
			

		}
		break;
		case 0x381:
		{
			//if (!_automous)
			{
				frames[3].canid = canmsg.ID;
				frames[3].dlc = canmsg.LEN;
			/*	if (isSliping)
				{
					canmsg.DATA[0] =  100;
					canmsg.DATA[1] = 100;
				}
				*/

				memcpy(&frames[3].data, canmsg.DATA, sizeof(canmsg.DATA));
			}
			/*else
			{
				frames[3].canid = 0;
			}
			*/
			

			_collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_Drive);
			 
		/*	int16_t accel = p->data[0];
		 
			bool lock_on_off = p->data[6] != 0;
			_window->GetEgoClient()->OnAccel(accel, lock_on_off);*/
		}
		break;
		case 0x185:
		{
			 
	//		if (!_bflash)
	//		{
			
	//			_bflash = true;
				 
				{
					//
				//if (!_automous)
				{
					int32_t steer = make_int16(canmsg.DATA[1], canmsg.DATA[0]);
					float period = steer - _old_steer;
					_old_steer = steer;
					float  diff = timestamp - _old_timestamp;
					diff /= 1000.f;
					diff /= 1000.f;
					_old_timestamp = timestamp;
					//float diff = tick - _cur_tick;
					frames[1].canid = canmsg.ID;
					frames[1].dlc = 8;
					int32_t speed = (period / (diff * 2.0f));
					if (speed == 0)
					{
						auto tick = GetTickCount64();
						if (_remote_active == false && tick - _active_tick > 500)
						{
							_remote_active = true;
						 
						}

					}
					if (_window->GetSteerAngle() < -MAX_DGREE)
					{

						if (isFront)
						{
							if (speed < 0)
								speed = 0;
						}
						else
						{
							if (speed > 0)
								speed = 0;
						}
					}
					else if (_window->GetSteerAngle() > MAX_DGREE)
					{
						if (isFront)
						{
							if (speed > 0)
								speed = 0;
						}
						else {
							if (speed < 0)
								speed = 0;
						}
					}
					else
					{
						if (_last_speed != 0xF000)
						{
							if ((_last_speed * speed) < 0)
							{
								_last_speed = speed;
								speed = 0;
							 }
						}
						else
						{
							_last_speed = speed;
						}
						
					}

				 
					int16_t m = abs(speed);
					if (m < 40) m = 0;
					if (m > 40 && m < 600) m = 600;
				
					//m=std::min(m, 3000);
					//_window->GetEgoClient()->OnSteer(m);
					if (m > 2500) m = 2500;
					//m /= 4; 5haoche  shiyong 
					frames[1].data[0] = 0xFF;
					frames[1].data[1] = 0x00;
					frames[1].data[2] = hi_byte(m);
					frames[1].data[3] = lo_byte(m);
					if(isFront)
						frames[1].data[4] = speed < 0 ? 1 : 0;
					else
						frames[1].data[4] = speed < 0 ? 0 : 1;
					frames[1].data[4] |= 0x02;
					frames[1].data[5] = 0;
					frames[1].data[6] = 0;
					frames[1].data[7] = 0;
				}
				/*else
				{
					frames[1].canid = 0;
				}*/
				
					_collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_Steer);

				}
		//	}
		//	memcpy(&frames[1], p, sizeof(cannet_frame));
		
			
			
		}
		break;
		case 0x481:
		{
			_collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_Arm);
			frames[4].canid = canmsg.ID;
			frames[4].dlc = canmsg.LEN;
			int16_t d = canmsg.DATA[0];
			int16_t l = canmsg.DATA[2];
			int16_t k = canmsg.DATA[4];
			isSliping = std::abs(d) > 10 || std::abs(l) >10 ||std::abs(k) > 10;
			
			memcpy(&frames[4].data, canmsg.DATA, sizeof(canmsg.DATA));
			
			 
			//OutputDebugString(ret.c_str());
			/*int16_t flip = p->data[0];
				int16_t armL = p->data[2];
				int16_t armR = p->data[4];
				bool jar_on_off = (p->data[6]&0x80) != 0;
				_window->GetEgoClient()->OnArm(flip, armL, armR, jar_on_off);
			*/
		}

		break;

		case 0x701:
			//_automous = canmsg.DATA[5] != 0;
			frames[5].canid = canmsg.ID;
			frames[5].dlc = canmsg.LEN;
			memcpy(&frames[5].data, canmsg.DATA, sizeof(canmsg.DATA));

			//_collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_Switch);
			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());
		*/
	}
}
 
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);
		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);
	}

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