MOOS 0.2375
|
00001 00002 // 00003 // MOOS - Mission Oriented Operating Suite 00004 // 00005 // A suit of Applications and Libraries for Mobile Robotics Research 00006 // Copyright (C) 2001-2005 Massachusetts Institute of Technology and 00007 // Oxford University. 00008 // 00009 // This software was written by Paul Newman and others 00010 // at MIT 2001-2002 and Oxford University 2003-2005. 00011 // email: pnewman@robots.ox.ac.uk. 00012 // 00013 // This file is part of a MOOS Instrument. 00014 // 00015 // This program is free software; you can redistribute it and/or 00016 // modify it under the terms of the GNU General Public License as 00017 // published by the Free Software Foundation; either version 2 of the 00018 // License, or (at your option) any later version. 00019 // 00020 // This program is distributed in the hope that it will be useful, 00021 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00022 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00023 // General Public License for more details. 00024 // 00025 // You should have received a copy of the GNU General Public License 00026 // along with this program; if not, write to the Free Software 00027 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 00028 // 02111-1307, USA. 00029 // 00031 00032 //OK - this code compiles to make a matlab interface to MOOS 00033 //normal users should define MATLAB_MEX_FILE at compile time. 00034 //users who have want to extend the interface and use the VNL 00035 //numerics library should define HAVE_VNL. 00036 //the compile should also include mexVNLHelpers.cpp 00037 //as always link against MOOSLIB and MOOSGenLib 00038 //if you aren't using the CMake build system supplied with MOOS 00039 //make sure mex.h is in your include path - see the matlab documentation 00040 // 00041 // copyright Paul Newman, University of Oxford 2005 00042 00043 00044 #ifdef _WIN32 00045 #pragma warning(disable : 4786) 00046 #endif 00047 00048 00049 #include <iostream> 00050 #include <math.h> 00051 #include "mexVNLHelpers.h" 00052 #include <string> 00053 #include <map> 00054 #include <fstream> 00055 #include <MOOSLIB/MOOSLib.h> 00056 00057 extern "C" { 00058 #include "mex.h" 00059 } 00060 00061 00062 #ifdef MATLAB_MEX_FILE 00063 #define MOOSTrace mexPrintf 00064 #endif 00065 00066 00067 //this should deal with the new matlab API 00068 //in versions equal or younger than 7.3. 00069 #ifndef MX_API_VER 00070 #define DIM_TYPE int 00071 #else 00072 #if MX_API_VER>=0x07030000 00073 #define DIM_TYPE mwSize 00074 #else 00075 #warning MX_API_VER 00076 #define DIM_TYPE int 00077 #endif 00078 #endif 00079 00080 00082 #ifdef _WIN32 00083 #include <MOOSGenLib/MOOSNTSerialPort.h> 00084 CMOOSNTSerialPort gPort; 00085 #else 00086 #include <MOOSGenLib/MOOSLinuxSerialPort.h> 00087 CMOOSLinuxSerialPort gPort; 00088 #endif 00089 00090 00091 //some file scope variables - these stay resident 00092 //in matlab's memory 00094 CProcessConfigReader gConfigReader; 00095 00096 //good to go? 00097 bool bInitialised= false; 00098 00099 //MOOS connection info 00100 std::string sServerHost,sServerPort; 00101 long lServerPort; 00102 00104 static CMOOSCommClient* pComms=NULL; 00105 00106 typedef std::pair<std::string, double> REGINFO; 00107 typedef std::set< REGINFO > REGISTRATION_SET; 00108 static REGISTRATION_SET Registrations; 00109 00110 // a parameter holding class 00111 class Param 00112 { 00113 public: 00114 enum Type 00115 { 00116 STR, 00117 DBL, 00118 00119 UNK, 00120 }; 00121 double dfVal; 00122 std::string sVal; 00123 00124 Type m_eType; 00125 Param() 00126 { 00127 m_eType=UNK; 00128 dfVal = -1; 00129 } 00130 std::string Str() 00131 { 00132 switch(m_eType) 00133 { 00134 case STR: 00135 return "Str: "+sVal; 00136 case DBL: 00137 return MOOSFormat("Dbl: %.3f",dfVal); 00138 case UNK: 00139 return MOOSFormat("NOT SET! "); 00140 } 00141 return "ERROR"; 00142 } 00143 bool operator==(double dfV){return dfVal==dfV && m_eType==DBL;}; 00144 bool operator==(std::string sV){return sVal==sV && m_eType==STR;}; 00145 00146 00147 }; 00148 typedef std::map<std::string,Param> ARGMAP; 00149 00150 ARGMAP gArgMap; 00151 00152 void SetParam(std::string sParam,double dfVal) 00153 { 00154 Param NP; 00155 NP.m_eType=Param::DBL; 00156 NP.dfVal = dfVal; 00157 gArgMap[sParam] = NP; 00158 } 00159 00160 void SetParam(std::string sParam,std::string sVal) 00161 { 00162 Param NP; 00163 NP.m_eType=Param::STR; 00164 NP.sVal = sVal; 00165 gArgMap[sParam] = NP; 00166 } 00167 00168 void FillDefaultArgMap() 00169 { 00170 //here we add our default options 00171 SetParam("CONFIG_FILE","iMatlab.moos"); 00172 SetParam("SERIAL",0.0); 00173 SetParam("SERIAL_TIMEOUT",10.0); 00174 SetParam("MOOSNAME","iMatlab"); 00175 SetParam("SERVERPORT",9000); 00176 SetParam("SERVERHOST","localhost"); 00177 00178 } 00179 00180 bool GetParam(std::string sName, Param & P) 00181 { 00182 ARGMAP::iterator p = gArgMap.find(sName); 00183 if(p==gArgMap.end()) 00184 { 00185 return false; 00186 } 00187 else 00188 { 00189 P = p->second; 00190 return true; 00191 } 00192 } 00193 00194 Param GetParam(std::string sName) 00195 { 00196 Param NP; 00197 ARGMAP::iterator p = gArgMap.find(sName); 00198 if(p!=gArgMap.end()) 00199 { 00200 NP = p->second; 00201 } 00202 else 00203 { 00204 MOOSTrace("Warning no such parameter %s %s\n",sName.c_str(),MOOSHERE); 00205 } 00206 00207 return NP; 00208 } 00209 00210 00211 00212 00213 void OnExit() 00214 { 00215 00216 MOOSTrace("iMatlab is cleaning up\n"); 00217 if(pComms) 00218 { 00219 if(1 || pComms->IsConnected()) 00220 { 00221 MOOSTrace("Halting MOOSComms "); 00222 00223 //do an agressive close...don't know why this is needed :-( 00224 pComms->Close(true); 00225 delete pComms; 00226 pComms=NULL; 00227 } 00228 } 00229 } 00230 00231 00232 void DoRegistrations() 00233 { 00234 if(pComms!=NULL && pComms->IsConnected()) 00235 { 00236 REGISTRATION_SET::iterator p; 00237 for(p = Registrations.begin();p!=Registrations.end();p++) 00238 { 00239 pComms->Register(p->first,p->second); 00240 } 00241 } 00242 } 00243 00244 bool OnMOOSConnect(void * pParam) 00245 { 00246 MOOSTrace("DB connection established.\n"); 00247 DoRegistrations(); 00248 return true; 00249 } 00250 00251 00252 //called the fist time iMatlab runs or when 'init' is passed as the first parameter 00253 bool Initialise(const mxArray *prhs[], int nrhs) 00254 { 00255 00256 if(bInitialised) 00257 { 00258 MOOSTrace("Already initialised - use \"clear iMatlab\" to restart\n"); 00259 return true; 00260 } 00261 MOOSTrace("*********** iMatlab Initialising ***********\n"); 00262 MOOSTrace("* A box of MOOS accessories *\n"); 00263 MOOSTrace("* P. Newman *\n"); 00264 MOOSTrace("* Oxford 2005 *\n"); 00265 MOOSTrace("********************************************\n"); 00266 00267 //get our default args 00268 FillDefaultArgMap(); 00269 00270 //get custom args 00271 if(prhs!=NULL) 00272 { 00273 for(int i = 1;i<nrhs;i+=2) 00274 { 00275 if(i<nrhs-1) 00276 { 00277 std::string sParam; 00278 if(!Matlab2String(sParam,prhs[i])) 00279 { 00280 mexErrMsgTxt("Incorrect param value pair (not a string)"); 00281 } 00282 MOOSTrace("Read String %s\n",sParam.c_str()); 00283 00284 const mxArray *p = prhs[i+1]; 00285 00286 Param NewParam; 00287 NewParam.m_eType = Param::UNK; 00288 switch(mxGetClassID(p)) 00289 { 00290 case mxCHAR_CLASS: 00291 { 00292 std::string sVal; 00293 if(Matlab2String(sVal,p)) 00294 { 00295 NewParam.m_eType=Param::STR; 00296 NewParam.sVal = sVal; 00297 } 00298 } 00299 break; 00300 case mxDOUBLE_CLASS: 00301 { 00302 00303 00304 int nRows = mxGetM(p); 00305 int nCols = mxGetN(p); 00306 //a scalar 00307 NewParam.m_eType = Param::DBL; 00308 NewParam.dfVal = mxGetScalar(p); 00309 00310 } 00311 break; 00312 default: 00313 MOOSTrace("Failed to create a parameter\n"); 00314 break; 00315 } 00316 00317 //did we parse it OK? 00318 if(NewParam.m_eType==Param::UNK) 00319 { 00320 mexPrintf("PROBLEM : can't parse parameter value %s\n",sParam.c_str()); 00321 } 00322 else 00323 { 00324 ARGMAP::iterator p = gArgMap.find(sParam); 00325 00326 if(p==gArgMap.end()) 00327 { 00328 mexPrintf("warning no such parameter exists: %s\n",sParam.c_str()); 00329 } 00330 else 00331 { 00332 (p->second) = NewParam; 00333 } 00334 } 00335 } 00336 } 00337 } 00338 00339 00340 ARGMAP::iterator p; 00341 00342 for(p = gArgMap.begin();p!=gArgMap.end();p++) 00343 { 00344 mexPrintf("Property %-25s %s\n",p->first.c_str(),(p->second).Str().c_str()); 00345 } 00346 00347 00348 //waht to do when we exit? 00349 mexAtExit(OnExit); 00350 00351 00352 //set up a file reader 00353 Param N; 00354 std::string sMOOSName = "iMatlab"; 00355 if(!GetParam("MOOSNAME",N)) 00356 { 00357 MOOSTrace("No MOOSName found assuming default of %s\n",sMOOSName.c_str()); 00358 } 00359 else 00360 { 00361 sMOOSName = N.sVal; 00362 } 00363 gConfigReader.SetAppName(sMOOSName.c_str()); 00364 00365 //list of al out settings 00366 STRING_LIST ConfigFileParams; 00367 00368 Param P; 00369 if(!GetParam("CONFIG_FILE",P)) 00370 { 00371 MOOSTrace("No configuration file found\n"); 00372 } 00373 else 00374 { 00375 if(!gConfigReader.SetFile(P.sVal)) 00376 { 00377 MOOSTrace("Failed to set configuration file"); 00378 } 00379 else 00380 { 00381 if(!gConfigReader.GetConfiguration(gConfigReader.GetAppName(),ConfigFileParams)) 00382 { 00383 MOOSTrace("Failed to read configuration block for %s file %s", 00384 gConfigReader.GetAppName().c_str(), 00385 gConfigReader.GetFileName().c_str()); 00386 return false; 00387 } 00388 else 00389 { 00390 } 00391 } 00392 } 00393 00394 00395 std::string sBool; 00396 00397 //DO WE WANT MOOS COMMS? 00398 if(gConfigReader.GetConfigurationParam("MOOSComms",sBool)) 00399 { 00400 if(MOOSStrCmp(sBool,"TRUE")) 00401 { 00402 MOOSTrace("Setting Up MOOS Comms\n"); 00403 00404 if(!gConfigReader.GetValue("SERVERHOST",sServerHost)) 00405 { 00406 MOOSTrace("Warning Server host not read from mission file: assuming LOCALHOST\n"); 00407 sServerHost = "LOCALHOST"; 00408 } 00409 00410 00411 if(!gConfigReader.GetValue("SERVERPORT",sServerPort)) 00412 { 00413 MOOSTrace("Warning Server port not read from mission file: assuming 9000\n"); 00414 sServerPort = "9000"; 00415 } 00416 00417 long lServerPort = atoi(sServerPort.c_str()); 00418 00419 if(lServerPort==0) 00420 { 00421 lServerPort = 9000; 00422 MOOSTrace("Warning Server port not read from mission file: assuming 9000\n"); 00423 } 00424 00425 00426 double dfTimeOut; 00427 if(gConfigReader.GetValue("SERIAL_TIMEOUT",dfTimeOut)) 00428 { 00429 SetParam("SERIAL_TIMEOUT",dfTimeOut); 00430 } 00431 00432 //do we have any programmed subscriptions? 00433 STRING_LIST::iterator t; 00434 for(t = ConfigFileParams.begin();t!=ConfigFileParams.end();t++) 00435 { 00436 std::string sTok,sVal; 00437 if(gConfigReader.GetTokenValPair(*t,sTok,sVal)) 00438 { 00439 if(MOOSStrCmp(sTok,"SUBSCRIBE")) 00440 { 00441 std::string sWhat = MOOSChomp(sVal,"@"); 00442 MOOSTrimWhiteSpace(sWhat); 00443 double dfT = atof(sVal.c_str()); 00444 Registrations.insert( REGINFO(sWhat,dfT) ); 00445 MOOSTrace("Adding Registration for \"%s\" every %f seconds \n",sWhat.c_str(),dfT); 00446 } 00447 } 00448 } 00449 00450 00451 //here we launch the comms 00452 if(pComms==NULL) 00453 { 00454 pComms = new CMOOSCommClient; 00455 pComms->SetOnConnectCallBack(OnMOOSConnect,NULL); 00456 pComms->Run(sServerHost.c_str(),lServerPort,sMOOSName.c_str()); 00457 } 00458 00459 } 00460 } 00461 00462 //DO WE WANT SERIAL COMMS? 00463 if(gConfigReader.GetConfigurationParam("SerialComms",sBool)) 00464 { 00465 if(MOOSStrCmp(sBool,"TRUE")) 00466 { 00467 00468 MOOSTrace("Setting Up Serial Comms\n"); 00469 if(!gPort.Configure(ConfigFileParams)) 00470 { 00471 MOOSTrace("Failed to open serial port\n"); 00472 } 00473 else 00474 { 00475 MOOSTrace("Port %s opened succesfully %s at %d BPS\n", 00476 gPort.GetPortName().c_str(), 00477 gPort.IsStreaming()?"streaming":"polled", 00478 gPort.GetBaudRate()); 00479 00480 SetParam("SERIAL",1.0); 00481 00482 MOOSTrace("Beware - this release only supports receiving ASCII strings\n"); 00483 } 00484 } 00485 } 00486 00487 bInitialised = true; 00488 return bInitialised; 00489 } 00490 00491 00492 void PrintHelp() 00493 { 00494 00495 std::ifstream HelpFile("iMatlab.help"); 00496 00497 if(HelpFile.is_open()) 00498 { 00499 while(!HelpFile.eof()) 00500 { 00501 char Line[1024]; 00502 HelpFile.getline(Line,sizeof(Line)); 00503 mexPrintf("%s\n",Line); 00504 } 00505 } 00506 else 00507 { 00508 mexPrintf("help file \"iMatlab.help\" not found\n"); 00509 } 00510 00511 } 00512 00513 00514 00515 //-------------------------------------------------------------- 00516 // function: iMatlab - Entry point from Matlab environment (via 00517 // mexFucntion(), below) 00518 // INPUTS: 00519 // nlhs - number of left hand side arguments (outputs) 00520 // plhs[] - pointer to table where created matrix pointers are 00521 // to be placed 00522 // nrhs - number of right hand side arguments (inputs) 00523 // prhs[] - pointer to table of input matrices 00524 //-------------------------------------------------------------- 00525 void iMatlab( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) 00526 { 00527 // TODO: Add your application code here 00528 if(nrhs==0) 00529 { 00530 //no argument - print help 00531 PrintHelp(); 00532 return; 00533 } 00534 00535 00536 std::string sCmd; 00537 //first parameter is always a string command 00538 if(!Matlab2String(sCmd,prhs[0])) 00539 { 00540 mexErrMsgTxt("Param 1 (cmd) must be a string "); 00541 } 00542 00543 00544 if(MOOSStrCmp(sCmd,"INIT")) 00545 { 00546 Initialise(prhs,nrhs); 00547 } 00548 else 00549 { 00550 if(!bInitialised) 00551 { 00552 MOOSTrace("iMatlab is not initialised - must call \"iMatlab('init')\" first\n"); 00553 } 00555 else if(MOOSStrCmp(sCmd,"MOOS_MAIL_TX")) 00556 { 00557 if(nrhs<3) 00558 { 00559 MOOSTrace("Incorrect format : 'MOOS_MAIL_TX','VAR_NAME'.VarVal (string or double)\n"); 00560 return ; 00561 } 00562 std::string sKey; 00563 if(!Matlab2String(sKey,prhs[1])) 00564 { 00565 mexErrMsgTxt("Param 2 (key) must be a string name of the data being sent\n"); 00566 } 00567 00568 if(pComms && pComms->IsConnected()) 00569 { 00570 double dfTime=MOOSTime(); 00571 if(nrhs==4 && ! Matlab2Double(dfTime,prhs[3])) 00572 { 00573 mexErrMsgTxt("parameter 4 must be a double time\n"); 00574 } 00575 00576 std::string sTmp; 00577 double dfTmp; 00578 if(Matlab2String(sTmp,prhs[2])) 00579 { 00580 pComms->Notify(sKey,sTmp,dfTime); 00581 } 00582 else if(Matlab2Double(dfTmp,prhs[2])) 00583 { 00584 pComms->Notify(sKey,dfTmp,dfTime); 00585 } 00586 else 00587 { 00588 mexErrMsgTxt("MOOS transmit failed parameter 3 must be a string or double data value\n"); 00589 } 00590 } 00591 else 00592 { 00593 mexErrMsgTxt("MOOS transmit failed - not connected\n"); 00594 } 00595 } 00596 //COLLECTING MOOS MAIL FROM COMMS THREAD 00597 else if(MOOSStrCmp(sCmd,"MOOS_MAIL_RX")) 00598 { 00599 if(pComms->IsConnected()) 00600 { 00601 MOOSMSG_LIST NewMail; 00602 if(pComms->Fetch(NewMail)) 00603 { 00604 //how many have we got? 00605 int nMsgs = NewMail.size(); 00606 00607 if(nlhs==1) 00608 { 00609 00610 00611 //make a struct array 00612 00613 DIM_TYPE DimArray[2]; 00614 DimArray[0] = 1;DimArray[1] = nMsgs; 00615 00616 const char * FieldNames[] = {"KEY","TYPE","TIME","STR","DBL","SRC","ORIGINATING_COMMUNITY"}; 00617 plhs[0] = mxCreateStructArray(2, DimArray, sizeof(FieldNames)/sizeof(char*),FieldNames); 00618 00619 int nKeyField = mxGetFieldNumber(plhs[0],FieldNames[0]); 00620 int nTypeField = mxGetFieldNumber(plhs[0],FieldNames[1]); 00621 int nTimeField = mxGetFieldNumber(plhs[0],FieldNames[2]); 00622 int nStrField = mxGetFieldNumber(plhs[0],FieldNames[3]); 00623 int nDblField = mxGetFieldNumber(plhs[0],FieldNames[4]); 00624 int nSrcField = mxGetFieldNumber(plhs[0],FieldNames[5]); 00625 int nCommunityField = mxGetFieldNumber(plhs[0],FieldNames[6]); 00626 00627 00628 MOOSMSG_LIST::iterator p; 00629 00630 int i = 0; 00631 for(p = NewMail.begin();p!=NewMail.end();p++,i++) 00632 { 00633 //copy in the Key of the variable 00634 mxSetFieldByNumber(plhs[0],i,nKeyField,mxCreateString(p->m_sKey.c_str())); 00635 00636 //copy in the type 00637 char * pType = (char*)(p->IsDataType(MOOS_DOUBLE) ? "DBL":"STR"); 00638 mxSetFieldByNumber(plhs[0],i,nTypeField,mxCreateString(pType)); 00639 00640 //copy in time 00641 // Note: mxCreateScalarDouble is deprecated as of Matlab 2011a, see: 00642 // http://xerxes.robots.ox.ac.uk:8000/ticket/52 00643 // MX_API_VER = 7.04 for 2010b and 2011b 00644 #ifdef mxCreateScalarDouble 00645 mxSetFieldByNumber(plhs[0],i,nTimeField,mxCreateScalarDouble(p->GetTime())); 00646 #else 00647 mxSetFieldByNumber(plhs[0],i,nTimeField,mxCreateDoubleScalar(p->GetTime())); 00648 #endif 00649 00650 //copy in sVal 00651 mxSetFieldByNumber(plhs[0],i,nStrField,mxCreateString(p->m_sVal.c_str())); 00652 00653 //copy in dfVal 00654 #ifdef mxCreateScalarDouble 00655 mxSetFieldByNumber(plhs[0],i,nDblField,mxCreateScalarDouble(p->GetDouble())); 00656 #else 00657 mxSetFieldByNumber(plhs[0],i,nDblField,mxCreateDoubleScalar(p->GetDouble())); 00658 #endif 00659 00660 //copy in src process 00661 mxSetFieldByNumber(plhs[0],i,nSrcField,mxCreateString(p->m_sSrc.c_str())); 00662 00663 //copy in originating community 00664 mxSetFieldByNumber(plhs[0],i,nCommunityField,mxCreateString(p->m_sOriginatingCommunity.c_str())); 00665 00666 } 00667 } 00668 else 00669 { 00670 MOOSTrace("Picked up %d MOOSMsgs - but no output variables to return them in!\n",nMsgs); 00671 } 00672 00673 } 00674 else 00675 { 00676 //the Fethc failed but we are connected - probably no data 00677 //make a struct array 00678 DIM_TYPE DimArray[2];DimArray[0] = 1;DimArray[1] = 0; 00679 const char * FieldNames[] = {"KEY","TYPE","TIME","STR","DBL","SRC","ORIGINATING_COMMUNITY"}; 00680 plhs[0] = mxCreateStructArray(2, DimArray, sizeof(FieldNames)/sizeof(char*),FieldNames); 00681 } 00682 } 00683 else 00684 { 00685 MOOSTrace("No MOOS connection established Mail Rx failed\n"); 00686 DIM_TYPE DimArray[2];DimArray[0] = 1;DimArray[1] = 0; 00687 const char * FieldNames[] = {"KEY","TYPE","TIME","STR","DBL","SRC","ORIGINATING_COMMUNITY"}; 00688 plhs[0] = mxCreateStructArray(2, DimArray, sizeof(FieldNames)/sizeof(char*),FieldNames); 00689 00690 00691 return; 00692 } 00693 } 00694 //REGISTERING FOR MAIL 00695 else if(MOOSStrCmp(sCmd,"MOOS_REGISTER")) 00696 { 00697 if(nrhs!=3) 00698 { 00699 MOOSTrace("incorrect format. Use iMatlab('MOOS_REGISTER','WHAT',HOW_OFTEN\n"); 00700 return; 00701 } 00702 else 00703 { 00704 00705 std::string sWhat; 00706 00707 if(!Matlab2String(sWhat,prhs[1])) 00708 { 00709 MOOSTrace("incorrect format parameter 2 must be a string\n"); 00710 return; 00711 } 00712 double dfHowOften; 00713 if(!Matlab2Double(dfHowOften,prhs[2])) 00714 { 00715 MOOSTrace("incorrect format parameter 3 must be a double (min period between messages)\n"); 00716 return; 00717 } 00718 00719 //save the information 00720 Registrations.insert( REGINFO(sWhat,dfHowOften) ); 00721 00722 //reregister 00723 DoRegistrations(); 00724 } 00725 } 00726 //PAUSING 00727 else if(MOOSStrCmp(sCmd,"MOOS_PAUSE")) 00728 { 00729 double dfT; 00730 if(!Matlab2Double(dfT,prhs[1])) 00731 { 00732 MOOSFail("incoorect MOOS_PAUSE format - param 2 must be numeric seconds\n"); 00733 return; 00734 } 00735 MOOSPause(static_cast<int> (dfT*1000.0)); 00736 } 00737 //SENDING SERIAL DATA 00738 else if(MOOSStrCmp(sCmd,"SERIAL_TX")) 00739 { 00740 if(GetParam("SERIAL")==0.0) 00741 { 00742 MOOSTrace("No serial comms configured -> can't call SERIAL_TX\n"); 00743 } 00744 else 00745 { 00746 if(nrhs<2) 00747 { 00748 MOOSTrace("Incorrect format 'SERIAL_TX',DataToSend "); 00749 return; 00750 } 00751 00752 //OK we could be sending binary or ASCII - nothing else is OK 00753 switch(mxGetClassID(prhs[1])) 00754 { 00755 case mxUINT8_CLASS: 00756 case mxCHAR_CLASS: 00757 { 00758 int nLen = mxGetNumberOfElements(prhs[1]); 00759 char * pOp = (char *)mxGetData(prhs[1]); 00760 int nWritten = gPort.Write(pOp,nLen); 00761 if(nWritten!=nLen) 00762 { 00763 MOOSTrace("Failed to write %d bytes of data, wrote %d - oops\n",nLen,nWritten); 00764 return; 00765 } 00766 } 00767 break; 00768 00769 default: 00770 MOOSTrace("Problem: cannot send data of type \"%s\" must be UINT8 ot CHAR\n",mxGetClassName(prhs[1])); 00771 return; 00772 } 00773 00774 } 00775 } 00776 //RECEIVING SERIAL DATA 00777 else if(MOOSStrCmp(sCmd,"SERIAL_RX")) 00778 { 00779 if(GetParam("SERIAL")==0.0) 00780 { 00781 MOOSTrace("No serial comms configured -> can't call SERIAL_RX\n"); 00782 } 00783 else 00784 { 00785 std::string sRx;double dfTime; 00786 typedef std::pair< std::string, double > TG; 00787 typedef std::vector<TG > TGL; 00788 00789 //a list of telegrams 00790 TGL RxL; 00791 00792 if(gPort.IsStreaming()) 00793 { 00794 //suck em up... 00795 while(gPort.GetLatest(sRx,dfTime)) 00796 { 00797 RxL.push_back( TG(sRx,dfTime) ); 00798 } 00799 } 00800 else 00801 { 00802 //just read one 00803 if(gPort.GetTelegram(sRx,GetParam("SERIAL_TIMEOUT").dfVal,&dfTime)) 00804 { 00805 RxL.push_back( TG(sRx,dfTime) ); 00806 } 00807 } 00808 00809 if(nlhs>0) 00810 { 00811 00812 DIM_TYPE DimArray[2];DimArray[0] = 1;DimArray[1] = RxL.size(); 00813 const char * FieldNames[2] = {"STR","TIME"}; 00814 plhs[0] = mxCreateStructArray(2, DimArray, 2, FieldNames); 00815 00816 int nStrField = mxGetFieldNumber(plhs[0],FieldNames[0]); 00817 int nTimeField = mxGetFieldNumber(plhs[0],FieldNames[1]); 00818 00819 for (unsigned int i=0; i<RxL.size(); i++) 00820 { 00821 00822 //copy in the string Rx'd 00823 mxSetFieldByNumber(plhs[0],i,nStrField,mxCreateString(RxL[i].first.c_str())); 00824 00825 //copy in the time 00826 mxArray * pTmp = mxCreateDoubleMatrix(1,1,mxREAL); 00827 *mxGetPr(pTmp) = RxL[i].second; 00828 mxSetFieldByNumber(plhs[0],i,nTimeField,pTmp); 00829 } 00830 } 00831 else 00832 { 00833 MOOSTrace("Rx'd %d telegrams\n",RxL.size()); 00834 } 00835 00836 } 00837 } 00838 else 00839 { 00840 MOOSTrace("Huh? - command %s is not known\n",sCmd.c_str()); 00841 } 00842 } 00843 00844 00845 00846 } // end iMatlab() 00847 00848 00849 00850 extern "C" { 00851 //-------------------------------------------------------------- 00852 // mexFunction - Entry point from Matlab. From this C function, 00853 // simply call the C++ application function, above. 00854 //-------------------------------------------------------------- 00855 void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) 00856 { 00857 iMatlab(nlhs, plhs, nrhs, prhs); 00858 } 00859 } 00860 00861