matlab: Andrei's testing version of MCL like S-function

diff --git a/src/laser-nav/matlab/mcl_test.mdl b/src/laser-nav/matlab/mcl_test.mdl
old mode 100755 (executable)
new mode 100644 (file)
index a926d4c..96780a7
--- a/src/laser-nav/matlab/mcl_test.mdl
+++ b/src/laser-nav/matlab/mcl_test.mdl
@@ -6,7 +6,7 @@ Model {
     NumRootInports         0\r
     NumRootOutports        0\r
     ParameterArgumentNames  ""\r
-    ComputedModelVersion    "1.2"\r
+    ComputedModelVersion    "1.3"\r
     NumModelReferences     0\r
     NumTestPointedSignals   0\r
   }\r
@@ -54,8 +54,8 @@ Model {
   ModifiedByFormat       "%<Auto>"\r
   LastModifiedBy         "Andrei"\r
   ModifiedDateFormat     "%<Auto>"\r
-  LastModifiedDate       "Fri Apr 04 17:18:21 2008"\r
-  ModelVersionFormat     "1.%<AutoIncrement:2>"\r
+  LastModifiedDate       "Mon Apr 07 00:09:43 2008"\r
+  ModelVersionFormat     "1.%<AutoIncrement:3>"\r
   ConfigurationManager   "None"\r
   LinearizationMsg       "none"\r
   Profile                off\r
@@ -492,7 +492,7 @@ Model {
   }\r
   System {\r
     Name                   "mcl_test"\r
-    Location               [562, 164, 1142, 457]\r
+    Location               [621, 375, 1201, 668]\r
     Open                   on\r
     ModelBrowserVisibility  off\r
     ModelBrowserWidth      200\r
@@ -508,14 +508,14 @@ Model {
       Name                   "Constant"\r
       Position               [80, 95, 110, 125]\r
       Value                  "[1 1 1]"\r
-      SampleTime             "0.1"\r
+      SampleTime             "0.5"\r
     }\r
     Block {\r
       BlockType                      Constant\r
       Name                   "Constant1"\r
       Position               [80, 175, 110, 205]\r
-      Value                  "[0 0 0 ]"\r
-      SampleTime             "0.33"\r
+      Value                  "[0 0 0]"\r
+      SampleTime             "2"\r
     }\r
     Block {\r
       BlockType                      "S-Function"\r
@@ -531,8 +531,8 @@ Model {
       Ports                  [1]\r
       Position               [435, 134, 465, 166]\r
       Floating               off\r
-      Location               [188, 390, 512, 629]\r
-      Open                   off\r
+      Location               [303, 265, 1051, 605]\r
+      Open                   on\r
       NumInputPorts          "1"\r
       List {\r
        ListType                AxesTitles\r
@@ -542,23 +542,23 @@ Model {
       SampleTime             "0"\r
     }\r
     Line {\r
-      SrcBlock               "Constant"\r
+      SrcBlock               "S-Function"\r
       SrcPort                1\r
-      Points                 [35, 0; 0, 30]\r
-      DstBlock               "S-Function"\r
+      DstBlock               "Scope"\r
       DstPort                1\r
     }\r
     Line {\r
       SrcBlock               "Constant1"\r
       SrcPort                1\r
-      Points                 [35, 0; 0, -35]\r
+      Points                 [60, 0; 0, -35]\r
       DstBlock               "S-Function"\r
       DstPort                2\r
     }\r
     Line {\r
-      SrcBlock               "S-Function"\r
+      SrcBlock               "Constant"\r
       SrcPort                1\r
-      DstBlock               "Scope"\r
+      Points                 [60, 0; 0, 30]\r
+      DstBlock               "S-Function"\r
       DstPort                1\r
     }\r
   }\r

diff --git a/src/laser-nav/matlab/sf_mcl.c b/src/laser-nav/matlab/sf_mcl.c
old mode 100755 (executable)
new mode 100644 (file)
index 37196be..97d1e1b
--- a/src/laser-nav/matlab/sf_mcl.c
+++ b/src/laser-nav/matlab/sf_mcl.c
@@ -1,9 +1,8 @@
 
 
 #ifdef __cplusplus
-extern "C" { /* use the C fcn-call standard for all functions  */
-#endif
-/* defined within this scope */
+extern "C" { // use the C fcn-call standard for all functions  
+#endif       // defined within this scope                     
 
 #define S_FUNCTION_LEVEL 2
 #define S_FUNCTION_NAME  sf_mcl
@@ -18,10 +17,11 @@ extern "C" { /* use the C fcn-call standard for all functions  */
 
 #define INPUT_MOVE 0
 #define INPUT_MEASURE 1
+#define OUTPUT_EST_POS 0
 
 #define PARAM_PART_COUNT_IDX 0
 #define PART_COUNT_PARAM(S) (ssGetSFcnParam(S,PARAM_PART_COUNT_IDX))
-#define OUTPUT_EST_POS 0
+
 #define SAMPLETIME_MOVE 0
 #define SAMPLETIME_MEASURE 1
 
@@ -93,7 +93,7 @@ static void mdlInitializeSizes(SimStruct *S)
     ssSetNumSampleTimes(S, PORT_BASED_SAMPLE_TIMES);
     ssSetNumRWork(S, 0);
     ssSetNumIWork(S, 0);
-    ssSetNumPWork(S, 1); /* reserve element in the pointers vector */
+    ssSetNumPWork(S, 1); // reserve element in the pointers vector
     ssSetNumModes(S, 0); 
     ssSetNumNonsampledZCs(S, 0);
 
@@ -181,6 +181,7 @@ static void mdlInitializeSampleTimes(SimStruct *S)
       pwork[0] = (void*)st;  
 }
 #endif /*  MDL_START */
+
 #define MDL_UPDATE
 /* Function: mdlUpdate ======================================================
  * Abstract:
@@ -193,6 +194,8 @@ static void mdlUpdate(SimStruct *S, int_T tid)
         ssPrintf("Update %d %g\n", tid, ssGetT(S));
 }
 
+
+
 /* Function: mdlOutputs =======================================================
  * Abstract:
  *    In this function, you compute the outputs of your S-function
@@ -205,30 +208,53 @@ static void mdlOutputs(SimStruct *S, int_T tid)
     int moveTid = ssGetInputPortSampleTimeIndex(S,INPUT_MOVE);
     int measureTid = ssGetInputPortSampleTimeIndex(S,INPUT_MEASURE);
 
+    InputRealPtrsType iPtrsMove = ssGetInputPortRealSignalPtrs(S, INPUT_MOVE);
+    InputRealPtrsType iPtrsMeasure = ssGetInputPortRealSignalPtrs(S, INPUT_MEASURE);
+
+    
     ssPrintf("Output %d %g\n", tid, ssGetT(S));
-   
+    ssPrintf("=============================================\n");
     if (ssIsSampleHit(S, moveTid, tid)) {
+                
+                st->x = st->x + *iPtrsMove[0];
+                st->y = st->y + *iPtrsMove[1];
+                st->ang = st->ang + *iPtrsMove[2];
+                pos[0] = st->x;
+                pos[1] = st->y;
+                pos[2] = st->ang;
+                ssPrintf("x=%g, y=%g, ang=%g\n",pos[0], pos[1], pos[2]); 
                 ssPrintf("move \n");
             if (moveTid == measureTid || 
                 ssIsSpecialSampleHit(S, measureTid, moveTid, tid)) {
                 ssPrintf("both \n");
-                
+                        
             }
         }
     
     if (ssIsSampleHit(S, measureTid, tid)) {
-                ssPrintf("measure \n");
-            if (moveTid == measureTid || 
+                 
+                 st->x = *iPtrsMeasure[0];
+                 st->y = *iPtrsMeasure[1];
+                 st->ang = *iPtrsMeasure[2];
+                 pos[0] = st->x;
+                 pos[1] = st->y;
+                 pos[2] = st->ang;
+                 ssPrintf("xMeas=%g, yMeas=%g, angMeas=%g\n",pos[0], pos[1], pos[2]); 
+                 ssPrintf("measure \n"); 
+             if (moveTid == measureTid || 
                 ssIsSpecialSampleHit(S, measureTid, moveTid, tid)) {
                 ssPrintf("both \n");
                 
             }
         }
 
-    pos[0] = st->x;
-    pos[1] = st->y;
-    pos[2] = st->ang;
-    UNUSED_ARG(tid);
+
+
+    /* pos[0] = st->x;
+       pos[1] = st->y;
+       pos[2] = st->ang;*/
+       UNUSED_ARG(tid);  
+ 
 }                                                
 
 /* Function: mdlTerminate =====================================================
@@ -240,7 +266,7 @@ static void mdlOutputs(SimStruct *S, int_T tid)
 static void mdlTerminate(SimStruct *S)
 {
     free(ssGetPWork(S)[0]);
-}                                              /* function */
+}                                              // function
 /*======================================================*
  * See sfuntmpl.doc for the optional S-function methods *
  *======================================================*/