/** * Vehicle ModelOutput Behaviour Source. * * @file * Copyright © Digiteq automotive. All rights reserved * * $Author: voigtlpi $ * $Date: 2013-10-21 16:19:03 +0200 (Mon, 21 Oct 2013) $ * $Revision: 41925 $ * * @remarks */ #include "stdafx.h" #include "VehicleModelOutputBehaviour.h" /// Create filter for random number generator ADTF_FILTER_PLUGIN("Vehicle ModelOutput Behaviour", OID_ADTF_MODEL_OUT_BEHAVIOR_FILTER, cVehicleModelOutputB) cVehicleModelOutputB::cVehicleModelOutputB(const tChar* __info) : adtf::cAsyncDataTriggeredFilter(__info) { memset(&m_sVehicleBehaviour, 0, sizeof(m_sVehicleBehaviour)); m_DataReceived = false; m_CycleTime = 100000; } cVehicleModelOutputB::~cVehicleModelOutputB() { } tResult cVehicleModelOutputB::Start( __exception) { m_oThread.Run(); return cFilter::Start(__exception_ptr); } tResult cVehicleModelOutputB::Stop( __exception) { m_oThread.Suspend(); return cFilter::Stop(__exception_ptr); } const char OUTPUT_PIN_NAME[] = "output_vehicle_behaviour"; const char INPUT_PIN_NAME[] = "input_vehicle_behaviour"; tResult cVehicleModelOutputB::Init(tInitStage eStage, __exception) { RETURN_IF_FAILED(cAsyncDataTriggeredFilter::Init(eStage, __exception_ptr)); switch(eStage) { case StageFirst: // Output to ADTF framework RETURN_IF_FAILED(m_oOutputVehicleBehaviour.Create(OUTPUT_PIN_NAME, cObjectPtr(new cMediaType(MEDIA_TYPE_VEHICLE_MODEL_OUTPUT_BEHAVIOUR, MEDIA_SUBTYPE_VEHICLE_MODEL_OUTPUT_BEHAVIOUR_V1)), static_cast(this))); RETURN_IF_FAILED(RegisterPin(&m_oOutputVehicleBehaviour)); // Input from Matlab RETURN_IF_FAILED(m_pInputPin.Create(INPUT_PIN_NAME, cObjectPtr(new cMediaType(0,0)), //MEDIA_TYPE_STRUCTURED_DATA, MEDIA_SUBTYPE_STRUCT_STRUCTURED)), // Regular output for ADTF-Matlab connection //cObjectPtr(new cMediaType(MEDIA_TYPE_VEHICLE_MODEL_OUTPUT_BEHAVIOUR, MEDIA_SUBTYPE_VEHICLE_MODEL_OUTPUT_BEHAVIOUR_V1)), // Just a hack for EF6 testing. static_cast(this))); RETURN_IF_FAILED(RegisterPin(&m_pInputPin)); break; case StageNormal: // init all properties { adtf_util::cString strByteOrder = "LE"; if(PLATFORM_BYTEORDER == PLATFORM_BIG_ENDIAN_8) { strByteOrder = "BE"; } adtf_util::cString strTypeNameI = adtf_util::cString::Format("tVehicleModelOutputBehaviour_%s", OIGetInstanceName()); adtf_util::cString strMyDescriptionI = adtf_util::cString::Format( "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" ""); ucom::cObjectPtr pTypeI = NULL; RETURN_IF_FAILED(m_pInputPin.GetMediaType(&pTypeI)); ucom::cObjectPtr pTypeDescriptionI = NULL; if(IS_OK(pTypeI->GetInterface(IID_ADTF_MEDIA_TYPE_DESCRIPTION, (tVoid**)&pTypeDescriptionI))) { //if you are not sure your description works! //react on this return value! RETURN_IF_FAILED(pTypeDescriptionI->SetMediaSampleDescription("tVehicleModelOutputBehaviour", //strTypeName, strMyDescriptionI.GetPtr(), IMediaDescription::MDF_DDL020000)); } adtf_util::cString strMyDescription = adtf_util::cString::Format( "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "" "", offsetof(tVehicleModelAnimationState, fSteeringPosition), offsetof(tVehicleModelAnimationState, afWheelAngle), offsetof(tVehicleModelAnimationState, afWheelAngle) + sizeof(tFloat64), offsetof(tVehicleModelAnimationState, afWheelAngle) + 2*sizeof(tFloat64), offsetof(tVehicleModelAnimationState, afWheelAngle) + 3*sizeof(tFloat64), offsetof(tVehicleModelAnimationState, afWheelDeflection), offsetof(tVehicleModelAnimationState, afWheelDeflection) + sizeof(tFloat64), offsetof(tVehicleModelAnimationState, afWheelDeflection) + 2*sizeof(tFloat64), offsetof(tVehicleModelAnimationState, afWheelDeflection) + 3*sizeof(tFloat64), offsetof(tVehicleModelAnimationState, nLightMask), offsetof(tVehicleModelAnimationState, nLightMask) + 4, offsetof(tDrivetrain, fGear), offsetof(tDrivetrain, fGearboxType), offsetof(tDrivetrain, fDrivetrainType), offsetof(tDrivetrain, fTorqueGearBoxIn), offsetof(tDrivetrain, fTorqueCenterDiffOut), offsetof(tDrivetrain, fTorqueCenterDiffOut) + sizeof(tFloat64), offsetof(tDrivetrain, fTorqueShaft), offsetof(tEngine, fRpm), offsetof(tEngine, fTorque), offsetof(tEngine, fLoad), offsetof(tWheel, fSpringCompression), offsetof(tWheel, fSteeringAngle), offsetof(tWheel, fRotAngle), offsetof(tWheel, fRotAngleDot), offsetof(tWheel, fSlip), offsetof(tWheel, fBrakePressure), OUTPUT_PIN_NAME, offsetof(tVehicleModelOutputBehaviour,sAnimationState), offsetof(tVehicleModelOutputBehaviour,fSteeringMoment), offsetof(tVehicleModelOutputBehaviour,sEngine), offsetof(tVehicleModelOutputBehaviour,sDrivetrain), offsetof(tVehicleModelOutputBehaviour,sWheel), offsetof(tVehicleModelOutputBehaviour,sWheel) + sizeof(tWheel), offsetof(tVehicleModelOutputBehaviour,sWheel) + 2*sizeof(tWheel), offsetof(tVehicleModelOutputBehaviour,sWheel) + 3*sizeof(tWheel) ); ucom::cObjectPtr pType = NULL; RETURN_IF_FAILED(m_oOutputVehicleBehaviour.GetMediaType(&pType)); ucom::cObjectPtr pTypeDescription = NULL; if(IS_OK(pType->GetInterface(IID_ADTF_MEDIA_TYPE_DESCRIPTION, (tVoid**)&pTypeDescription))) { //if you are not sure your description works! //react on this return value! RETURN_IF_FAILED(pTypeDescription->SetMediaSampleDescription(OUTPUT_PIN_NAME, strMyDescription.GetPtr(), IMediaDescription::MDF_DDL020000)); } } break; case StageGraphReady: memset(&m_sVehicleBehaviour, 0, sizeof(m_sVehicleBehaviour)); m_DataReceived = false; m_oThread.SetCycleTime(m_CycleTime); m_oThread.Create(0,0,A_UTILS_NS::cThread::TF_Suspended,this); break; } RETURN_NOERROR; } tResult cVehicleModelOutputB::Shutdown(tInitStage eStage, __exception) { tResult nResult = ERR_NOERROR; RETURN_IF_FAILED(cFilter::Shutdown(eStage, __exception_ptr)); switch (eStage) { case StageFirst: break; case StageNormal: break; case StageGraphReady: { m_oThread.Release(); break; } } RETURN_NOERROR; } tResult cVehicleModelOutputB::Cycle(__exception) { if(!m_DataReceived) return ERR_NOERROR; // declare smart pointer to media sample which automatically // releases the reference to the sample in the sample pool // at function exit ucom::cObjectPtr pNewSample; // get a new media sample container from sample pool RETURN_IF_FAILED(AllocMediaSample((tVoid**)&pNewSample)); // alloc array buffer RETURN_IF_FAILED(pNewSample->AllocBuffer(sizeof(tVehicleModelOutputBehaviour))); tTimeStamp tmStreamTime = _clock ? _clock->GetStreamTime() : adtf_util::cHighResTimer::GetTime(); pNewSample->SetTime(tmStreamTime); // access the sample data buffer try { // acquire the write lock __sample_write_lock(pNewSample, tVehicleModelOutputBehaviour, pDestDataX); if(pDestDataX==NULL) return ERR_POINTER; memcpy(pDestDataX, &m_sVehicleBehaviour, sizeof(tVehicleModelOutputBehaviour)); // the write lock will be released when leaving this scope } catch(...) { LOG_ERROR("VehicleModelOutputBehaviour: cVehicleModelOutputB::Cycle crashed."); return ERR_EXCEPTION_RAISED; } // transmit sample through the output pin RETURN_IF_FAILED(m_oOutputVehicleBehaviour.Transmit(pNewSample)); RETURN_NOERROR; } tResult cVehicleModelOutputB::ThreadFunc(A_UTILS_NS::cThread *pThread, tVoid*pvUserData, tSize szUserData) { if(pThread==&m_oThread) Cycle(); RETURN_NOERROR; } tResult cVehicleModelOutputB::OnPinEvent(IPin* pSource, tInt nEventCode, tInt nParam1, tInt nParam2, IMediaSample* pMediaSample) { if(nEventCode == IPinEventSink::PE_MediaSampleReceived) { if(pSource == &m_pInputPin) { tResult ret = ProcessInputBehaviour(pMediaSample); if(ret != ERR_NOERROR) return ret; ret = PushOutputVehicleBehaviour(); if(ret != ERR_NOERROR) return ret; } else RETURN_ERROR(ERR_NOT_SUPPORTED); } RETURN_NOERROR; } tResult cVehicleModelOutputB::OnAsyncPinEvent(IPin *pSource, tInt nEventCode, tInt nParam1, tInt nParam2, IMediaSample *pMediaSample) { if(nEventCode == IPinEventSink::PE_MediaSampleReceived) { if(pSource == &m_pInputPin) { tResult ret = ProcessInputBehaviour(pMediaSample); if(ret != ERR_NOERROR) return ret; ret = PushOutputVehicleBehaviour(); if(ret != ERR_NOERROR) return ret; } else RETURN_ERROR(ERR_NOT_SUPPORTED); } RETURN_NOERROR; } #define LD_Float64(dest,src) memcpy(&dest,src,sizeof(double)) #define LD_UInt32(dest,src) memcpy(&dest,src,sizeof(unsigned)) void cVehicleModelOutputB::LoadVTDposition(const char *psInput, tVTDPosition &pVTDPosition) { // LD_Float64(pVTDPosition.afXYZ[0],psInput); // LD_Float64(pVTDPosition.afXYZ[1],psInput+8); // LD_Float64(pVTDPosition.afXYZ[2],psInput+16); // LD_Float64(pVTDPosition.afXYZdot[0],psInput+24); // LD_Float64(pVTDPosition.afXYZdot[1],psInput+32); // LD_Float64(pVTDPosition.afXYZdot[2],psInput+40); // LD_Float64(pVTDPosition.afXYZdotdot[0],psInput+48); // LD_Float64(pVTDPosition.afXYZdotdot[1],psInput+56); // LD_Float64(pVTDPosition.afXYZdotdot[2],psInput+64); // LD_Float64(pVTDPosition.afHPR[0],psInput+72); // LD_Float64(pVTDPosition.afHPR[1],psInput+80); // LD_Float64(pVTDPosition.afHPR[2],psInput+88); // LD_Float64(pVTDPosition.afHPRdot[0],psInput+96); // LD_Float64(pVTDPosition.afHPRdot[1],psInput+104); // LD_Float64(pVTDPosition.afHPRdot[2],psInput+112); // // } void cVehicleModelOutputB::LoadModelAnimationState(const char *psInput, tVehicleModelAnimationState &pAnimationState) { // LD_Float64(pAnimationState.fSteeringPosition,psInput); // LD_Float64(pAnimationState.afWheelAngle[0],psInput+8); // LD_Float64(pAnimationState.afWheelAngle[1],psInput+16); // LD_Float64(pAnimationState.afWheelAngle[2],psInput+24); // LD_Float64(pAnimationState.afWheelAngle[3],psInput+32); // LD_Float64(pAnimationState.afWheelDeflection[0],psInput+40); // LD_Float64(pAnimationState.afWheelDeflection[1],psInput+48); // LD_Float64(pAnimationState.afWheelDeflection[2],psInput+56); // LD_Float64(pAnimationState.afWheelDeflection[3],psInput+64); // LD_UInt32(pAnimationState.afWheelDeflection[0],psInput+72); // // // } void cVehicleModelOutputB::LoadEngine(const char *psInput, tEngine &pEngine) { // LD_Float64(pEngine.fRpm,psInput); //// LD_Float64(pEngine.fTorque,psInput+8); // LD_Float64(pEngine.fLoad,psInput+16); // // } void cVehicleModelOutputB::LoadWheel(const char *psInput, tWheel &pWheel) { // LD_Float64(pWheel.fSpringCompression,psInput); // LD_Float64(pWheel.fSteeringAngle,psInput+8); // LD_Float64(pWheel.fRotAngle,psInput+16); // LD_Float64(pWheel.fRotAngleDot,psInput+24); // LD_Float64(pWheel.fSlip,psInput+32); // LD_Float64(pWheel.fBrakePressure,psInput+40); // // } void cVehicleModelOutputB::LoadDrivetrain(const char *psInput, tDrivetrain &pDrivetrain) { // LD_Float64(pDrivetrain.fGear,psInput); // LD_Float64(pDrivetrain.fGearboxType,psInput+8); // LD_Float64(pDrivetrain.fDrivetrainType,psInput+16);// LD_Float64(pDrivetrain.fTorqueGearBoxIn,psInput+24);// LD_Float64(pDrivetrain.fTorqueCenterDiffOut[0],psInput+32);// LD_Float64(pDrivetrain.fTorqueCenterDiffOut[1],psInput+40);// LD_Float64(pDrivetrain.fTorqueShaft,psInput+48); // // } //############################################################################# tResult cVehicleModelOutputB::ProcessInputBehaviour(IMediaSample* pSample) { RETURN_IF_POINTER_NULL(pSample); const char *psInput; const int sz = pSample->GetSize(); if(sz != 480) return ERR_INVALID_TYPE; pSample->Lock((const tVoid**)&psInput); memset(&m_sVehicleBehaviour,0,sizeof(m_sVehicleBehaviour)); // LoadVTDposition(psInput,m_sVehicleBehaviour.sPosition); // LoadModelAnimationState(psInput+120,m_sVehicleBehaviour.sAnimationState); // LD_Float64(m_sVehicleBehaviour.fSteeringMoment,psInput+200); // LoadEngine(psInput+208,m_sVehicleBehaviour.sEngine); // LoadDrivetrain(psInput+232,m_sVehicleBehaviour.sDrivetrain); // LoadWheel(psInput+288,m_sVehicleBehaviour.sWheel[0]); // LoadWheel(psInput+336,m_sVehicleBehaviour.sWheel[1]); // LoadWheel(psInput+384,m_sVehicleBehaviour.sWheel[2]); // LoadWheel(psInput+432,m_sVehicleBehaviour.sWheel[3]); // // pSample->Unlock((tVoid*)psInput); m_DataReceived = true; RETURN_NOERROR; } //############################################################################# tResult cVehicleModelOutputB::PushOutputVehicleBehaviour() { cObjectPtr pVehicleBehaviourSample; RETURN_IF_FAILED(AllocMediaSample((tVoid**)&pVehicleBehaviourSample)); tTimeStamp tmStreamTime = _clock ? _clock->GetStreamTime() : adtf_util::cHighResTimer::GetTime(); try{RETURN_IF_FAILED(pVehicleBehaviourSample->Update(tmStreamTime, &m_sVehicleBehaviour, sizeof(m_sVehicleBehaviour), IMediaSample::MSF_None));} catch(...) { LOG_ERROR("VehicleModelOutputBehaviour: pVehicleInputInit->Update crashed."); return ERR_EXCEPTION_RAISED; } RETURN_IF_FAILED(m_oOutputVehicleBehaviour.Transmit(pVehicleBehaviourSample)); RETURN_NOERROR; }