Maltab: Update MCL to accept laser data of variable length

diff --git a/src/mcl/matlab/sf_mcl.c b/src/mcl/matlab/sf_mcl.c
index f1eb2e9d3a1cc188273a4f031a6d5271bd52d5a3..eaa42691d864fbb6cd568adb52838154a859ff07 100644 (file)
--- a/src/mcl/matlab/sf_mcl.c
+++ b/src/mcl/matlab/sf_mcl.c
@@ -114,7 +114,8 @@ static void mdlInitializeSizes(SimStruct *S)
     ssSetInputPortDirectFeedThrough(S, INPUT_MOVE, 1);
   /*  ssSetInputPortRequiredContiguous(S, INPUT_MOVE, 1);*/
 
-    ssSetInputPortWidth(S, INPUT_MEASURE, 3);
+    /* Number of measured angles can vary */
+    ssSetInputPortWidth(S, INPUT_MEASURE, DYNAMICALLY_SIZED);
     ssSetInputPortDirectFeedThrough(S, INPUT_MEASURE, 1);
  /*   ssSetInputPortRequiredContiguous(S, INPUT_MEASURE, 1);*/
     
@@ -152,6 +153,83 @@ static void mdlSetInputPortSampleTime(SimStruct *S,
     ssSetInputPortSampleTime(S, portIdx, sampleTime);
     ssSetInputPortOffsetTime(S, portIdx, offsetTime);
 }
