tResult FunctionDriver::Init(tInitStage eStage, __exception)
{
RETURN_IF_FAILED(cFilter::Init(eStage, __exception_ptr));
if (eStage == StageFirst)
{
// create all the pins
RETURN_IF_FAILED(CreateInputPins(__exception_ptr));
RETURN_IF_FAILED(CreateOutputPins(__exception_ptr));
}
else if (eStage == StageNormal)
{
speed = tFloat32(GetPropertyFloat(SPEED_PROPERTY));
sinWidth = tFloat32(GetPropertyFloat(SINWIDTH_PROPERTY));
sinLength = tFloat32(GetPropertyFloat(SINLENGTH_PROPERTY));
}
RETURN_NOERROR;
}
/**
* Integrates the given function f from a to b using Method m
* @param f: static function pointer used to get integral values
* @param a: lower boundary
* @param b: upper boundary
* @param steps: steps used to calculate the result, use more to get a better result
* @param m: calculation method
*/
tFloat32 HTWKMath::Integrate(tFloat32 (*f)(tFloat32), tFloat32 a, tFloat32 b, int steps, MethodEnum m)
{
tFloat32 s = 0;
tFloat32 h = (b - a) / steps;
tFloat32 (*method)(tFloat32 (*f)(tFloat32), tFloat32 x, tFloat32 h);
switch (m)
{
case SimpsonMethod:
method = Integrate_Simpson;
}
for (int i = 0; i < steps; ++i)
{
s += method(f, a + h * i, h);
}
return h * s;
}
void ROI::CalculateRgbRoi()
{
tInt rgbRoomEndY = 0;
tInt rgbHoodStartY = rgbVideoInputFormat.nHeight;
if (isRoomDetectionEnabled)
{
rgbRoomEndY = tFloat32(roomHeight) / depthVideoInputFormat.nHeight * rgbVideoInputFormat.nHeight;
}
if (isHoodDetectionEnabled)
{
rgbHoodStartY = rgbVideoInputFormat.nHeight -
(tFloat32(hoodHeight) / depthVideoInputFormat.nHeight * rgbVideoInputFormat.nHeight);
}
rgbROI.tX = 0;
rgbROI.tY = rgbRoomEndY;
rgbROI.tWidth = rgbVideoInputFormat.nWidth;
rgbROI.tHeight = rgbHoodStartY - rgbRoomEndY;
logger.Log(cString::Format("RGB ROI x: %d, y: %d, width: %d, height: %d", rgbROI.tX, rgbROI.tY, rgbROI.tWidth, rgbROI.tHeight)
.GetPtr());
}
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;
}
// create and register the output pin
RETURN_IF_FAILED(m_pin_output_accelerate.Create("accelerate_out", pTypeSignalValueOutput, static_cast<IPinEventSink*> (this)));
RETURN_IF_FAILED(RegisterPin(&m_pin_output_accelerate));
RETURN_IF_FAILED(m_pin_output_steer_angle.Create("steerAngle", pTypeSignalValueOutput, static_cast<IPinEventSink*> (this)));
RETURN_IF_FAILED(RegisterPin(&m_pin_output_steer_angle));
/* RETURN_IF_FAILED(m_pin_output_turn_left_enabled.Create("turnSignalLeftEnabled", pTypeBoolSignalValueOutput, static_cast<IPinEventSink*> (this)));
RETURN_IF_FAILED(RegisterPin(&m_pin_output_turn_left_enabled));
RETURN_IF_FAILED(m_pin_output_turn_right_enabled.Create("turnSignalRightEnabled", pTypeBoolSignalValueOutput, static_cast<IPinEventSink*> (this)));
RETURN_IF_FAILED(RegisterPin(&m_pin_output_turn_right_enabled));*/
// get properties
m_pparkluecke = tFloat32(GetPropertyFloat("Parallel Parkluecke"));
m_pdistanz_a = tFloat32(GetPropertyFloat("Parallel Distanz A"));
m_pdistanz_b = tFloat32(GetPropertyFloat("Parallel Distanz B"));
m_pwinkel1 = tFloat32(GetPropertyFloat("Parallel Winkel 1"));
m_pwinkel2 = tFloat32(GetPropertyFloat("Parallel Winkel 2"));
m_cparkluecke = tFloat32(GetPropertyFloat("Cross Parkluecke"));
m_cdistanz_a = tFloat32(GetPropertyFloat("Cross Distanz A"));
m_cdistanz_b = tFloat32(GetPropertyFloat("Cross Distanz B"));
m_cwinkel1 = tFloat32(GetPropertyFloat("Cross Winkel 1"));
m_cwinkel2 = tFloat32(GetPropertyFloat("Cross Winkel 2"));
//set distance, status and active
distance_a = 0;
active = 0;
status = 0;
gyro_a = 0;
RETURN_IF_FAILED(pTypeSignalValueOutput->GetInterface(IID_ADTF_MEDIA_TYPE_DESCRIPTION, (tVoid**)&m_pCoderDescSignalOutput));
tChar const * strDescBoolSignalValue = pDescManager->GetMediaDescription("tBoolSignalValue");
RETURN_IF_POINTER_NULL(strDescBoolSignalValue);
cObjectPtr<IMediaType> pTypeBoolSignalValueOutput = new cMediaType(0, 0, 0, "tBoolSignalValue", strDescBoolSignalValue,IMediaDescription::MDF_DDL_DEFAULT_VERSION);
RETURN_IF_FAILED(pTypeBoolSignalValueOutput->GetInterface(IID_ADTF_MEDIA_TYPE_DESCRIPTION, (tVoid**)&m_pCoderDescBoolSignalOutput));
// create and register the output pin
RETURN_IF_FAILED(m_pin_output_accelerate.Create("accelerate_out", pTypeSignalValueOutput, static_cast<IPinEventSink*> (this)));
RETURN_IF_FAILED(RegisterPin(&m_pin_output_accelerate));
RETURN_IF_FAILED(m_pin_output_stuck.Create("got_stuck", pTypeBoolSignalValueOutput, static_cast<IPinEventSink*> (this)));
RETURN_IF_FAILED(RegisterPin(&m_pin_output_stuck));
// get properties
m_stopDistance = tFloat32(GetPropertyFloat("Hard Stop Distance"));
m_slowDistance15 = tFloat32(GetPropertyFloat("Slow down to 15 Distance"));
m_slowDistance25 = tFloat32(GetPropertyFloat("Slow down to 25 Distance"));
m_slowDistance30 = tFloat32(GetPropertyFloat("Slow down to 30 Distance"));
m_slowDistance_back = tFloat32(GetPropertyFloat("Back Distance"));
m_stuck_time = tFloat32(GetPropertyFloat("Stuck Time in s"));
m_stuck_distance = tFloat32(GetPropertyFloat("Stuck Distance"));
}
else if (eStage == StageNormal)
{
// In this stage you would do further initialisation and/or create your dynamic pins.
// Please take a look at the demo_dynamicpin example for further reference.
}
else if (eStage == StageGraphReady)
{
// All pin connections have been established in this stage so you can query your pins