bool HandlerBroker::connect(MyThread * TID, const wxString& sDevName,const std::string& strDevInit, DEVID *id){
DevDesc * pNewDev;
DevDesc* pDevLocker = getDev(sDevName);
if (!pDevLocker) return false;
std::string strUniqueDev = std::string(sDevName.utf8_str())+ pDevLocker->makeUniqueDev(strDevInit);
{
wxMutexLocker ml1(mtxDevHandlesTreeEdit);
std::map<std::string, CountedDevHandle*>::iterator itDevH = mapDevHandlesInUse.find(strUniqueDev);
if(itDevH != mapDevHandlesInUse.end()){
pNewDev = itDevH->second->dev;
}else{
pNewDev = pDevLocker->clone();
if (!(pNewDev->connect(strDevInit)))return false;
wxMutexLocker ml2(mtxBusLockTreeEdit);
std::string strBusLock= pNewDev->makeBusLock(strDevInit);
pNewDev->itBusLocker = mapBusLockers.find(strBusLock);
if(pNewDev->itBusLocker == mapBusLockers.end())
pNewDev->itBusLocker = mapBusLockers.insert(std::make_pair(strBusLock, new mtxCont)).first;
++(*(pNewDev->itBusLocker->second));
itDevH = mapDevHandlesInUse.insert( std::make_pair( strUniqueDev, new CountedDevHandle(pNewDev)) ).first;
itDevH->second->dev->itDevH = itDevH;
}
++(*(itDevH->second));
}
wxMutexLocker ml0(mtxDevTreeEdit);
std::map<MyThread *, ThrDevs*>::iterator it=mapThreadDevsInUse.find(TID);
if (it == mapThreadDevsInUse.end()) it = mapThreadDevsInUse.insert(std::pair<MyThread * , ThrDevs*>(TID, new ThrDevs(TID)) ).first;
*id = it->second->insert(pNewDev);
return true;
};
bool HandlerBroker::attribute(MyThread * TID, DEVID id, Attr *pA){
DevDesc * pDev = getDev(TID, id);
if(!pDev) return false;
wxMutexLocker ml(*(pDev->itBusLocker->second->pmtx));
//wxMutexLocker ml(pDev->locker);
return pDev->attribute(pA);
};
void FAXAdpDisconnectCnf(SPPCallbackParms *Parms)
{
bt_fax_disconnect_cnf_struct *bt_fax_disconnect_cnf;
SPP_DEV_OS *vp_ptr = 0;
bt_fax_disconnect_cnf =
(bt_fax_disconnect_cnf_struct*) construct_local_para(sizeof(bt_fax_disconnect_cnf_struct), TD_UL);
bt_fax_disconnect_cnf->cid = Parms->p.fax_disconnect_cnf.cid;
bt_fax_disconnect_cnf->port = Parms->p.fax_disconnect_cnf.port;
bt_fax_disconnect_cnf->lap = Parms->p.fax_disconnect_cnf.lap;
bt_fax_disconnect_cnf->uap = Parms->p.fax_disconnect_cnf.uap;
bt_fax_disconnect_cnf->nap = Parms->p.fax_disconnect_cnf.nap;
bt_fax_disconnect_cnf->result = Parms->p.fax_disconnect_cnf.result;
SPPOS_sendMsg(MSG_ID_BT_FAX_DISCONNECT_CNF, Parms->dstMod, BT_APP_SAP, (local_para_struct*) bt_fax_disconnect_cnf, NULL);
// Deactivate if this is a Client port
vp_ptr = getDev(Parms->p.fax_disconnect_cnf.port);
if ((vp_ptr->sppDev.portType & SPP_CLIENT_PORT) && (SPP_DISCONNECT_SUCCESS == Parms->p.fax_disconnect_cnf.result))
{
btmtk_fax_deactivate_req_struct *msg =
(btmtk_fax_deactivate_req_struct*) construct_local_para(sizeof(btmtk_fax_deactivate_req_struct), TD_UL);
msg->port = Parms->p.fax_disconnect_cnf.port;
SPPOS_sendMsg(MSG_ID_BT_FAX_DEACTIVATE_REQ, MOD_BT, 0, (local_para_struct*) msg, NULL);
}
}
bool HandlerBroker::write(MyThread * TID, DEVID id, const std::string& str){
DevDesc * pDev= getDev(TID, id);
if(!pDev ) return false;
//wxMutexLocker ml(pDev->locker);
wxMutexLocker ml(*(pDev->itBusLocker->second->pmtx));
return pDev->write(str);
};
bool HandlerBroker::read(MyThread * TID, DEVID id, std::string* pstr, int count){
DevDesc * pDev= getDev(TID, id);
if(!pDev ) return false;
wxMutexLocker ml(*(pDev->itBusLocker->second->pmtx));
//wxMutexLocker mlDev(pDev->locker);
bool rc = pDev->read(pstr, count);
return rc;
};
void UCamRad::updateLensParams(const int height, const int width)
{
double f, cx, cy, k1, k2, s;
CvSize imageSize;
bool redo;
UMatrix4 intrinsic, distortion;
//
if (varIntrinsic != NULL)
{
f = ((varIntrinsic->getValued(0) + varIntrinsic->getDouble(4))/2.0);
cx = varIntrinsic->getDouble(2);
cy = varIntrinsic->getDouble(5);
k1 = varDistortion->getDouble(0)/f;
k2 = varDistortion->getDouble(0)/(f*f*f);
s = getDev()->getPixelSize();
redo = (par.getK1() != k1 or
par.getK2() != k2 or
par.getFocalLength() != f or
par.getHx() != cx or
par.getHy() != cy);
if (redo)
par.setCameraParameters(cx, cy, k1, k2, f, s);
//
//
imageSize = cvSize(width, height);
if (imageSize.width > 0 and imageSize.height > 0)
{ // and image size is available, so continue
if (mapx == NULL)
{
mapx = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
mapy = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
redo = true;
}
else if (imageSize.width != mapx->width)
{
cvReleaseImage(&mapx);
cvReleaseImage(&mapy);
mapx = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
mapy = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
redo = true;
}
if (redo)
{
intrinsic.setMat(3, 3, varIntrinsic->getValuesd());
distortion.setMat(5, 1, varDistortion->getValuesd());
#if (CV_MAJOR_VERSION >= 1)
cvInitUndistortMap( intrinsic.cvMat(), distortion.cvMat(), mapx, mapy );
#else
printf("UCamRad::updateLensParams: used cvInitUndistortMap(...) is incompatible with openCV version\n");
#endif
}
}
}
}
void FAXAdpDisconnectInd(SPPCallbackParms *Parms)
{
bt_fax_disconnect_ind_struct *msg2 =
(bt_fax_disconnect_ind_struct*) construct_local_para(sizeof(bt_fax_disconnect_ind_struct), TD_UL);
SPP_DEV_OS *vp_ptr = 0;
msg2->port = Parms->p.fax_disconnect_ind.port;
SPPOS_sendMsg(MSG_ID_BT_FAX_DISCONNECT_IND, Parms->dstMod, BT_APP_SAP, (local_para_struct*) msg2, NULL);
// Deactivate if this is a Client port
vp_ptr = getDev(Parms->p.fax_disconnect_cnf.port);
if (vp_ptr->sppDev.portType & SPP_CLIENT_PORT)
{
btmtk_fax_deactivate_req_struct *msg =
(btmtk_fax_deactivate_req_struct*) construct_local_para(sizeof(btmtk_fax_deactivate_req_struct), TD_UL);
msg->port = Parms->p.fax_disconnect_cnf.port;
SPPOS_sendMsg(MSG_ID_BT_FAX_DEACTIVATE_REQ, MOD_BT, 0, (local_para_struct*) msg, NULL);
}
}
/*
* Start interfacing the IMU using rs232
*/
int IMUManager::init(bool imu_rec, bool sim_snd) {
while(devid<0 && imu_rec){
devid = getDev();
if(devid == -1){sleep(5);}
}
if(RS232_OpenComport(devid, BAUD) && imu_rec) {
logError("Could not connect to comport ");
return 1;
}else{
conn = true;
}
// Used for testing simulator!!
if ((simsock = socket(AF_INET,SOCK_DGRAM,0)) < 0){
logError("socket error");
logError(strerror(errno));
return 1;
}
memset((char *)&simAddr, 0, sizeof(simAddr));
inet_pton(AF_INET, SIM_ADDR, &(simAddr.sin_addr));
simAddr.sin_port = htons(SIM_PORTEN);
if(imu_rec){
std::thread t1 (&IMUManager::readImu, this);
std::thread t2 (&IMUManager::getControl , this);
t1.detach();
t2.detach();
}
if(sim_snd){
std::thread t3 (&IMUManager::sendData, this);
t3.detach();
}
return -1;
}
bool UCamRad::removeRadialError(UImage * source, UImage * destination)
{ // remove radial error and move result to destination image
bool result = false;
const int width = source->getWidth();
const int height = source->getHeight();
// copy image meta data to destination
if (lensParChanged() or mapx == NULL or height != (int)getDev()->getHeight())
updateLensParams(height, width);
if (par.isValid() and mapx != NULL)
{ // take one plane at a time
switch (source->getColorType())
{
case PIX_PLANES_YUV420:
destination->copyMeta(source, true);
result = removeRadialErrorOnePlane(source->getYline(0),
destination->getYline(0),
height, width, par.getPixelSize(), false);
if (result)
result = removeRadialErrorOnePlane(source->getUline(0),
destination->getUline(0),
height/2, width/2, par.getPixelSize() * 2.0, true);
if (result)
result = removeRadialErrorOnePlane(source->getVline(0),
destination->getVline(0),
height/2, width/2, par.getPixelSize() * 2.0, true);
break;
/* case PIX_PLANES_BW:
result = removeRadialErrorOnePlane(source->getYline(0),
destination->getYline(0),
height, width, par.getPixelSize(), false);
break;*/
default: // all other 3-plane images
// can not rectify a bayer coded image, nor a YUV420
#if (CV_MAJOR_VERSION >= 1)
if (source->getColorType() == PIX_PLANES_BGGR or
source->getColorType() == PIX_PLANES_RGGB or
source->getColorType() == PIX_PLANES_GBRG or
source->getColorType() == PIX_PLANES_GRBG or
source->getColorType() == PIX_PLANES_YUV420)
source->toBGR(source);
destination->copyMeta(source, true);
cvRemap(source->cvArr(), destination->cvArr(), mapx, mapy ); // undistort image
result = true;
#else
result = false;
#endif
//result = removeRadialErrorPixels(source->getLine(0),
// destination->getLine(0),
// height, width, par.getPixelSize());
break;
}
}
// debug
/*
printf("UCamRad::removeRadialError: raw - pixSize %f, image height %d\n",
par.getPixelSize(), source->getHeight());
par.print("cam-par");
if (source == destination)
printf("UCamRad::removeRadialError: raw Source == destnation!\n");
*/
// debug end
if (result)
{
destination->radialErrorRemoved = true;
destination->updated();
}
return result;
}
void *CLInterface(void *data)
{
printf(RED_BEGIN"#Ready to operation ('h' for help)#\n"COLOR_END);
(void)data;
OCStackResult ret;
char query[MAX_LINE] = {0,};
const char prompt[] = BOLD_BEGIN"IoTivity-DP#"COLOR_END" ";
const char* helpmsg[6] = {
GREEN_BEGIN"# h (or help) : show help message"COLOR_END,
GREEN_BEGIN"# dd (DP device discovery) : discover Direct-Pairing devices"COLOR_END,
GREEN_BEGIN"# dp (start Direct-Pairing) : negotiate DP method & start Direct-Pairing"COLOR_END,
GREEN_BEGIN"# sd (send data) : send data to device"COLOR_END,
GREEN_BEGIN"# ll (list all device) : list all discovered/paired devices"COLOR_END,
GREEN_BEGIN"# q (quit) : quit test"COLOR_END,
};
for (size_t i=0; i<(sizeof(helpmsg)/sizeof(char*)); i++)
{
fprintf(stderr, "%s\n", helpmsg[i]);
}
printf("\n");
// cli
for (;;)
{
const char *input = readline(prompt, NULL);
if (!input) {
continue;
}
strncpy(query, input, MAX_LINE);
if (!strlen(query))
{
continue;
}
else if (!strcmp(query, "h") || !strcmp(query, "help"))
{
for (size_t i=0; i<(sizeof(helpmsg)/sizeof(char*)); i++)
{
fprintf(stderr, "%s\n", helpmsg[i]);
}
continue;
}
else
{
if (!strcmp(query, "dd"))
{
OIC_LOG(INFO, TAG, "- Direct-Pairing device discovery -");
ret = DirectPairingDiscovery();
if (OC_STACK_OK != ret)
{
OIC_LOG(ERROR, TAG, "Error in DirectPairingDiscovery()");
}
}
else if (!strcmp(query, "dp"))
{
OIC_LOG(INFO, TAG, "- Negotiate DP method & Start Direct-Pairing -");
printf("\n * List of discovered device\n");
printList(discoveredDevs);
// target peer
OCDPDev_t *peer = NULL;
long peerIdx;
input = readline(" > Enter Peer Device Number to initiate Direct-Pairing: ", NULL);
if (!input || !strlen(input))
{
continue;
}
char *ptr;
peerIdx = strtol(input, &ptr, 10);
peer = getDev(discoveredDevs, (uint32_t)peerIdx);
if (NULL == peer)
{
OIC_LOG(ERROR, TAG, "Not found the peer in discovered list");
continue;
}
// get pairing method
long pmIdx;
OCPrm_t pmSel = DP_NOT_ALLOWED;
if (false == printPairingMethod(peer))
{
OIC_LOG(ERROR, TAG, "Target does not support the Direct-Pairing");
continue;
}
input = readline(" > Enter pairing method: ", NULL);
if (!input || !strlen(input))
{
continue;
}
pmIdx = strtol(input, &ptr, 10);
printf("\n");
if (0 >= pmIdx || peer->prmLen+1 < (size_t)pmIdx)
{
OIC_LOG(ERROR, TAG, "Invalid mode selection");
continue;
}
pmSel = peer->prm[pmIdx-1];
// get PIN
char pinNumber[DP_PIN_LENGTH+1];
input = readline(" > Enter PIN Number for authentication (ex - '00000000' [8 digit] ): ", NULL);
if (!input || DP_PIN_LENGTH != strlen(input))
{
OIC_LOG(ERROR, TAG, "Invalid PIN");
continue;
}
sscanf(input, "%9s", pinNumber);
printf("\n");
ret = DoDirectPairing(peer, pmSel, pinNumber);
if (OC_STACK_OK != ret)
{
OIC_LOG(ERROR, TAG, "Error in DoDirectPairing()");
}
}
else if (!strcmp(query, "sd"))
{
OIC_LOG(INFO, TAG, "- Send data(GET Request) to device(led server) -");
//pairedDevs = OCGetDirectPairedDevices();
//printList(pairedDevs);
printList(discoveredDevs);
// target peer
OCDPDev_t *peer = NULL;
long peerIdx;
input = readline(" > Enter Peer Device Number to initiate Direct-Pairing: ", NULL);
if (!input || !strlen(input))
{
continue;
}
char *ptr;
peerIdx = strtol(input, &ptr, 10);
//peer = getDev(pairedDevs, peerIdx);
peer = getDev(discoveredDevs, (uint32_t)peerIdx);
if (NULL == peer)
{
OIC_LOG(ERROR, TAG, "Not found the peer in discovered list");
continue;
}
// send Get Req
ret = SendGetRequest(peer);
if (OC_STACK_OK != ret)
{
OIC_LOG(ERROR, TAG, "Error in SendGetRequest()");
}
}
else if (!strcmp(query, "ll"))
{
OIC_LOG(INFO, TAG, "- List all discovered and paired devices) -");
printf(" > List of discovered devices\n");
printList(discoveredDevs);
printf("\n");
printf(" > List of paired devices\n");
pairedDevs = OCGetDirectPairedDevices();
printList(pairedDevs);
printf("\n");
}
else if (!strcmp(query, "q"))
{
printf("QUIT\n");
gQuitFlag = 1;
break;
}
}
}
return 0;
}