+
+#define MDL_SET_INPUT_PORT_DIMENSION_INFO
+/* Function: mdlSetInputPortDimensionInfo ====================================
+ * Abstract:
+ *    This routine is called with the candidate dimensions for an input port
+ *    with unknown dimensions. If the proposed dimensions are acceptable, the 
+ *    routine should go ahead and set the actual port dimensions.  
+ *    If they are unacceptable an error should be generated via 
+ *    ssSetErrorStatus.  
+ *    Note that any other input or output ports whose dimensions are 
+ *    implicitly defined by virtue of knowing the dimensions of the given port 
+ *    can also have their dimensions set.
+ */
+static void mdlSetInputPortDimensionInfo(SimStruct        *S, 
+                                         int_T            port,
+                                         const DimsInfo_T *dimsInfo)
+{
+    int_T  uNumDims = dimsInfo->numDims;
+    int_T  uWidth   = dimsInfo->width;
+    int_T  *uDims   = dimsInfo->dims;
+
+    boolean_T  isOk = true;
+    
+    /* Set input port dimension */
+    if(!ssSetInputPortDimensionInfo(S, port, dimsInfo)) return;
+
+    /* 
+     * The block only accepts 1-D signals. Check number of dimensions. 
+     * If the parameter and the input signal are non-scalar, their dimensions
+     * must be the same.
+     */
+    isOk = (uNumDims == 1) && (uWidth >= 1) && (uWidth < 10);
+
+    if(!isOk){
+        ssSetErrorStatus(S, "Invalid input port dimensions. bla bla bla");
+        return;
+    }
+}
+
+# define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
+/* Function: mdlSetOutputPortDimensionInfo ===================================
+ * Abstract:
+ *    This routine is called with the candidate dimensions for an output port 
+ *    with unknown dimensions. If the proposed dimensions are acceptable, the 
+ *    routine should go ahead and set the actual port dimensions.  
+ *    If they are unacceptable an error should be generated via 
+ *    ssSetErrorStatus.  
+ *    Note that any other input or output ports whose dimensions are  
+ *    implicitly defined by virtue of knowing the dimensions of the given 
+ *    port can also have their dimensions set.
+ */
+static void mdlSetOutputPortDimensionInfo(SimStruct        *S, 
+                                          int_T            port,
+                                          const DimsInfo_T *dimsInfo)
+{
+    if(!ssSetOutputPortDimensionInfo(S, port, dimsInfo)) return;
+}
+
+#if 0
+# define MDL_SET_DEFAULT_PORT_DIMENSION_INFO
+/* Function: mdlSetDefaultPortDimensionInfo ====================================
+ *    This routine is called when Simulink is not able to find dimension
+ *    candidates for ports with unknown dimensions. This function must set the
+ *    dimensions of all ports with unknown dimensions.
+ */
+static void mdlSetDefaultPortDimensionInfo(SimStruct *S)
+{
+    int_T outWidth = ssGetOutputPortWidth(S, 0);
+    /* Input port dimension must be unknown. Set it to scalar. */
+    if(!ssSetInputPortMatrixDimensions(S, 0, 1, 1)) return;
+    if(outWidth == DYNAMICALLY_SIZED){
+        /* Output dimensions are unknown. Set it to scalar. */
+        if(!ssSetOutputPortMatrixDimensions(S, 0, 1, 1)) return;
+    }
+}
+#endif
+
 /* Function: mdlSetOutputPortSampleTime ========================================
  * Abstract:
  *     When asked by Simulink, set the sample time of the specified output
@@ -275,16 +353,18 @@ static do_move(struct mcl_model *mcl, InputRealPtrsType iPtrsMove)
        mcl->move(mcl, dx, dy, dangle);
 }
 
-static do_measure(struct mcl_model *mcl, InputRealPtrsType iPtrsMeasure)
+static do_measure(struct mcl_model *mcl, InputRealPtrsType iPtrsMeasure, int_T width)
 {
        struct mcl_angles angles;
-       angles.count = 3;
+       angles.count = width;
        int i;
        bool nonzero = false;
-       for (i=0; i<3; i++) {
+       for (i=0; i<width; i++) {
                angles.val[i] = *iPtrsMeasure[i];
                nonzero |= (angles.val[i] != 0);
+               printf(" %g", angles.val[i]/M_PI*180);
        }
+       printf("\n");
        if (nonzero) {
                mcl->update(mcl, &angles);
                mcl->normalize(mcl);
@@ -308,16 +388,20 @@ static void mdlOutputs(SimStruct *S, int_T tid)
     double *bitmap = ssGetOutputPortRealSignal(S,OUTPUT_BITMAP);
     InputRealPtrsType iPtrsMove = ssGetInputPortRealSignalPtrs(S, INPUT_MOVE);
     InputRealPtrsType iPtrsMeasure = ssGetInputPortRealSignalPtrs(S, INPUT_MEASURE);
+    int_T             iMeasureWidth = ssGetInputPortWidth(S,INPUT_MEASURE);
+
 
     int moveTid = ssGetInputPortSampleTimeIndex(S,INPUT_MOVE);
     int measureTid = ssGetInputPortSampleTimeIndex(S,INPUT_MEASURE);
 
     int i;
-
-/*     ssPrintf("Output %d %g\n", tid, ssGetT(S)); */
+    if (ssIsContinuousTask(S, tid)) {
+           printf("Continuous ");
+    }
+    ssPrintf("Output %d %g\n", tid, ssGetT(S));
 /*     ssPrintf("=============================================\n"); */
     if (ssIsSampleHit(S, moveTid, tid)) {
-/*         ssPrintf("move \n"); */
+           ssPrintf("move \n");
            do_move(mcl, iPtrsMove);
 /*             if (moveTid == measureTid ||  */
 /*                 ssIsSpecialSampleHit(S, measureTid, moveTid, tid)) { */
@@ -327,8 +411,8 @@ static void mdlOutputs(SimStruct *S, int_T tid)
         }
     
     if (ssIsSampleHit(S, measureTid, tid)) {
-/*         ssPrintf("measure \n");  */
-           do_measure(mcl, iPtrsMeasure);
+           ssPrintf("measure \n");
+           do_measure(mcl, iPtrsMeasure, iMeasureWidth);
 /*         if (moveTid == measureTid ||  */
 /*                 ssIsSpecialSampleHit(S, measureTid, moveTid, tid)) { */
 /*                 ssPrintf("measure both \n"); */