int mantis_suspend(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
unsigned char reg;
int result;
struct mantis_private_data *private_data =
(struct mantis_private_data *)pdata->private_data;
result = mantis_set_odr(mlsl_handle, pdata, &private_data->suspend,
TRUE, private_data->suspend.odr);
ERROR_CHECK(result);
result = mantis_set_irq(mlsl_handle, pdata, &private_data->suspend,
TRUE, private_data->suspend.irq_type);
ERROR_CHECK(result);
result = inv_serial_read(mlsl_handle, pdata->address,
MPUREG_PWR_MGMT_2, 1, ®);
ERROR_CHECK(result);
reg |= (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA);
result = inv_serial_single_write(mlsl_handle, pdata->address,
MPUREG_PWR_MGMT_2, reg);
ERROR_CHECK(result);
return INV_SUCCESS;
}
void NetShrVarInterface::updateParamCNVImpl(int param_index, CNVData data, CNVDataType type, unsigned int nDims, bool do_asyn_param_callbacks)
{
if (nDims == 0)
{
typename CNV2C<cnvType>::ctype val;
int status = CNVGetScalarDataValue (data, type, &val);
ERROR_CHECK("CNVGetScalarDataValue", status);
updateParamValue(param_index, val, do_asyn_param_callbacks);
CNV2C<cnvType>::free(val);
}
else
{
typename CNV2C<cnvType>::ctype* val;
size_t dimensions[10];
int status = CNVGetArrayDataDimensions(data, nDims, dimensions);
ERROR_CHECK("CNVGetArrayDataDimensions", status);
size_t nElements = 1;
for(unsigned i=0; i<nDims; ++i)
{
nElements *= dimensions[i];
}
val = new typename CNV2C<cnvType>::ctype[nElements];
status = CNVGetArrayDataValue(data, type, val, nElements);
ERROR_CHECK("CNVGetArrayDataValue", status);
updateParamArrayValue(param_index, val, nElements);
delete[] val;
}
}
void report(FILE* fp, const char* conn_type, void* handle, bool buffered)
{
int error, conn_error;
CNVConnectionStatus status;
fprintf(fp, " Connection type: %s", conn_type);
if (handle == 0)
{
fprintf(fp, " Status: <not being used>\n");
return;
}
error = CNVGetConnectionAttribute(handle, CNVConnectionStatusAttribute, &status);
ERROR_CHECK("CNVGetConnectionAttribute", error);
fprintf(fp, " status: %s", connectionStatus(status));
error = CNVGetConnectionAttribute(handle, CNVConnectionErrorAttribute, &conn_error);
ERROR_CHECK("CNVGetConnectionAttribute", error);
if (conn_error < 0)
{
fprintf(fp, " error present: %s", CNVGetErrorDescription(conn_error));
}
if (buffered)
{
int nitems, maxitems;
error = CNVGetConnectionAttribute(handle, CNVClientBufferNumberOfItemsAttribute, &nitems);
ERROR_CHECK("CNVGetConnectionAttribute", error);
error = CNVGetConnectionAttribute(handle, CNVClientBufferMaximumItemsAttribute, &maxitems);
ERROR_CHECK("CNVGetConnectionAttribute", error);
fprintf(fp, " Client buffer: %d items (buffer size = %d)", nitems, maxitems);
}
fprintf(fp, "\n");
}
/**
Parses the Sync Session response contained in the parseStruct to retrieve Tper session ID. If the Sync Session response
parameters do not match the comID, extended ComID and host session ID then a failure is returned.
@param[in/out] ParseStruct Structure used to parse received TCG response, contains Sync Session response.
@param[in] ComId Expected ComID that is compared to actual ComID of response
@param[in] ComIdExtension Expected Extended ComID that is compared to actual Extended ComID of response
@param[in] HostSessionId Expected Host Session ID that is compared to actual Host Session ID of response
@param[in/out] TperSessionId Tper Session ID retrieved from the Sync Session response.
**/
TCG_RESULT
EFIAPI
TcgParseSyncSession(
const TCG_PARSE_STRUCT *ParseStruct,
UINT16 ComId,
UINT16 ComIdExtension,
UINT32 HostSessionId,
UINT32 *TperSessionId
)
{
UINT8 MethodStatus;
TCG_PARSE_STRUCT TmpParseStruct;
UINT16 ParseComId;
UINT16 ParseExtComId;
TCG_UID InvokingUID;
TCG_UID MethodUID;
UINT32 RecvHostSessionId;
NULL_CHECK(ParseStruct);
NULL_CHECK(TperSessionId);
CopyMem (&TmpParseStruct, ParseStruct, sizeof(TCG_PARSE_STRUCT));
// verify method status is good
ERROR_CHECK(TcgGetMethodStatus(&TmpParseStruct, &MethodStatus));
METHOD_STATUS_ERROR_CHECK (MethodStatus, TcgResultFailure);
// verify comids
ERROR_CHECK(TcgGetComIds(&TmpParseStruct, &ParseComId, &ParseExtComId));
if ((ComId != ParseComId) || (ComIdExtension != ParseExtComId)) {
DEBUG ((DEBUG_INFO, "unmatched comid (exp: 0x%X recv: 0x%X) or comid extension (exp: 0x%X recv: 0x%X)\n", ComId, ParseComId, ComIdExtension, ParseExtComId));
return TcgResultFailure;
}
ERROR_CHECK(TcgGetNextCall(&TmpParseStruct));
ERROR_CHECK(TcgGetNextTcgUid(&TmpParseStruct, &InvokingUID));
ERROR_CHECK(TcgGetNextTcgUid(&TmpParseStruct, &MethodUID));
ERROR_CHECK(TcgGetNextStartList(&TmpParseStruct));
ERROR_CHECK(TcgGetNextUINT32(&TmpParseStruct, &RecvHostSessionId));
ERROR_CHECK(TcgGetNextUINT32(&TmpParseStruct, TperSessionId));
ERROR_CHECK(TcgGetNextEndList(&TmpParseStruct));
ERROR_CHECK(TcgGetNextEndOfData(&TmpParseStruct));
if (InvokingUID != TCG_UID_SMUID) {
DEBUG ((DEBUG_INFO, "Invoking UID did not match UID_SMUID\n"));
return TcgResultFailure;
}
if (MethodUID != TCG_UID_SM_SYNC_SESSION) {
DEBUG ((DEBUG_INFO, "Method UID did not match UID_SM_SYNC_SESSION\n"));
return TcgResultFailure;
}
if (HostSessionId != RecvHostSessionId) {
DEBUG ((DEBUG_INFO, "unmatched HostSessionId (exp: 0x%X recv: 0x%X)\n", HostSessionId, RecvHostSessionId));
return TcgResultFailure;
}
return TcgResultSuccess;
}
int hscdtd004a_read(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata,
unsigned char *data)
{
unsigned char stat;
tMLError result = ML_SUCCESS;
int status = ML_SUCCESS;
/* Read status reg. to check if data is ready */
result =
MLSLSerialRead(mlsl_handle, pdata->address,
COMPASS_HSCDTD004A_STAT, 1, &stat);
ERROR_CHECK(result);
if (stat & 0x48) {
result =
MLSLSerialRead(mlsl_handle, pdata->address,
COMPASS_HSCDTD004A_DATAX, 6,
(unsigned char *) data);
ERROR_CHECK(result);
status = ML_SUCCESS;
} else if (stat & 0x68) {
status = ML_ERROR_COMPASS_DATA_OVERFLOW;
} else {
status = ML_ERROR_COMPASS_DATA_NOT_READY;
}
/* trigger next measurement read */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
COMPASS_HSCDTD004A_CTRL3, 0x40);
ERROR_CHECK(result);
return status;
}
int hscdtd00xx_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = ML_SUCCESS;
/* Soft reset */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
COMPASS_HSCDTD00XX_CTRL3, 0x80);
ERROR_CHECK(result);
/* Force state; Power mode: active */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
COMPASS_HSCDTD00XX_CTRL1, 0x82);
ERROR_CHECK(result);
/* Data ready enable */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
COMPASS_HSCDTD00XX_CTRL2, 0x08);
ERROR_CHECK(result);
MLOSSleep(1); /* turn-on time */
return result;
}
int ami30x_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = ML_SUCCESS;
/* Set CNTL1 reg to power model active */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
AMI30X_REG_CNTL1,
AMI30X_BIT_CNTL1_PC1|AMI30X_BIT_CNTL1_FS1);
ERROR_CHECK(result);
/* Set CNTL2 reg to DRDY active high and enabled */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
AMI30X_REG_CNTL2,
AMI30X_BIT_CNTL2_DREN |
AMI30X_BIT_CNTL2_DRP);
ERROR_CHECK(result);
/* Set CNTL3 reg to forced measurement period */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
AMI30X_REG_CNTL3, AMI30X_BIT_CNTL3_F0RCE);
return result;
}
int CVxParamDistribution::CompareFrame(int frameNumber)
{
// check if there is no user request to compare
if (m_fileNameCompare.length() < 1) return 0;
// make sure m_bufForRead is allocated
if (!m_bufForCompare) NULLPTR_CHECK(m_bufForCompare = new vx_uint32[m_numBins]);
// reading data from reference file
char fileName[MAX_FILE_NAME_LENGTH]; sprintf(fileName, m_fileNameCompare.c_str(), frameNumber);
FILE * fp = fopen(fileName, m_compareFileIsBinary ? "rb" : "r");
if (!fp) {
ReportError("ERROR: Unable to open: %s\n", fileName);
}
int status = ReadFileIntoBuffer(fp, m_bufForCompare);
fclose(fp);
if (status) ReportError("ERROR: distribution compare reference doesn't have enough data: %s\n", fileName);
// compare and report error if mismatched
vx_uint32 * bufRef = nullptr;
ERROR_CHECK(vxAccessDistribution(m_distribution, (void **)&bufRef, VX_READ_ONLY));
status = memcmp(bufRef, m_bufForCompare, m_numBins * sizeof(vx_uint32)) ? -1 : 0;
ERROR_CHECK(vxCommitDistribution(m_distribution, bufRef));
if (status) {
m_compareCountMismatches++;
printf("ERROR: distribution COMPARE MISMATCHED for %s with frame#%d of %s\n", GetVxObjectName(), frameNumber, fileName);
if (!m_discardCompareErrors) return -1;
}
else {
m_compareCountMatches++;
if (m_verbose) printf("OK: distribution COMPARE MATCHED for %s with frame#%d of %s\n", GetVxObjectName(), frameNumber, fileName);
}
return 0;
}
int ak8975_read(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata, unsigned char *data)
{
unsigned char regs[6];
int result = ML_SUCCESS;
int status = ML_SUCCESS;
mpu_dbg("%s\n", __func__);
result = MLSLSerialRead(mlsl_handle, pdata->address,
AK8975_REG_HXL, 6, regs);
ERROR_CHECK(result);
memcpy(data, ®s[0], 6);
status = ML_SUCCESS;
/*
* trigger next measurement if:
* - stat is non zero;
* - if stat is zero and stat2 is non zero.
* Won't trigger if data is not ready and there was no error.
*/
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
AK8975_REG_CNTL,
AK8975_CNTL_MODE_SINGLE_MEASUREMENT);
ERROR_CHECK(result);
return status;
}
int CVxParamDistribution::ReadFrame(int frameNumber)
{
// check if there is no user request to read
if (m_fileNameRead.length() < 1) return 0;
// for single frame reads, there is no need to read it again
// as it is already read into the object
if (!m_fileNameForReadHasIndex && frameNumber != m_captureFrameStart) {
return 0;
}
// reading data from input file
char fileName[MAX_FILE_NAME_LENGTH]; sprintf(fileName, m_fileNameRead.c_str(), frameNumber);
FILE * fp = fopen(fileName, m_readFileIsBinary ? "rb" : "r");
if (!fp) {
if (frameNumber == m_captureFrameStart) {
ReportError("ERROR: Unable to open: %s\n", fileName);
}
else {
return 1; // end of sequence detected for multiframe sequences
}
}
vx_uint32 * data = nullptr;
ERROR_CHECK(vxAccessDistribution(m_distribution, (void **)&data, VX_WRITE_ONLY));
int status = ReadFileIntoBuffer(fp, data);
ERROR_CHECK(vxCommitDistribution(m_distribution, data));
fclose(fp);
return status;
}
int CVxParamDistribution::WriteFrame(int frameNumber)
{
// check if there is no user request to write
if (m_fileNameWrite.length() < 1) return 0;
// reading data from input file
char fileName[MAX_FILE_NAME_LENGTH]; sprintf(fileName, m_fileNameWrite.c_str(), frameNumber);
FILE * fp = fopen(fileName, m_writeFileIsBinary ? "wb" : "w");
if (!fp) ReportError("ERROR: Unable to create: %s\n", fileName);
vx_uint32 * data = nullptr;
ERROR_CHECK(vxAccessDistribution(m_distribution, (void **)&data, VX_READ_ONLY));
if (m_writeFileIsBinary)
{ // write in BINARY mode
fwrite(data, sizeof(data[0]), m_numBins, fp);
}
else
{ // write in ASCII mode
for (size_t i = 0; i < m_numBins; i++)
fprintf(fp, "%8d\n", data[i]);
}
ERROR_CHECK(vxCommitDistribution(m_distribution, data));
fclose(fp);
return 0;
}
// Update Body
inline void Kinect::updateBody()
{
// Retrieve Body Frame
ComPtr<IBodyFrame> bodyFrame;
const HRESULT ret = bodyFrameReader->AcquireLatestFrame( &bodyFrame );
if( FAILED( ret ) ){
return;
}
// Release Previous Bodies
Concurrency::parallel_for_each( bodies.begin(), bodies.end(), []( IBody*& body ){
SafeRelease( body );
} );
// Retrieve Body Data
ERROR_CHECK( bodyFrame->GetAndRefreshBodyData( static_cast<UINT>( bodies.size() ), &bodies[0] ) );
Concurrency::parallel_for( 0, BODY_COUNT, [&]( const int count ){
const ComPtr<IBody> body = bodies[count];
BOOLEAN tracked;
ERROR_CHECK( body->get_IsTracked( &tracked ) );
if( !tracked ){
return;
}
// Retrieve Tracking ID
UINT64 trackingId;
ERROR_CHECK( body->get_TrackingId( &trackingId ) );
// Registration Tracking ID
ComPtr<IFaceFrameSource> faceFrameSource;
ERROR_CHECK( faceFrameReader[count]->get_FaceFrameSource( &faceFrameSource ) );
ERROR_CHECK( faceFrameSource->put_TrackingId( trackingId ) );
} );
}
bool CVxParamArray::CompareFrameBitwiseExact(size_t numItems, size_t numItemsRef, vx_uint8 * bufItems, int frameNumber, const char * fileName)
{
// bitwise exact compare
size_t numItemsMin = min(numItems, numItemsRef);
size_t numMismatches = 0;
if (numItemsMin > 0) {
void * ptr = nullptr;
vx_size stride = 0;
ERROR_CHECK(vxAccessArrayRange(m_array, 0, numItems, &stride, &ptr, VX_READ_ONLY));
for (size_t i = 0; i < numItems; i++) {
vx_uint8 * item = vxFormatArrayPointer(ptr, i, stride);
if (memcmp(item, bufItems + i * m_itemSize, m_itemSize) != 0) {
numMismatches++;
}
}
ERROR_CHECK(vxCommitArrayRange(m_array, 0, numItems, ptr));
}
numMismatches += max(numItems, numItemsRef) - numItemsMin;
bool mismatchDetected = false;
if (numMismatches > 0) {
printf("ERROR: array COMPARE MISMATCHED %d/%d for %s with frame#%d of %s\n", (int)numMismatches, (int)numItems, GetVxObjectName(), frameNumber, fileName);
mismatchDetected = true;
}
else {
if (m_verbose) printf("OK: array COMPARE MATCHED for %s with frame#%d of %s\n", GetVxObjectName(), frameNumber, fileName);
}
return mismatchDetected;
}
void initialize()
{
createInstance();
// Kinectの設定を初期化する
ERROR_CHECK( kinect->NuiInitialize( NUI_INITIALIZE_FLAG_USES_COLOR | NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX | NUI_INITIALIZE_FLAG_USES_SKELETON ) );
// RGBカメラを初期化する
ERROR_CHECK( kinect->NuiImageStreamOpen( NUI_IMAGE_TYPE_COLOR, CAMERA_RESOLUTION,
0, 2, 0, &imageStreamHandle ) );
// 距離カメラを初期化する
ERROR_CHECK( kinect->NuiImageStreamOpen( NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX, CAMERA_RESOLUTION,
0, 2, 0, &depthStreamHandle ) );
// Nearモード
//ERROR_CHECK( kinect->NuiImageStreamSetImageFrameFlags(
// depthStreamHandle, NUI_IMAGE_STREAM_FLAG_ENABLE_NEAR_MODE ) );
// スケルトンを初期化する
ERROR_CHECK( kinect->NuiSkeletonTrackingEnable( 0, NUI_SKELETON_TRACKING_FLAG_ENABLE_SEATED_SUPPORT ) );
// フレーム更新イベントのハンドルを作成する
streamEvent = ::CreateEvent( 0, TRUE, FALSE, 0 );
ERROR_CHECK( kinect->NuiSetFrameEndEvent( streamEvent, 0 ) );
// 指定した解像度の、画面サイズを取得する
::NuiImageResolutionToSize(CAMERA_RESOLUTION, width, height );
}
int mma8451_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = ML_SUCCESS;
unsigned char reg;
result =
MLSLSerialRead(mlsl_handle, pdata->address, 0x0E, 1, ®);
ERROR_CHECK(result);
/* data rate = 200Hz */
/* Full Scale */
reg &= ~ACCEL_MMA8451_CTRL_MASK;
if (slave->range.mantissa == 4)
reg |= 0x1;
else if (slave->range.mantissa == 8)
reg |= 0x2;
else {
slave->range.mantissa = 2;
reg |= 0x0;
}
slave->range.fraction = 0;
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x0E, reg);
ERROR_CHECK(result);
/* 200Hz + active mode */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x2A, 0x11);
ERROR_CHECK(result);
return result;
}
/**
* Destroys the background thread. This function will block until all tasks are
* completed.
*
* @param thread the thread to destroy.
* @return CUDA_SUCCESS on success,
* CUDA_ERROR_OPERATING_SYSTEM if the thread could not be stopped.
*/
static inline CUresult cuThreadDestroy(CUthread thread) {
// Lock the queue
ERROR_CHECK(pthread_mutex_lock(&thread->mutex));
// Place a NULL task on the queue to signal the thread to exit the main loop
CU_ERROR_CHECK(cuTaskQueuePush(&thread->queue, NULL));
// Unlock the queue
ERROR_CHECK(pthread_mutex_unlock(&thread->mutex));
// Signal to the thread that there are tasks waiting
ERROR_CHECK(pthread_cond_signal(&thread->nonEmpty));
// Wait for the thread to exit
ERROR_CHECK(pthread_join(thread->thread, (void **)&thread));
// Destroy the queue
cuTaskQueueDestroy(&thread->queue);
// Copy the thread error status
CUresult error = thread->error;
// Free the thread object
free(thread);
// Return the thread error status
return error;
}
void KinectApp::initialize() {
// デフォルトのKinectを取得
ERROR_CHECK(::GetDefaultKinectSensor(&kinect));
ERROR_CHECK(kinect->Open());
// 座標変換インターフェースを取得
ERROR_CHECK(kinect->get_CoordinateMapper(&coordinateMapper));
// フレームの初期化
initializeColorFrame();
initializeDepthFrame();
WindowName[0] = "color";
WindowName[1] = "banana";
WindowName[2] = "depth";
WindowName[3] = "thresh_depth";
WindowName[4] = "d_depth";
for (int i = 0; i < 5; i++) {
cv::namedWindow(WindowName[i]);
}
cv::moveWindow(WindowName[0], 700, 0);
cv::moveWindow(WindowName[1], 1200, 0);
cv::moveWindow(WindowName[2], 700, 400);
cv::moveWindow(WindowName[3], 1100, 400);
cv::moveWindow(WindowName[4], 1500, 400);
cv::FileStorage fs("../backImg.xml", cv::FileStorage::READ);
if (fs.isOpened()) {
std::cout << "backImg exist." << std::endl;
fs["backImg"] >> backImg;
}
int ami30x_read(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata, unsigned char *data)
{
unsigned char stat;
int result = ML_SUCCESS;
/* Read status reg and check if data ready (DRDY) */
result =
MLSLSerialRead(mlsl_handle, pdata->address, AMI30X_REG_STAT1,
1, &stat);
ERROR_CHECK(result);
if (stat & 0x40) {
result =
MLSLSerialRead(mlsl_handle, pdata->address,
AMI30X_REG_DATAX, 6,
(unsigned char *) data);
ERROR_CHECK(result);
/* start another measurement */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
AMI30X_REG_CNTL3,
AMI30X_BIT_CNTL3_F0RCE);
ERROR_CHECK(result);
return ML_SUCCESS;
}
return ML_ERROR_COMPASS_DATA_NOT_READY;
}
/**
* @brief resume the device in the proper power state given the configuration
* chosen.
*
* @param mlsl_handle
* the handle to the serial channel the device is connected to.
* @param slave
* a pointer to the slave descriptor data structure.
* @param pdata
* a pointer to the slave platform data.
*
* @return INV_SUCCESS if successful or a non-zero error code.
*/
static int mma8450_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = INV_SUCCESS;
struct mma8450_private_data *private_data = pdata->private_data;
/* Full Scale */
if (private_data->resume.fsr == 4000)
slave->range.mantissa = 4;
else if (private_data->resume.fsr == 8000)
slave->range.mantissa = 8;
else
slave->range.mantissa = 2;
slave->range.fraction = 0;
ERROR_CHECK(result);
result = inv_serial_single_write(mlsl_handle, pdata->address,
ACCEL_MMA8450_CTRL_REG1, 0);
ERROR_CHECK(result);
if (private_data->resume.ctrl_reg1) {
result = inv_serial_single_write(mlsl_handle, pdata->address,
ACCEL_MMA8450_CTRL_REG1,
private_data->resume.ctrl_reg1);
ERROR_CHECK(result);
}
result = mma8450_set_irq(mlsl_handle, pdata,
&private_data->resume,
TRUE, private_data->resume.irq_type);
ERROR_CHECK(result);
return result;
}
// read image
int ReadImage(vx_image image, vx_rectangle_t * rectFull, FILE * fp)
{
// get number of planes, image format, and pixel type
vx_df_image format = VX_DF_IMAGE_VIRT;
vx_size num_planes = 0;
ERROR_CHECK(vxQueryImage(image, VX_IMAGE_ATTRIBUTE_FORMAT, &format, sizeof(format)));
ERROR_CHECK(vxQueryImage(image, VX_IMAGE_ATTRIBUTE_PLANES, &num_planes, sizeof(num_planes)));
// read all image planes into vx_image and check if EOF has occured while reading
bool eofDetected = false;
for (vx_uint32 plane = 0; plane < (vx_uint32)num_planes; plane++){
vx_imagepatch_addressing_t addr;
vx_uint8 * src = NULL;
ERROR_CHECK(vxAccessImagePatch(image, rectFull, plane, &addr, (void **)&src, VX_WRITE_ONLY));
vx_size width = (addr.dim_x * addr.scale_x) / VX_SCALE_UNITY;
vx_size width_in_bytes = (format == VX_DF_IMAGE_U1_AMD) ? ((width + 7) >> 3) : (width * addr.stride_x);
for (vx_uint32 y = 0; y < addr.dim_y; y += addr.step_y){
vx_uint8 *srcp = (vx_uint8 *)vxFormatImagePatchAddress2d(src, 0, y, &addr);
if (fread(srcp, 1, width_in_bytes, fp) != width_in_bytes) {
eofDetected = true;
break;
}
}
ERROR_CHECK(vxCommitImagePatch(image, rectFull, plane, &addr, src));
}
// return 1 if EOF detected, other 0
return eofDetected ? 1 : 0;
}
static int mantis_set_fsr(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct mantis_config *config, long apply, long fsr)
{
unsigned char fsr_mask;
int result;
if (fsr <= 2000) {
config->fsr = 2000;
fsr_mask = 0x00;
} else if (fsr <= 4000) {
config->fsr = 4000;
fsr_mask = 0x08;
} else if (fsr <= 8000) {
config->fsr = 8000;
fsr_mask = 0x10;
} else { /* fsr = [8001, oo) */
config->fsr = 16000;
fsr_mask = 0x18;
}
if (apply) {
unsigned char reg;
result = inv_serial_read(mlsl_handle, pdata->address,
MPUREG_ACCEL_CONFIG, 1, ®);
ERROR_CHECK(result);
result = inv_serial_single_write(mlsl_handle, pdata->address,
MPUREG_ACCEL_CONFIG,
reg | fsr_mask);
ERROR_CHECK(result);
MPL_LOGV("FSR: %d\n", config->fsr);
}
return INV_SUCCESS;
}
static int bma250_set_irq(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct bma250_config *config,
int apply,
long irq_type)
{
unsigned char irq_bits = 0;
int result = ML_SUCCESS;
return ML_SUCCESS;
if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION)
return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
config->irq_type = (unsigned char)irq_type;
if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
irq_bits = 0x20;
config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
} else {
irq_bits = 0x00;
config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
}
config->int_reg &= ACCEL_BOSCH_INT_MASK_IRQ;
config->int_reg |= irq_bits;
if (apply) {
if (!config->power_mode) {
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BMA250_REG_SOFT_RESET,
0xB6);
ERROR_CHECK(result);
MLOSSleep(1);
}
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BOSCH_CTRL_REG, config->ctrl_reg);
ERROR_CHECK(result);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BOSCH_INT_REG, config->int_reg);
ERROR_CHECK(result);
if (!config->power_mode) {
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BOSCH_PWR_REG, 0x80);
ERROR_CHECK(result);
MLOSSleep(1);
} else {
result = set_normal_mode(mlsl_handle, pdata);
ERROR_CHECK(result);
}
}
return result;
}
/**
* @brief resume the device in the proper power state given the configuration
* chosen.
*
* @param mlsl_handle
* the handle to the serial channel the device is connected to.
* @param slave
* a pointer to the slave descriptor data structure.
* @param pdata
* a pointer to the slave platform data.
*
* @return ML_SUCCESS if successful or a non-zero error code.
*/
static int bma250_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
struct bma250_config *resume_config =
&((struct bma250_private_data *)pdata->private_data)->resume;
if (pdata->check_sleep_status && pdata->vote_sleep_status) {
mutex_lock(&bma250_power_lock);
if (pdata->check_sleep_status() == BMA250_SLEEP) {
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BMA250_SOFTRESET_REG,
BMA250_SOFTRESET_MASK);
ERROR_CHECK_MUTEX(result, bma250_power_lock);
/* 4 ms needed for to be stable after reset. */
MLOSSleep(4);
}
result = bma250_set_odr(mlsl_handle, pdata, resume_config,
TRUE, resume_config->odr);
ERROR_CHECK_MUTEX(result, bma250_power_lock);
result = bma250_set_fsr(mlsl_handle, pdata, resume_config,
TRUE, resume_config->fsr);
ERROR_CHECK_MUTEX(result, bma250_power_lock);
if (pdata->check_sleep_status() == BMA250_SLEEP) {
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BMA250_PWR_REG,
BMA250_PWR_AWAKE_MASK);
ERROR_CHECK_MUTEX(result, bma250_power_lock);
}
pdata->vote_sleep_status(BMA250_SLAVE2, BMA250_AWAKE);
mutex_unlock(&bma250_power_lock);
} else {
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BMA250_SOFTRESET_REG, BMA250_SOFTRESET_MASK);
ERROR_CHECK(result);
/* 4 ms needed for to be stable after reset. */
MLOSSleep(4);
result = bma250_set_odr(mlsl_handle, pdata, resume_config,
TRUE, resume_config->odr);
ERROR_CHECK(result);
result = bma250_set_fsr(mlsl_handle, pdata, resume_config,
TRUE, resume_config->fsr);
ERROR_CHECK(result);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BMA250_PWR_REG, BMA250_PWR_AWAKE_MASK);
ERROR_CHECK(result);
}
return result;
}
/*
* @brief メソッドKinect::setDepthImage(Mat& Mat_image).デプス画像を取得するメソッド(c57)
* @param cv::Mat& Mat_image
* @return pcl::PointCloud<pcl::PointXYZ>::Ptr points
*/
pcl::PointCloud<pcl::PointXYZRGB>::Ptr Kinect::getPointCloud(Mat& Mat_image)
{
try{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr points(new pcl::PointCloud<pcl::PointXYZRGB>()); //ポイントクラウド保存用(c57)
points->width = width;
points->height = height;
Mat_image = Mat(height, width, CV_8UC1, Scalar(0)); //距離画像の準備
//距離カメラのフレームデータを取得
NUI_IMAGE_FRAME depthFrame = { 0 };
ERROR_CHECK(kinect->NuiImageStreamGetNextFrame(depthStreamHandle, 0, &depthFrame));
//距離データを取得する
NUI_LOCKED_RECT depthData = { 0 };
depthFrame.pFrameTexture->LockRect(0, &depthData, 0, 0);
USHORT* depth = (USHORT*)depthData.pBits;
for (int i = 0; i < (depthData.size / sizeof(USHORT)); ++i){
USHORT distance = ::NuiDepthPixelToDepth(depth[i]);
LONG depthX = i % width;
LONG depthY = i / width;
LONG colorX = depthX;
LONG colorY = depthY;
// 距離カメラの座標を、RGBカメラの座標に変換する
kinect->NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution(CAMERA_RESOLUTION, CAMERA_RESOLUTION, 0, depthX, depthY, 0, &colorX, &colorY);
// 距離画像作成
Mat_image.at<UCHAR>(colorY, colorX) = distance / 8192.0 * 255.0;
// ポイントクラウド
Vector4 real = NuiTransformDepthImageToSkeleton(depthX, depthY, distance, CAMERA_RESOLUTION);
pcl::PointXYZRGB point;
point.x = real.x;
point.y = real.y;
point.z = real.z; //PCLは右手系で表現されるため-をつけている
//テクスチャ
Vec4b color = image.at<Vec4b>(colorY, colorX);
point.r = color[2];
point.g = color[1];
point.b = color[0];
points->push_back(point);
}
cloud = points;
//フレームデータを開放する(c58)
ERROR_CHECK(kinect->NuiImageStreamReleaseFrame(depthStreamHandle, &depthFrame));
}
catch (exception& ex){
cout << ex.what() << endl;
}
return cloud;
}
void release()
{
// Image Frameの解放
ERROR_CHECK( NuiFusionReleaseImageFrame( depthImageFrame ) );
ERROR_CHECK( NuiFusionReleaseImageFrame( smoothDepthImageFrame ) );
ERROR_CHECK( NuiFusionReleaseImageFrame( colorImageFrame ) );
ERROR_CHECK( NuiFusionReleaseImageFrame( pointCloudImageFrame ) );
ERROR_CHECK( NuiFusionReleaseImageFrame( surfaceImageFrame ) );
/*ERROR_CHECK( NuiFusionReleaseImageFrame( normalImageFrame ) );*/
}
/**
Creates ComPacket with a Method call that sets the PIN column for the row specified.
This assumes a start session has already been opened with the desired SP.
@param [in/out] CreateStruct Structure used to add method call.
@param [in/out] Size Describes the size of the entire ComPacket (header and payload). Filled out by function.
@param [in] ComId ComID for the ComPacket
@param [in] ComIdExtension Extended ComID for the ComPacket
@param [in] TperSession Tper Session ID for the Packet
@param [in] HostSession Host Session ID for the Packet
@param [in] SidRow UID of row of current SP to set PIN column
@param [in] Password value of PIN to set
@param [in] PasswordSize Size of PIN
**/
TCG_RESULT
EFIAPI
TcgCreateSetCPin(
TCG_CREATE_STRUCT *CreateStruct,
UINT32 *Size,
UINT16 ComId,
UINT16 ComIdExtension,
UINT32 TperSession,
UINT32 HostSession,
TCG_UID SidRow,
const VOID *Password,
UINT32 PasswordSize
)
{
// set new SID Password
ERROR_CHECK(TcgStartComPacket(CreateStruct, ComId, ComIdExtension));
ERROR_CHECK(TcgStartPacket(CreateStruct, TperSession, HostSession, 0x0, 0x0, 0x0));
ERROR_CHECK(TcgStartSubPacket(CreateStruct, 0x0));
ERROR_CHECK(TcgStartMethodSet(CreateStruct, SidRow, 0x03)); // "PIN"
ERROR_CHECK(TcgAddByteSequence(CreateStruct, Password, PasswordSize, FALSE));
ERROR_CHECK(TcgEndMethodSet(CreateStruct));
ERROR_CHECK(TcgEndSubPacket(CreateStruct));
ERROR_CHECK(TcgEndPacket(CreateStruct));
ERROR_CHECK(TcgEndComPacket(CreateStruct, Size));
return TcgResultSuccess;
}
// Initialize Body
inline void Kinect::initializeBody()
{
// Open Body Reader
ComPtr<IBodyFrameSource> bodyFrameSource;
ERROR_CHECK( kinect->get_BodyFrameSource( &bodyFrameSource ) );
ERROR_CHECK( bodyFrameSource->OpenReader( &bodyFrameReader ) );
// Initialize Body Buffer
Concurrency::parallel_for_each( bodies.begin(), bodies.end(), []( IBody*& body ){
SafeRelease( body );
} );
}
/**
Set end method.
@param CreateStruct Input create structure.
**/
TCG_RESULT
EFIAPI
TcgEndMethodSet(
TCG_CREATE_STRUCT *CreateStruct
)
{
ERROR_CHECK(TcgAddEndName(CreateStruct));
ERROR_CHECK(TcgAddEndList(CreateStruct));
ERROR_CHECK(TcgAddEndName(CreateStruct));
ERROR_CHECK(TcgEndParameters(CreateStruct));
ERROR_CHECK(TcgEndMethodCall(CreateStruct));
return TcgResultSuccess;
}
int CVxParamArray::Finalize()
{
// get attributes
ERROR_CHECK(vxQueryArray(m_array, VX_ARRAY_ATTRIBUTE_ITEMSIZE, &m_itemSize, sizeof(m_itemSize)));
ERROR_CHECK(vxQueryArray(m_array, VX_ARRAY_ATTRIBUTE_CAPACITY, &m_capacity, sizeof(m_capacity)));
// process user requested directives
if (m_useSyncOpenCLWriteDirective) {
ERROR_CHECK_AND_WARN(vxDirective((vx_reference)m_array, VX_DIRECTIVE_AMD_COPY_TO_OPENCL), VX_ERROR_NOT_ALLOCATED);
}
return 0;
}
static int ak8975_get_config(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata,
struct ext_slave_config *data)
{
struct ak8975_private_data *private_data = pdata->private_data;
int result;
if (!data->data)
return INV_ERROR_INVALID_PARAMETER;
switch (data->key) {
case MPU_SLAVE_READ_REGISTERS:
{
unsigned char *serial_data = (unsigned char *)data->data;
result = inv_serial_read(mlsl_handle, pdata->address,
serial_data[0],
data->len - 1,
&serial_data[1]);
ERROR_CHECK(result);
break;
}
case MPU_SLAVE_READ_SCALE:
{
unsigned char *serial_data = (unsigned char *)data->data;
serial_data[0] = private_data->init.asa[0];
serial_data[1] = private_data->init.asa[1];
serial_data[2] = private_data->init.asa[2];
result = INV_SUCCESS;
ERROR_CHECK(result);
break;
}
case MPU_SLAVE_CONFIG_ODR_SUSPEND:
(*(unsigned long *)data->data) = 0;
break;
case MPU_SLAVE_CONFIG_ODR_RESUME:
(*(unsigned long *)data->data) = 8000;
break;
case MPU_SLAVE_CONFIG_FSR_SUSPEND:
case MPU_SLAVE_CONFIG_FSR_RESUME:
case MPU_SLAVE_CONFIG_MOT_THS:
case MPU_SLAVE_CONFIG_NMOT_THS:
case MPU_SLAVE_CONFIG_MOT_DUR:
case MPU_SLAVE_CONFIG_NMOT_DUR:
case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
case MPU_SLAVE_CONFIG_IRQ_RESUME:
default:
return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
};
return INV_SUCCESS;
}