// ------------------------------------------------------------------------------------------------- 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; }