Cockpit-cpp/EgoQt/common/can_sensor.cpp

#include <stdint.h>
#include <iostream>
#include <assert.h>
#include "can_sensor.h"
TPCANStatus CCanSensor::Initialize()
{
	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 !=PCAN_NONEBUS)
	{
	 
		stsResult= ::CAN_Initialize(_handle,PCAN_BAUD_250K, 0,0,0);  
		if(stsResult==PCAN_ERROR_OK)
		{
			iBuffer=PCAN_PARAMETER_ON; 
			stsResult=CAN_SetValue(_handle,PCAN_BUSOFF_AUTORESET,&iBuffer, sizeof(iBuffer));
		}
		return stsResult;
	}
	return stsResult;
}

TPCANStatus CCanSensor::GetErrorText(TPCANStatus Error, WORD Language, LPSTR Buffer)
{
	return CAN_GetErrorText(Error, Language, Buffer);
}
TPCANStatus CCanSensor::Uninitialize()
{
	return CAN_Uninitialize(_handle);
}
TPCANStatus CCanSensor::Read(TPCANMsg* MessageBuffer, TPCANTimestamp* TimestampBuffer)
{
	auto ret= CAN_Read(_handle,MessageBuffer,TimestampBuffer);
	//assert(ret==PCAN_ERROR_OK||PCAN_ERROR_QRCVEMPTY==ret);
	return ret;
}
TPCANStatus CCanSensor::Write(TPCANMsg* MessageBuffer)
{
	auto ret= CAN_Write(_handle,MessageBuffer);
	if(ret!=PCAN_ERROR_OK)
	{
		std::cout<<ret<<std::endl;
		CAN_Reset(_handle);
	}
	
	// assert(ret==PCAN_ERROR_OK);
	return ret;
}