MOOS 0.2375
/home/toby/moos-ivp/MOOS-2375-Oct0611/Tools/Matlab/iMatlab/iMatlab.cpp
Go to the documentation of this file.
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines