tResult cXtionCamera::Init(tInitStage eStage, __exception )
{
RETURN_IF_FAILED(cFilter::Init(eStage, __exception_ptr));
if (eStage == StageFirst)
{
//create the output pins of the filter
RETURN_IF_FAILED(m_oRGBVideo.Create("Video_RGB", adtf::IPin::PD_Output, static_cast<IPinEventSink*> (this)));
RETURN_IF_FAILED(RegisterPin(&m_oRGBVideo));
RETURN_IF_FAILED(m_oDepthImage.Create("Depth_Image", adtf::IPin::PD_Output, static_cast<IPinEventSink*> (this)));
RETURN_IF_FAILED(RegisterPin(&m_oDepthImage));
}
else if (eStage == StageNormal)
{
//load the configuration file for the settings of xtion camera
tResult nResult;
nResult = LoadConfiguration();
if (IS_FAILED(nResult))
THROW_ERROR_DESC(nResult,"Failed to load the configuration file for the xtion");
//set the videoformat of the rgb video pin
m_sBitmapFormat.nWidth = m_nWidthVideo;
m_sBitmapFormat.nHeight = m_nHeightVideo;
m_sBitmapFormat.nBitsPerPixel = 24;
m_sBitmapFormat.nPixelFormat = cImage::PF_RGB_888;
m_sBitmapFormat.nBytesPerLine = m_nWidthVideo * 3;
m_sBitmapFormat.nSize = m_sBitmapFormat.nBytesPerLine * m_nHeightVideo;
m_sBitmapFormat.nPaletteSize = 0;
m_oRGBVideo.SetFormat(&m_sBitmapFormat, NULL);
//set the videoformat of the depth video pin
m_sBitmapFormatDepth.nWidth = m_nWidthDepth;
m_sBitmapFormatDepth.nHeight = m_nHeightDepth;
m_sBitmapFormatDepth.nBitsPerPixel = 16;
m_sBitmapFormatDepth.nPixelFormat = cImage::PF_GREYSCALE_16;
m_sBitmapFormatDepth.nBytesPerLine = m_nWidthDepth * 2;
m_sBitmapFormatDepth.nSize = m_sBitmapFormatDepth.nBytesPerLine * m_nHeightDepth;
m_sBitmapFormatDepth.nPaletteSize = 0;
m_oDepthImage.SetFormat(&m_sBitmapFormatDepth, NULL);
}
else if (eStage == StageGraphReady)
{
//check which streams has to be enabled in th xtion
tBool videoEnabled = m_oRGBVideo.IsConnected();
tBool depthEnabled = m_oDepthImage.IsConnected();
// if no output pin is connected the depth image is enabled because one stream must be set to tTrue;
if (videoEnabled==tFalse && depthEnabled==tFalse) depthEnabled = tTrue;
// initialize the grabber with the videoformats, which streams has the be enabled, the framerates and some more options
if (!(m_xtionGrabber.initialize(m_nWidthDepth,m_nHeightDepth,m_FPSDepth,m_nWidthVideo,m_nHeightVideo,m_FPSVideo,depthEnabled,videoEnabled,m_setDepthInMillimeter,m_setRegistration,m_setDepthColorSync,m_setAutoExposure,m_setAutoWhiteBalance)))
THROW_ERROR_DESC(ERR_DEVICE_NOT_READY,"Failed to initialize Xtion!")
else
LOG_INFO(cString::Format("Xtion Capture Device: Grabber is now initialized. Depth mode: %d Video mode: %d",depthEnabled,videoEnabled));
// creates the thread for the reading of data from the xtion
tResult nResult;
nResult = m_oWorker.Create(cKernelThread::TF_Suspended, static_cast<adtf::IKernelThreadFunc*>(this));
if (IS_FAILED(nResult))
THROW_ERROR_DESC(nResult,"Failed to create thread for xtion");
}
static int do_spi_io(struct spi_device* lp_dev, u8* lp_send_buffer, u8* lp_recv_buffer,
int buffer_size)
{
int ret_value;
struct spi_message msg;
struct spi_transfer xfer =
{
.len = buffer_size,
.tx_buf = (void*)lp_send_buffer,
.rx_buf = (void*)lp_recv_buffer,
.speed_hz = 1000000,
};
spi_message_init(&msg);
spi_message_add_tail(&xfer, &msg);
dev_info(&lp_dev->dev, "spi io: transfer size = %d\n", buffer_size);
ret_value = spi_sync(lp_dev, &msg);
if (IS_SUCCESS(ret_value))
{
dev_info(&lp_dev->dev, "spi io done.\n");
}
dev_info(&lp_dev->dev, "do_spi_io ret_value = %d\n", ret_value);
return ret_value;
}
int do_io_transaction(struct spi_device* lp_dev,
_IN_ struct spi_io_context* lp_io_context,
_IN_ u8* const lp_send_buffer, int send_buffer_size,
_OUT_ u8* lp_recv_buffer, int recv_buffer_size,
_OUT_ int* lp_recved_size )
{
int ret_value = ER_FAILED;
int total_trafster = 0;
int one_time_transfer = 0;
int total_receive_size = 0;
int remain_send_count = send_buffer_size;
int remain_recv_count = 0;
int is_recved_vaild_fh = 0;
struct buffer* lp_send_operator = NULL;
struct buffer* lp_recv_operator = NULL;
struct buffer dummy_send_buffer;
struct buffer dummy_recv_buffer;
struct buffer send_buffer;
struct buffer recv_buffer;
INIT_BUFFER(&dummy_send_buffer,
lp_io_context->send_dummy_buffer,
lp_io_context->send_dummy_buffer_size);
INIT_BUFFER(&dummy_recv_buffer,
lp_io_context->recv_dummy_buffer,
lp_io_context->recv_dummy_buffer_size);
INIT_BUFFER(&send_buffer, lp_send_buffer, send_buffer_size);
INIT_BUFFER(&recv_buffer, lp_recv_buffer, recv_buffer_size);
/*need some check here, but still in think.*/
total_trafster = send_buffer_size;
while(total_trafster > 0)
{
int send_buffer_is_dummy;
int recv_buffer_is_dummy;
/*
Step1. try calc out transfer bye count
*/
if (0 != BUFFER_REMAIN_LENGTH(send_buffer))
{
lp_send_operator = &send_buffer;
send_buffer_is_dummy = FALSE;
}
else
{
lp_send_operator = &dummy_send_buffer;
send_buffer_is_dummy = TRUE;
}
if (0 != remain_recv_count
&& is_recved_vaild_fh)
{
lp_recv_operator = &recv_buffer;
recv_buffer_is_dummy = FALSE;
}
else
{
lp_recv_operator = &dummy_recv_buffer;
recv_buffer_is_dummy = TRUE;
}
if (is_recved_vaild_fh)
{
RESET_BUFFER(&dummy_send_buffer);
RESET_BUFFER(&dummy_recv_buffer);
if (send_buffer_is_dummy && recv_buffer_is_dummy)
{
one_time_transfer = 0;
}
else
{
one_time_transfer = MIN(BUFFER_REMAIN_LENGTH(*lp_send_operator),
BUFFER_REMAIN_LENGTH(*lp_recv_operator));
}
}
else
{
/*
can't reset dummy recv buffer because it contain last time received
splited data
*/
one_time_transfer = MIN(BUFFER_REMAIN_LENGTH(*lp_send_operator),
BUFFER_REMAIN_LENGTH(dummy_recv_buffer));
}
if (0 == one_time_transfer)
{
/*caller's receive buffer is not enough case.*/
if ( 0 != remain_recv_count)
{
ret_value = ER_NO_ENOUGH_RECV_BUFFER;
}
break;
}
/*
Step 2. Prepare and do transfer
*/
dev_info(&lp_dev->dev, "before do_spi_io\n");
ret_value = do_spi_io(lp_dev,
BUFFER_PTR(*lp_send_operator),
BUFFER_PTR(*lp_recv_operator),
one_time_transfer);
if (IS_FAILED(ret_value))
{
dev_err(&lp_dev->dev, "do_spi_io() failed! \n");
break;
}
dev_info(&lp_dev->dev, "after do_spi_io\n");
lp_send_operator->index += one_time_transfer;
lp_recv_operator->index += one_time_transfer;
remain_send_count = MAX(0, remain_send_count - one_time_transfer);
remain_recv_count = MAX(0, remain_recv_count - one_time_transfer);
total_trafster -= one_time_transfer;
/*
Step 3. check if we received valid frame header
*/
if (!is_recved_vaild_fh)
{
int total_payload_size;
int contained_payload_size;
int fh_start_index;
int is_valid_fh;
is_valid_fh = verify_frame_head_and_get_payload_size(lp_recv_operator->lp_ptr,
BUFFER_USED_LENGTH(*lp_recv_operator),
&total_payload_size,
&contained_payload_size,
&fh_start_index);
if (IS_SUCCESS(is_valid_fh))
{
int copy_size = contained_payload_size + SIZE_OF_FRAME_HEAD;
int need_recv_buffer_size = total_payload_size + SIZE_OF_FRAME_HEAD;
/*received new frame head!*/
remain_recv_count = total_payload_size - contained_payload_size;
/*received frame head, so we update total transfer count here*/
total_trafster = MAX(remain_recv_count, remain_send_count);
/*
printf("[packege check]: total payload = %d, contained = %d, fh_start = %d\n",
total_payload_size,
contained_payload_size,
fh_start_index);
*/
/*copy all valid data to actual receive buffer head*/
if (need_recv_buffer_size > BUFFER_REMAIN_LENGTH(recv_buffer))
{
ret_value = ER_NO_ENOUGH_RECV_BUFFER;
break;
}
/*do not reset buffer, because we now support
received mulit-frame in one io cycle.
*/
//RESET_BUFFER(&recv_buffer);
memcpy(BUFFER_PTR(recv_buffer),
lp_recv_operator->lp_ptr + fh_start_index,
copy_size);
/*
save total received size here to support receive mulit-frame
*/
total_receive_size += need_recv_buffer_size;
recv_buffer.index += copy_size;
recv_buffer.length = total_receive_size;
// pr_err("dump: index = %d, length = %d\n",
// recv_buffer.index, recv_buffer.length);
is_recved_vaild_fh = TRUE;
}
else
{
int is_recved_hf_prefix = ER_FAILED;
remain_recv_count = 0;
//copy SIZEOF_FRAME_HEAD bytes from tail to head
memcpy(dummy_recv_buffer.lp_ptr,
BUFFER_PTR_FROM_USED_TAIL(*lp_recv_operator, SIZE_OF_FRAME_HEAD),
SIZE_OF_FRAME_HEAD);
dummy_recv_buffer.index = SIZE_OF_FRAME_HEAD;
/*check if the last SIZE_OF_FRAME_HEAD bytes contained frame head prefix,
we will read more data if it contained, to resovle slice case
*/
is_recved_hf_prefix = verify_frame_head_prefix(BUFFER_PTR_FROM_USED_TAIL(*lp_recv_operator, SIZE_OF_FRAME_HEAD),
SIZE_OF_FRAME_HEAD);
/*
check if the received data included frame head prefix 0x53
*/
if (IS_SUCCESS(is_recved_hf_prefix))
{
total_trafster += BUFFER_REMAIN_LENGTH(dummy_recv_buffer);
/*
printf("set total_transfer = %d\n", total_trafster);
*/
}
is_recved_vaild_fh = FALSE;
}
}
else
{
/*
if we already received one frame, but still has some data need
send, we need change is_recved_vaild_fh = FALSE to prepare receive
the next frame
*/
#if 1
if (remain_send_count > 0
&& 0 == remain_recv_count)
{
is_recved_vaild_fh = FALSE;
RESET_BUFFER(&dummy_recv_buffer);
//pr_err("psh: note: try receive mulit-frame.\n");
}
#endif
}
}
#if 1
if (IS_FAILED(ret_value))
{
/*
dump recvied buffer
*/
dump_buffer(lp_recv_operator->lp_ptr, BUFFER_USED_LENGTH(*lp_recv_operator));
}
else
{
//dump_buffer(recv_buffer.lp_ptr, BUFFER_USED_LENGTH(recv_buffer));
}
#endif
lp_recved_size ? *lp_recved_size = BUFFER_USED_LENGTH(recv_buffer) : 0;
return ret_value;
}
tResult cJuryTransmitter::Run(tInt nActivationCode, const tVoid* pvUserData, tInt szUserDataSize, ucom::IException** __exception_ptr/* =NULL */)
{
if (nActivationCode == IRunnable::RUN_TIMER)
{
//not aus timer
if (pvUserData==&m_hTimerNotAusCounter)
{
m_hTimerNotAusCounter++;
RETURN_IF_FAILED(sendEmergencyStop());
// the signal was transmitted three times
if (m_hTimerNotAusCounter >= 3 && m_hTimerNotAus)
{
__synchronized_obj(m_oCriticalSectionTimerNotAus);
// Destroy the timer
tResult nResult = _kernel->TimerDestroy(m_hTimerNotAus);
if (IS_FAILED(nResult)) THROW_ERROR(nResult);
}
}
//start event timer
if (pvUserData==&m_hTimerStartCounter)
{
m_hTimerStartCounter++;
RETURN_IF_FAILED(sendJuryStruct(action_START,m_Start_entry_id));
// the signal was transmitted three times
if (m_hTimerStartCounter >= 3 && m_hTimerStart)
{
__synchronized_obj(m_oCriticalSectionTimerStart);
// Destroy the timer
tResult nResult = _kernel->TimerDestroy(m_hTimerStart);
if (IS_FAILED(nResult)) THROW_ERROR(nResult);
}
}
//request ready event timer
if (pvUserData==&m_hTimerRequestReadyCounter)
{
m_hTimerRequestReadyCounter++;
RETURN_IF_FAILED(sendJuryStruct(action_GETREADY,m_Request_Ready_entry_id));
// the signal was transmitted three times
if (m_hTimerRequestReadyCounter >= 3 && m_hTimerRequestReady)
{
__synchronized_obj(m_oCriticalSectionTimerRequestReady);
// Destroy the timer
tResult nResult = _kernel->TimerDestroy(m_hTimerRequestReady);
if (IS_FAILED(nResult)) THROW_ERROR(nResult);
}
}
//stop event timer
if (pvUserData==&m_hTimerStopCounter)
{
m_hTimerStopCounter++;
RETURN_IF_FAILED(sendJuryStruct(action_STOP,m_Stop_entry_id));
// the signal was transmitted three times
if (m_hTimerStopCounter >= 3 && m_hTimerStop)
{
__synchronized_obj(m_oCriticalSectionTimerStop);
// Destroy the timer
tResult nResult = _kernel->TimerDestroy(m_hTimerStop);
if (IS_FAILED(nResult)) THROW_ERROR(nResult);
}
}
}
return cBaseQtFilter::Run(nActivationCode, pvUserData, szUserDataSize, __exception_ptr);
}
/**
*
* Main function of the program
*
* @param argc [in] Argument count
* @param argv [in] Argument pointer
* @return Returns a standard result code.
*
*/
int main(int argc, const char **argv)
{
cCommandLine oCmd(argc, argv);
// parameter check
if (argc != 3 && argc != 4)
{
LOG_INFO("Usage: programme name -ip=DEST_IP -message=MSG_NAME "
"[-port=PORT_NB(otherwise 5000)] [-tcp]");
return 1;
}
// name of the message e.g. inport-name inside ATDF
cString strMessage = oCmd.GetProperty("message") + "_raw";
cString strDestIp = oCmd.GetProperty("ip");
cString strPort = oCmd.GetProperty("port");
tInt nPort = 5000;
if (strPort.IsNotEmpty())
{
nPort = strPort.AsInt();
}
// my user data
cString strData = "myProcessData";
tInt szSize = strData.GetLength();
if (!oCmd.GetFlag("tcp"))
{
// use UDP
// filling the header with information
cMemoryBlock oBlock(ADTF_DX_UDP_MAX_PACKAGESIZE);
tADTFDXUDPHeaderASync *psHeader = (tADTFDXUDPHeaderASync *) oBlock.GetPtr();
psHeader->ui32StartBytes = ADTF_DX_UDP_STARTBYTES;
psHeader->ui32SenderUID = 1;
psHeader->tmTime = cSystem::GetTime();
psHeader->ui8HeaderType = ADTF_DX_UDP_HEADER_TYPE_ASYNC;
psHeader->ui32DataSize = szSize;
cString::Copy(psHeader->strMsgId, strMessage, -1, ADTF_DX_UDP_MAX_MESSAGEID_LENGTH + 2);
psHeader->ui32MsgSize = szSize;
// copy data
tVoid *pDataArea = (tVoid *) (((tUInt8 *) psHeader) + sizeof(tADTFDXUDPHeaderASync));
cMemoryBlock::MemCopy(pDataArea, strData.GetPtr(), psHeader->ui32DataSize);
/*!
* socket information
* IP 192.168.1.151 is the Destination Address. ADTF is running there
*/
tUInt32 ui32Address = cSocket::AddressToUInt32(strDestIp);
LOG_INFO("Destination IP: " + strDestIp);
cDatagramSocket oSenderPort;
oSenderPort.Open();
// sending the data
for (tInt n = 0; n < 20; n++)
{
LOG_INFO("Try to send data. " + cString::FromInt(n));
if (IS_FAILED(oSenderPort.Write(ui32Address,
nPort,
oBlock.GetPtr(),
sizeof(tADTFDXUDPHeaderASync) + psHeader->ui32DataSize)))
{
LOG_ERROR("Unable to send data");
return 1;
}
}
}
else
{
//use TCP
cStreamSocket oSocket;
if (IS_FAILED(oSocket.Connect(strDestIp, ADTF_DX_TCP_CHANNEL_DEFAULT_PORT)))
{
LOG_ERROR("Unable to connect");
return 1;
}
//calculate the overall size of the message
tSize nPayloadSize = strData.GetLength() + 1;
tADTFDXTCPMessageHeader sHeader;
sHeader.nMessageNameSize = strMessage.GetLength() + 1;
sHeader.nReplyMessageNameSize = 0;
sHeader.nSize = sizeof(tADTFDXTCPMessageHeader) + sHeader.nMessageNameSize + (tUInt32) nPayloadSize;
// important: convert to network byte order
cSystem::HostToNetwork(&sHeader.nSize);
for (tInt nMessage = 0; nMessage < 20; ++nMessage)
{
LOG_INFO("Try to send data. " + cString::FromInt(nMessage));
// first send the header
if (IS_FAILED(oSocket.Write(&sHeader, sizeof(sHeader))))
{
LOG_ERROR("Unable to send header");
return 1;
}
// then the message name
if (IS_FAILED(oSocket.Write(strMessage.GetPtr(), sHeader.nMessageNameSize)))
{
LOG_ERROR("Unable to send message name");
return 1;
}
// then the payload (we do not send a reply message name)
if (IS_FAILED(oSocket.Write(strData.GetPtr(), (tInt) nPayloadSize)))
{
LOG_ERROR("Unable to send payload");
return 1;
}
}
oSocket.Close();
}
return 0;
}