// -------------------------------------------------------------------------------------------------
tResult EmergencyFilter::OnPinEvent(IPin* source, tInt event_code, tInt param1, tInt param2,
IMediaSample* media_sample) {
// -------------------------------------------------------------------------------------------------
RETURN_IF_POINTER_NULL(source);
RETURN_IF_POINTER_NULL(media_sample);
if (event_code == IPinEventSink::PE_MediaSampleReceived) {
// Retrieve the sensor values from the sensor package media sample
if (source == &sensor_package_input_) {
SensorData data;
{
__adtf_sample_read_lock_mediadescription(sensor_package_description_, media_sample, coder);
coder->Get("ir_l_fc", (tVoid*) &(data.value_ir_l_fc));
coder->Get("ir_l_fl", (tVoid*) &(data.value_ir_l_fl));
coder->Get("ir_l_fr", (tVoid*) &(data.value_ir_l_fr));
coder->Get("ir_s_fc", (tVoid*) &(data.value_ir_s_fc));
coder->Get("ir_s_fl", (tVoid*) &(data.value_ir_s_fl));
coder->Get("ir_s_fr", (tVoid*) &(data.value_ir_s_fr));
coder->Get("ir_s_l", (tVoid*) &(data.value_ir_s_l));
coder->Get("ir_s_r", (tVoid*) &(data.value_ir_s_r));
coder->Get("ir_s_rc", (tVoid*) &(data.value_ir_s_rc));
coder->Get("us_f_l", (tVoid*) &(data.value_us_f_l));
coder->Get("us_f_r", (tVoid*) &(data.value_us_f_r));
coder->Get("us_r_l", (tVoid*) &(data.value_us_r_l));
coder->Get("us_r_r", (tVoid*) &(data.value_us_r_r));
}
// Check if the car needs to stop immediately
if (stopCarNow(data) && !suppressed_) {
if (debug_) LOG_INFO("Enable emergency stopping");
transmitBool(wheel_active_output_, false);
transmitBool(hazard_light_output_, true);
} else if (!suppressed_) {
if (debug_) LOG_INFO("Disable emergency stopping");
transmitBool(wheel_active_output_, true);
transmitBool(hazard_light_output_, false);
}
} else if (source == &suppress_input_) {
{
__adtf_sample_read_lock_mediadescription(suppression_data_description_, media_sample, coder);
coder->Get("bValue", (tVoid*)&suppressed_);
}
if(suppressed_) {
transmitBool(wheel_active_output_, true);
}
}
}
RETURN_NOERROR;
}
tResult cSensorAnalyzer::ProcessUltrasonicSample(IMediaSample* pMediaSample, SensorDefinition::ultrasonicSensor usSensor)
{
__synchronized_obj(m_oProcessUsDataCritSection);
//write values with zero
tFloat32 f32value = 0;
{ // focus for sample read lock
// read-out the incoming Media Sample
__adtf_sample_read_lock_mediadescription(m_pDescriptionUsData,pMediaSample,pCoderInput);
// get the IDs for the items in the media sample
if(!m_bIDsUltrasonicSet)
{
pCoderInput->GetID("f32Value", m_szIDUltrasonicF32Value);
pCoderInput->GetID("ui32ArduinoTimestamp", m_szIDUltrasonicArduinoTimestamp);
m_bIDsUltrasonicSet = tTrue;
}
//get values from media sample
pCoderInput->Get(m_szIDUltrasonicF32Value, (tVoid*)&f32value);
// convert distance from meter to centimeter
f32value = f32value * 100.0f;
//emit signal to gui
switch (usSensor)
{
case SensorDefinition::usFrontLeft:
emit SensorDataChanged(LIST_ULTRASONIC_FRONT_LEFT, static_cast<float>(f32value));
break;
case SensorDefinition::usFrontCenterLeft:
emit SensorDataChanged(LIST_ULTRASONIC_FRONT_CENTER_LEFT, static_cast<float>(f32value));
break;
case SensorDefinition::usFrontCenter:
emit SensorDataChanged(LIST_ULTRASONIC_FRONT_CENTER, static_cast<float>(f32value));
break;
case SensorDefinition::usFrontCenterRight:
emit SensorDataChanged(LIST_ULTRASONIC_FRONT_CENTER_RIGHT, static_cast<float>(f32value));
break;
case SensorDefinition::usFrontRight:
emit SensorDataChanged(LIST_ULTRASONIC_FRONT_RIGHT, static_cast<float>(f32value));
break;
case SensorDefinition::usSideLeft:
emit SensorDataChanged(LIST_ULTRASONIC_SIDE_LEFT, static_cast<float>(f32value));
break;
case SensorDefinition::usSideRight:
emit SensorDataChanged(LIST_ULTRASONIC_SIDE_RIGHT, static_cast<float>(f32value));
break;
case SensorDefinition::usRearLeft:
emit SensorDataChanged(LIST_ULTRASONIC_REAR_LEFT, static_cast<float>(f32value));
break;
case SensorDefinition::usRearCenter:
emit SensorDataChanged(LIST_ULTRASONIC_REAR_CENTER, static_cast<float>(f32value));
break;
case SensorDefinition::usRearRight:
emit SensorDataChanged(LIST_ULTRASONIC_REAR_RIGHT, static_cast<float>(f32value));
break;
}
}
RETURN_NOERROR;
}
tResult cSensorAnalyzer::ProcessInerMeasUnitSample(IMediaSample* pMediaSample)
{
//write values with zero
tFloat32 f32Q_w = 0;
tFloat32 f32Q_x = 0;
tFloat32 f32Q_y = 0;
tFloat32 f32Q_z = 0;
tFloat32 f32A_x = 0;
tFloat32 f32A_y = 0;
tFloat32 f32A_z = 0;
tFloat32 f32Roll = 0;
tFloat32 f32Pitch = 0;
tFloat32 f32Yaw = 0;
{ // focus for sample read lock
// read-out the incoming Media Sample
__adtf_sample_read_lock_mediadescription(m_pDescriptionInerMeasUnitData,pMediaSample,pCoderInput);
// get the IDs for the items in the media sample
if(!m_bIDsInerMeasUnitSet)
{
pCoderInput->GetID("f32Q_w", m_szIDInerMeasUnitF32Q_w);
pCoderInput->GetID("f32Q_x", m_szIDInerMeasUnitF32Q_x);
pCoderInput->GetID("f32Q_y", m_szIDInerMeasUnitF32Q_y);
pCoderInput->GetID("f32Q_z", m_szIDInerMeasUnitF32Q_z);
pCoderInput->GetID("f32A_x", m_szIDInerMeasUnitF32A_x);
pCoderInput->GetID("f32A_y", m_szIDInerMeasUnitF32A_y);
pCoderInput->GetID("f32A_z", m_szIDInerMeasUnitF32A_z);
pCoderInput->GetID("ui32ArduinoTimestamp", m_szIDInerMeasUnitArduinoTimestamp);
m_bIDsInerMeasUnitSet = tTrue;
}
//get values from media sample
pCoderInput->Get(m_szIDInerMeasUnitF32Q_w, (tVoid*)&f32Q_w);
pCoderInput->Get(m_szIDInerMeasUnitF32Q_x, (tVoid*)&f32Q_x);
pCoderInput->Get(m_szIDInerMeasUnitF32Q_y, (tVoid*)&f32Q_y);
pCoderInput->Get(m_szIDInerMeasUnitF32Q_z, (tVoid*)&f32Q_z);
pCoderInput->Get(m_szIDInerMeasUnitF32A_x, (tVoid*)&f32A_x);
pCoderInput->Get(m_szIDInerMeasUnitF32A_y, (tVoid*)&f32A_y);
pCoderInput->Get(m_szIDInerMeasUnitF32A_z, (tVoid*)&f32A_z);
//emit signal to gui
calulateEulerAngles(f32Q_w,f32Q_x,f32Q_y,f32Q_z,f32Yaw, f32Pitch,f32Roll);
// multiplicate with 180/pi (*180/M_PI) to get angle in degree
emit SensorDataChanged(LIST_GYROSCOPE_X_ACC, static_cast<tFloat>(f32A_x));
emit SensorDataChanged(LIST_GYROSCOPE_Y_ACC, static_cast<tFloat>(f32A_y));
emit SensorDataChanged(LIST_GYROSCOPE_Z_ACC, static_cast<tFloat>(f32A_z));
emit SensorDataChanged(LIST_GYROSCOPE_YAW, static_cast<tFloat>(f32Yaw*180/M_PI));
emit SensorDataChanged(LIST_GYROSCOPE_PITCH, static_cast<tFloat>(f32Pitch*180/M_PI));
emit SensorDataChanged(LIST_GYROSCOPE_ROLL, static_cast<tFloat>(f32Roll*180/M_PI));
}
RETURN_NOERROR;
}
tResult FunctionDriver::OnPinEvent(IPin *source, tInt nEventCode, tInt nParam1, tInt nParam2, IMediaSample *mediaSample)
{
if (nEventCode != IPinEventSink::PE_MediaSampleReceived)
{
RETURN_NOERROR;
}
if (source == &distanceOverallPin)
{
{
__adtf_sample_read_lock_mediadescription(descriptionSignal, mediaSample, inputCoder);
inputCoder->Get("f32Value", (tVoid *) &distanceOverall);
distanceOverall = distanceOverall * 100;
}
}
else
{
RETURN_ERROR(ERR_NOT_SUPPORTED);
}
tFloat32 x = wayLengthFunction.CalculateX(distanceOverall);
tFloat32 y = CalculateSinus(0, x);
double d = 1;
tFloat32 m = CalculateSinus(0, x + d) - CalculateSinus(0, x - d);
tFloat32 m_arc = atan(m) * (180 / M_PI);
tFloat32 y1 = CalculateSinus(3, x);
tFloat32 m1 = CalculateSinus(3, x + d) - CalculateSinus(3, x - d);
tFloat32 m1_arc = atan(m1) * (180 / M_PI);
tFloat32 steeringAngle = 90 - (m_arc - m1_arc);
logger.StartLog();
logger.Log(cString::Format("Distance Overall: %f", distanceOverall).GetPtr());
logger.Log(cString::Format("x: %f", x).GetPtr());
logger.Log(cString::Format("m: %f", m).GetPtr());
logger.Log(cString::Format("y: %f", y).GetPtr());
logger.Log(cString::Format("m1: %f", m1).GetPtr());
logger.Log(cString::Format("y1: %f", y1).GetPtr());
logger.Log(cString::Format("Steering Angle: %f", steeringAngle).GetPtr());
logger.Log("----");
logger.EndLog();
SendValue(mediaSample, speedPin, speed);
SendValue(mediaSample, steeringAnglePin, steeringAngle);
RETURN_NOERROR;
}
tFloat32 YawToSteer::GetFloat(IMediaSample *mediaSample)
{
__synchronized_obj(iosync);
tFloat32 value = 0.0f;
{
__adtf_sample_read_lock_mediadescription(descriptionSignalValue, mediaSample, pCoderInput);
// get the IDs for the items in the media sample
if (!m_bIDsSignalSet)
{
pCoderInput->GetID("f32Value", m_szIDSignalF32Value);
m_bIDsSignalSet = tTrue;
}
//get values from media sample
pCoderInput->Get(m_szIDSignalF32Value, (tVoid *) &value);
}
return value;
}
tResult cSensorAnalyzer::ProcessWheelSample(IMediaSample* pMediaSample, SensorDefinition::wheelSensor wheelSensorType)
{
__synchronized_obj(m_oProcessWheelDataCritSection);
//write values with zero
tUInt32 ui32Tach = 0;
tInt8 i8Direction = 0;
{ // focus for sample read lock
// read-out the incoming Media Sample
__adtf_sample_read_lock_mediadescription(m_pDescriptionWheelData,pMediaSample,pCoderInput);
// get the IDs for the items in the media sample
if(!m_bIDsWheelDataSet)
{
pCoderInput->GetID("i8WheelDir", m_szIDWheelDataI8WheelDir);
pCoderInput->GetID("ui32WheelTach", m_szIDWheelDataUi32WheelTach);
pCoderInput->GetID("ui32ArduinoTimestamp", m_szIDWheelDataArduinoTimestamp);
m_bIDsWheelDataSet = tTrue;
}
//get values from media sample
pCoderInput->Get(m_szIDWheelDataUi32WheelTach, (tVoid*)&ui32Tach);
pCoderInput->Get(m_szIDWheelDataI8WheelDir, (tVoid*)&i8Direction);
//LOG_INFO(cString::Format("test %f, %d",ui32Tach,i8Direction));
//emit signal to gui
switch (wheelSensorType)
{
case SensorDefinition::wheelLeft:
emit SensorDataChanged(LIST_WHEEL_TACH_LEFT, static_cast<tFloat>(ui32Tach));
emit SensorDataChanged(LIST_WHEEL_DIR_LEFT, static_cast<tFloat>(i8Direction));
break;
case SensorDefinition::wheelRight:
emit SensorDataChanged(LIST_WHEEL_TACH_LEFT, static_cast<tFloat>(ui32Tach));
emit SensorDataChanged(LIST_WHEEL_DIR_LEFT, static_cast<tFloat>(i8Direction));
break;
}
}
RETURN_NOERROR;
}
tResult cSensorAnalyzer::ProcessVoltageSample(IMediaSample* pMediaSample, SensorDefinition::voltageSensor voltageSensor)
{
__synchronized_obj(m_oProcessVoltageDataCritSection);
//write values with zero
tFloat32 f32value = 0;
{ // focus for sample read lock
// read-out the incoming Media Sample
__adtf_sample_read_lock_mediadescription(m_pDescriptionVoltData,pMediaSample,pCoderInput);
// get the IDs for the items in the media sample
if(!m_bIDsVoltageSet)
{
pCoderInput->GetID("f32Value", m_szIDVoltageF32Value);
pCoderInput->GetID("ui32ArduinoTimestamp", m_szIDVoltageArduinoTimestamp);
m_bIDsVoltageSet = tTrue;
}
//get values from media sample
pCoderInput->Get(m_szIDVoltageF32Value, (tVoid*)&f32value);
//emit signal to gui
switch (voltageSensor)
{
case SensorDefinition::measurementCircuit:
emit SensorDataChanged(LIST_VOLTAGE_MEASUREMENT, static_cast<float>(f32value));
break;
case SensorDefinition::speedCntrlCircuit:
emit SensorDataChanged(LIST_VOLTAGE_SPEEDCTR, static_cast<float>(f32value));
break;
default:
break;
}
}
RETURN_NOERROR;
}
tResult cUSSSensorBundle::ProcessData(IMediaSample* pMediaSample)
{
RETURN_IF_POINTER_NULL(pMediaSample);
if (pMediaSample != NULL && m_pCoderDescInput != NULL)
{
//write values with zero
tUInt16 front_Left = 0;
tUInt16 front_Right = 0;
tUInt16 rear_Left = 0;
tUInt16 rear_Right = 0;
tUInt32 timestampValue = 0;
//get values from media sample
{ // focus for sample read lock
// read-out the incoming Media Sample
__adtf_sample_read_lock_mediadescription(m_pCoderDescInput,pMediaSample,pCoder);
pCoder->Get("ui16Front_Left", (tVoid*)&front_Left);
pCoder->Get("ui16Front_Right", (tVoid*)&front_Right);
pCoder->Get("ui16Rear_Left", (tVoid*)&rear_Left);
pCoder->Get("ui16Rear_Right", (tVoid*)&rear_Right);
pCoder->Get("ui32ArduinoTimestamp", (tVoid*)×tampValue);
}
//create and transmit output mediasamples
tFloat32 flFront_Left = tFloat32(front_Left);
tFloat32 flFront_Right = tFloat32(front_Right);
tFloat32 flRear_Left = tFloat32(rear_Left);
tFloat32 flRear_Right= tFloat32(rear_Right);
TransmitData(pMediaSample->GetTime(),flFront_Left,flFront_Right,flRear_Left,flRear_Right,timestampValue);
}
RETURN_NOERROR;
}
tResult cDriverModule::OnPinEvent(IPin* pSource, tInt nEventCode, tInt nParam1, tInt nParam2, IMediaSample* pMediaSample)
{
RETURN_IF_POINTER_NULL(pMediaSample);
RETURN_IF_POINTER_NULL(pSource);
if (nEventCode == IPinEventSink::PE_MediaSampleReceived )
{
if (pSource == &m_JuryStructInputPin && m_pDescJuryStruct != NULL)
{
tInt8 i8ActionID = -2;
tInt16 i16entry = -1;
{ // focus for sample read lock
__adtf_sample_read_lock_mediadescription(m_pDescJuryStruct,pMediaSample,pCoder);
// get the IDs for the items in the media sample
if(!m_bIDsJuryStructSet)
{
pCoder->GetID("i8ActionID", m_szIDJuryStructI8ActionID);
pCoder->GetID("i16ManeuverEntry", m_szIDJuryStructI16ManeuverEntry);
m_bIDsJuryStructSet = tTrue;
}
pCoder->Get(m_szIDJuryStructI8ActionID, (tVoid*)&i8ActionID);
pCoder->Get(m_szIDJuryStructI16ManeuverEntry, (tVoid*)&i16entry);
}
switch (juryActions(i8ActionID))
{
case action_GETREADY:
if(m_bDebugModeEnabled) LOG_INFO(cString::Format("Driver Module: Received Request Ready with maneuver ID %d",i16entry));
emit SendRequestReady((int)i16entry);
break;
case action_START:
if(m_bDebugModeEnabled) LOG_INFO(cString::Format("Driver Module: Received Run with maneuver ID %d",i16entry));
emit SendRun((int)i16entry);
break;
case action_STOP:
if(m_bDebugModeEnabled) LOG_INFO(cString::Format("Driver Module: Received Stop with maneuver ID %d",i16entry));
emit SendStop((int)i16entry);
break;
}
}
else if (pSource == &m_ManeuverListInputPin && m_pDescManeuverList != NULL)
{
{ // focus for sample read lock
__adtf_sample_read_lock_mediadescription(m_pDescManeuverList,pMediaSample,pCoder);
std::vector<tSize> vecDynamicIDs;
// retrieve number of elements by providing NULL as first paramter
tSize szBufferSize = 0;
if(IS_OK(pCoder->GetDynamicBufferIDs(NULL, szBufferSize)))
{
// create a buffer depending on the size element
tChar* pcBuffer = new tChar[szBufferSize];
vecDynamicIDs.resize(szBufferSize);
// get the dynamic ids (we already got the first "static" size element)
if (IS_OK(pCoder->GetDynamicBufferIDs(&(vecDynamicIDs.front()), szBufferSize)))
{
// iterate over all elements
for (tUInt32 nIdx = 0; nIdx < vecDynamicIDs.size(); ++nIdx)
{
// get the value and put it into the buffer
pCoder->Get(vecDynamicIDs[nIdx], (tVoid*)&pcBuffer[nIdx]);
}
// set the resulting char buffer to the string object
m_strManeuverFileString = (const tChar*) pcBuffer;
}
// cleanup the buffer
delete pcBuffer;
}
}
// trigger loading maneuver list and update the ui
TriggerLoadManeuverList();
}
}
RETURN_NOERROR;
}
tResult cJuryTransmitter::OnPinEvent(IPin* pSource, tInt nEventCode, tInt nParam1, tInt nParam2, IMediaSample* pMediaSample)
{
RETURN_IF_POINTER_NULL(pMediaSample);
RETURN_IF_POINTER_NULL(pSource);
if (nEventCode == IPinEventSink::PE_MediaSampleReceived && pSource == &m_DriverStructInputPin)
{
tInt8 stateDriver = 0;
tInt16 entry = -1;
{ // focus for sample read lock
// read-out the incoming Media Sample
__adtf_sample_read_lock_mediadescription(m_pCoderDescDriverStruct,pMediaSample,pCoder);
pCoder->Get("i8StateID", (tVoid*)&stateDriver);
pCoder->Get("i16ManeuverEntry", (tVoid*)&entry);
}
//update the gui
emit sendDriverState(stateDriver, entry);
// set the first entry manually
if (m_last_timestamp==0)
{
m_last_timestamp = pMediaSample->GetTime();
m_last_entryId = entry;
}
//update the timeline
if (m_last_entryId < entry || m_last_entryId == -1 || stateDriver == -1 || stateDriver == 2)
{
tTimeStamp timeDiff = (pMediaSample->GetTime() - m_last_timestamp)/1000; //time in milliseconds
QString Message;
switch (stateCar(stateDriver))
{
case stateCar_READY:
Message = "from " + QString::number(m_last_entryId) + " to " + QString::number(entry) + ": " + QString::number(timeDiff) + " msec: Ready";
break;
case stateCar_RUNNING:
Message = "from " + QString::number(m_last_entryId) + " to " + QString::number(entry) + ": " + QString::number(timeDiff) + " msec: OK";
break;
case stateCar_COMPLETE:
Message = "from " + QString::number(m_last_entryId) + " to " + QString::number(entry) + ": " + QString::number(timeDiff) + " msec: Complete";
break;
case stateCar_ERROR:
Message = "from " + QString::number(m_last_entryId) + " to " + QString::number(entry) + ": " + QString::number(timeDiff) + " msec: Error";
break;
case stateCar_STARTUP:
break;
}
m_last_timestamp = pMediaSample->GetTime();
m_last_entryId = entry;
//update the gui
sendMessage(Message);
}
if(m_bDebugModeEnabled) LOG_INFO(cString::Format("Jury Module: State value: %i with index %i",stateDriver,entry));
}
RETURN_NOERROR;
}
void StateControllerNew::EvaluatePin(IPin *sourcePin, IMediaSample *mediaSample)
{
if (sourcePin == &maneuverListInput)
{
cString maneuverFileString;
{
__adtf_sample_read_lock_mediadescription(maneuverListDescription, mediaSample, pCoder);
std::vector<tSize> vecDynamicIDs;
// retrieve number of elements by providing NULL as first parameter
tSize bufferSize = 0;
if (IS_OK(pCoder->GetDynamicBufferIDs(NULL, bufferSize)))
{
// create a buffer depending on the size element
tChar pcBuffer[bufferSize];
vecDynamicIDs.resize(bufferSize);
// get the dynamic ids (we already got the first "static" size element)
if (IS_OK(pCoder->GetDynamicBufferIDs(&(vecDynamicIDs.front()), bufferSize)))
{
// iterate over all elements
for (tUInt32 nIdx = 0; nIdx < vecDynamicIDs.size(); ++nIdx)
{
// get the value and put it into the buffer
pCoder->Get(vecDynamicIDs[nIdx], (tVoid *) &pcBuffer[nIdx]);
}
maneuverFileString = (const tChar *) pcBuffer;
}
}
}
LoadManeuverList(maneuverFileString);
return;
}
if (sourcePin == &juryInput)
{
tInt8 juryActionId = -2;
tInt16 maneuverId = -1;
{
__adtf_sample_read_lock_mediadescription(juryDescription, mediaSample, pCoder);
pCoder->Get("i8ActionID", (tVoid *) &juryActionId);
pCoder->Get("i16ManeuverEntry", (tVoid *) &maneuverId);
}
switch (juryActions(juryActionId))
{
case action_GETREADY:
JuryGetReady(maneuverId);
break;
case action_START:
JuryStart(maneuverId);
break;
case action_STOP:
JuryStop(maneuverId);
break;
}
return;
}
if (sourcePin == &errorInput)
{
tBool isError = tFalse;
{
__adtf_sample_read_lock_mediadescription(boolDescription, mediaSample, pCoder);
pCoder->Get("bValue", (tVoid *) &isError);
}
if (isError)
{
logger.Log("Received Error");
SetState(tCarState::Error);
}
return;
}
if (sourcePin == &emergencyStopInput)
{
tBool isEmergencyStop = tFalse;
{
__adtf_sample_read_lock_mediadescription(boolDescription, mediaSample, pCoder);
pCoder->Get("bEmergencyStop", (tVoid *) &isEmergencyStop);
}
if (isEmergencyStop)
{
logger.Log("Received EmergencyStop");
SetState(tCarState::Startup);
}
return;
}
if (sourcePin == &incrementManeuverInput)
{
tBool incrementManeuver = tFalse;
{
__adtf_sample_read_lock_mediadescription(boolDescription, mediaSample, pCoder);
pCoder->Get("bValue", (tVoid *) &incrementManeuver);
}
if (incrementManeuver)
{
IncrementManeuver();
}
return;
}
if (sourcePin == &readyInput)
{
tReadyModule::ReadyModuleEnum module = tReadyModule::Nothing;
{
__adtf_sample_read_lock_mediadescription(enumDescription, mediaSample, pCoder);
pCoder->Get("tEnumValue", (tVoid *) &module);
}
logger.Log(cString::Format("Received ready from %s", tReadyModule::ToString(module).c_str()).GetPtr());
modulesReady.push_back(module);
if (tCarState::GetReady == carState && AreAllModulesReady())
{
logger.Log("All modules ready");
SetState(tCarState::Ready);
}
return;
}
}
tResult MarkerEvaluator::OnPinEvent(IPin *sourcePin, tInt eventCode, tInt nParam1, tInt nParam2, IMediaSample *mediaSample)
{
if (eventCode != IPinEventSink::PE_MediaSampleReceived)
{
RETURN_NOERROR;
}
RETURN_IF_POINTER_NULL(mediaSample);
logger.StartLog();
if (sourcePin == &roadSignInput)
{
struct timeval tv;
gettimeofday(&tv, NULL);
unsigned long long msnew =
(unsigned long long) (tv.tv_sec) * 1000 +
(unsigned long long) (tv.tv_usec) / 1000;
if (lastUpdate + 3000 < msnew)
{
signBuffer.clear();
}
lastUpdate = msnew;
tInt16 signId = 0;
tFloat32 signSize = 0;
{
__adtf_sample_read_lock_mediadescription(roadSignDescription, mediaSample, pCoderInput);
pCoderInput->Get("i16Identifier", (tVoid *) &signId);
pCoderInput->Get("f32Imagesize", (tVoid *) &signSize);
}
signBuffer.push_back(signId);
if (signBuffer.size() > RANGE_DATASET)
{
calcData();
}
}
else if (sourcePin == &getReadyInput)
{
tReadyModule::ReadyModuleEnum module;
{
__adtf_sample_read_lock_mediadescription(enumDescription, mediaSample, pCoder);
pCoder->Get("tEnumValue", (tVoid *) &module);
}
if (tReadyModule::MarkerEvaluator == module)
{
logger.Log("Resetting.", false);
signBuffer.clear();
SendEnum(markerOutput, static_cast<tInt>(tRoadSign::NO_MATCH));
SendValue(noOvertakingOutput, false);
SendEnum(readyOutput, static_cast<tInt>(tReadyModule::MarkerEvaluator));
}
}
logger.EndLog();
RETURN_NOERROR;
}
// -------------------------------------------------------------------------------------------------
tResult SensorPackageFilter::OnPinEvent(IPin* source, tInt event_code, tInt param1, tInt param2,
IMediaSample* media_sample) {
// -------------------------------------------------------------------------------------------------
RETURN_IF_POINTER_NULL(source);
RETURN_IF_POINTER_NULL(media_sample);
// Create a synchronizer, that enters the critical section whenever possible. If a previous
// thread is still inside the critical section, all proceeding threads have to wait for the
// first to leave.
__synchronized_obj(critical_section_);
if (event_code == IPinEventSink::PE_MediaSampleReceived) {
tFloat32 sensor_value = 0;
tUInt32 time_stamp = 0;
{
__adtf_sample_read_lock_mediadescription(sensor_data_description_, media_sample, coder);
coder->Get("f32Value", (tVoid*) &sensor_value);
coder->Get("ui32ArduinoTimestamp", (tVoid*) &time_stamp);
}
if (source == &ir_long_front_center_input_pin_) {
ir_l_fc_.add(sensor_value);
x_ &= 0b1111111111110;
}
else if (source == &ir_long_front_left_input_pin_) {
ir_l_fl_.add(sensor_value);
x_ &= 0b1111111111101;
}
else if (source == &ir_long_front_right_input_pin_) {
ir_l_fr_.add(sensor_value);
x_ &= 0b1111111111011;
}
// IR Short sensors
else if (source == &ir_short_front_center_input_pin_) {
ir_s_fc_.add(sensor_value);
x_ &= 0b1111111110111;
}
else if (source == &ir_short_front_left_input_pin_) {
ir_s_fl_.add(sensor_value);
x_ &= 0b1111111101111;
}
else if (source == &ir_short_front_right_input_pin_) {
ir_s_fr_.add(sensor_value);
x_ &= 0b1111111011111;
}
else if (source == &ir_short_left_input_pin_) {
ir_s_l_.add(sensor_value);
x_ &= 0b1111110111111;
}
else if (source == &ir_short_right_input_pin_) {
ir_s_r_.add(sensor_value);
x_ &= 0b1111101111111;
}
else if (source == &ir_short_rear_center_input_pin_) {
ir_s_rc_.add(sensor_value);
x_ &= 0b1111011111111;
}
else if (source == &us_front_left_input_pin_) {
us_f_l_.add(sensor_value);
x_ &= 0b1110111111111;
}
else if (source == &us_front_right_input_pin_) {
us_f_r_.add(sensor_value);
x_ &= 0b1101111111111;
}
else if (source == &us_rear_left_input_pin_) {
us_r_l_.add(sensor_value);
x_ &= 0b1011111111111;
}
else if (source == &us_rear_right_input_pin_) {
us_r_r_.add(sensor_value);
x_ &= 0b0111111111111;
}
if (!x_) {
if (GetPropertyBool("debug")) LOG_INFO("Transmit sensor package");
transmitSensorPackage(time_stamp);
x_ = 0b1111111111111;
} else {
}
}
RETURN_NOERROR;
}
//#############################################################################
tResult cMovementAnalyzer::ProcessInput(IMediaSample *pMediaSample, IPin *pSource)
{
//write values with zero
tFloat32 f32Value = 0;
tUInt32 ui32TimeStamp = 0;
//cString* logMsg = new cString();
{ // focus for sample read lock
// read-out the incoming Media Sample
__adtf_sample_read_lock_mediadescription(m_pDescriptionSignal, pMediaSample, pCoderInput);
// get the IDs for the items in the media sample
if (!m_bIDsSignalSet)
{
pCoderInput->GetID("f32Value", m_szIDSignalF32Value);
pCoderInput->GetID("ui32ArduinoTimestamp", m_szIDSignalArduinoTimestamp);
m_bIDsSignalSet = tTrue;
}
//get values from media sample
pCoderInput->Get(m_szIDSignalF32Value, (tVoid *) &f32Value);
pCoderInput->Get(m_szIDSignalArduinoTimestamp, (tVoid *) &ui32TimeStamp);
}
if (pSource == &this->m_oInputSpeed)
{
this->movementCalculator->setMeasSpeed(f32Value, ui32TimeStamp);
if (movementCalculator->newMovement())
{
tFloat32 calcSpeed = movementCalculator->calculateSpeedKalman();
Transmit(calcSpeed, pMediaSample, ui32TimeStamp, &this->m_oInputSpeed);
}
}
if (pSource == &this->m_oInputAccX)
{
RETURN_IF_FAILED(f32Value = adjustAcc(f32Value, calcOffsetAccX));
this->movementCalculator->setX(f32Value, ui32TimeStamp);
if (movementCalculator->newMovement())
{
newDistance = movementCalculator->calculateDistance();
Transmit(newDistance, pMediaSample, ui32TimeStamp, &this->m_oInputDistance);
Transmit(f32Value, pMediaSample, ui32TimeStamp, &this->m_oInputAccX);
}
}
if (pSource == &this->m_oInputAccY)
{
RETURN_IF_FAILED(f32Value = adjustAcc(f32Value, calcOffsetAccY));
this->movementCalculator->setY(f32Value, ui32TimeStamp);
if (movementCalculator->newMovement())
{
newDistance = movementCalculator->calculateDistance();
Transmit(newDistance, pMediaSample, ui32TimeStamp, &this->m_oInputDistance);
Transmit(f32Value, pMediaSample, ui32TimeStamp, &this->m_oInputAccY);
}
}
if (pSource == &this->m_oInputDistance)
{
}
//RETURN_IF_FAILED(Transmit(f32Value, pMediaSample, ui32TimeStamp, pSource));
RETURN_NOERROR;
}
