更改定位底层逻辑代码

GaussianFilter/CGaussianFilter.cpp

@@ -0,0 +1,160 @@
+//
+// File: CGaussianFilter.cpp
+//
+// MATLAB Coder version            : 4.3
+// C/C++ source code generated on  : 12-Jan-2022 14:46:32
+//
+
+// Include Files
+#include "CGaussianFilter.h"
+#include "GaussianFilter_data.h"
+#include "GaussianFilter_emxutil.h"
+#include "conv2AXPY.h"
+#include <math.h>
+
+// Function Definitions
+
+//
+// Arguments    : void
+// Return Type  : void
+//
+CGaussianFilter::~CGaussianFilter()
+{
+  //omp_destroy_nest_lock(&emlrtNestLockGlobal);
+}
+
+//
+// Arguments    : void
+// Return Type  : void
+//
+CGaussianFilter::CGaussianFilter()
+{
+  //omp_init_nest_lock(&emlrtNestLockGlobal);
+}
+
+//
+// Arguments    : const emxArray_real_T *x
+//                emxArray_real_T *xlpf
+// Return Type  : void
+//
+void CGaussianFilter::GaussianFilter(const emxArray_real_T *x, emxArray_real_T
+  *xlpf)
+{
+  emxArray_real_T *work;
+  int32_T rank;
+  int32_T k;
+  static const real_T s[25] = { 0.22354978351691851, 0.0, 0.0, 0.0, 0.0, 0.0,
+    3.1892324657149476E-17, 0.0, 0.0, 0.0, 0.0, 0.0, 8.1422304240527224E-18, 0.0,
+    0.0, 0.0, 0.0, 0.0, 6.5350494864904178E-19, 0.0, 0.0, 0.0, 0.0, 0.0,
+    2.4277338668394078E-34 };
+
+  int32_T nb;
+  int32_T i;
+  int32_T u1;
+  int32_T jlo;
+  int32_T ix;
+  static const real_T hrow[5] = { 0.1200783842432135, 0.23388075658535037,
+    0.29208171834287255, 0.23388075658535035, 0.1200783842432135 };
+
+  emxInit_real_T(&work, 2);
+  if (25 <= x->size[1]) {
+    frexp(2.4277338668394078E-34, &rank);
+    rank = 0;
+    for (k = 0; k < 5; k++) {
+      if (s[k + 5 * k] > 2.1382117680737565E-49) {
+        rank++;
+      }
+    }
+
+    if (rank == 1) {
+      nb = x->size[1];
+      i = work->size[0] * work->size[1];
+      work->size[0] = 1;
+      work->size[1] = x->size[1];
+      emxEnsureCapacity_real_T(work, i);
+      rank = x->size[1];
+      for (i = 0; i < rank; i++) {
+        work->data[i] = 0.0;
+      }
+
+      i = xlpf->size[0] * xlpf->size[1];
+      xlpf->size[0] = 1;
+      xlpf->size[1] = x->size[1];
+      emxEnsureCapacity_real_T(xlpf, i);
+      rank = x->size[1];
+      for (i = 0; i < rank; i++) {
+        xlpf->data[i] = 0.0;
+      }
+
+      for (rank = 0; rank < nb; rank++) {
+        work->data[rank] += 0.29208171834287244 * x->data[rank];
+      }
+
+      u1 = x->size[1] + 2;
+      if (5 < u1) {
+        u1 = 5;
+      }
+
+      for (k = 1; k <= u1; k++) {
+        if (k - 2 > 0) {
+          jlo = k - 3;
+        } else {
+          jlo = 0;
+        }
+
+        rank = (nb + k) - 3;
+        if (rank > nb) {
+          rank = nb;
+        }
+
+        rank -= jlo;
+        if (rank >= 1) {
+          ix = (jlo - k) + 3;
+          i = rank - 1;
+          for (rank = 0; rank <= i; rank++) {
+            xlpf->data[jlo] += hrow[k - 1] * work->data[ix];
+            ix++;
+            jlo++;
+          }
+        }
+      }
+    } else {
+      conv2AXPYSameCMP(x, xlpf);
+    }
+  } else {
+    conv2AXPYSameCMP(x, xlpf);
+  }
+
+  emxFree_real_T(&work);
+}
+
+void CGaussianFilter::SetPdoa1dEnable(bool bEnable)
+{
+	s_enabled_pdoa1d = bEnable;
+}
+
+bool CGaussianFilter::GetPdoa1dEnable()
+{
+	return s_enabled_pdoa1d;
+}
+
+void CGaussianFilter::SetPdoa1dDist(double dist)
+{
+	s_pdoa1d_act_dist = dist;
+}
+
+double CGaussianFilter::GetPdoa1dDist()
+{
+	return 25;
+	//return s_pdoa1d_act_dist;
+}
+
+bool CGaussianFilter::s_enabled_pdoa1d = false;
+
+double CGaussianFilter::s_pdoa1d_act_dist = 0.0;
+
+//
+// File trailer for CGaussianFilter.cpp
+//
+// [EOF]
+//

GaussianFilter/CGaussianFilter.h

@@ -0,0 +1,39 @@
+//
+// File: CGaussianFilter.h
+//
+// MATLAB Coder version            : 4.3
+// C/C++ source code generated on  : 12-Jan-2022 14:46:32
+//
+#ifndef CGAUSSIANFILTER_H
+#define CGAUSSIANFILTER_H
+
+// Include Files
+#include <cstddef>
+#include <cstdlib>
+#include "rtwtypes.h"
+#include <omp.h>
+#include "GaussianFilter_types.h"
+
+// Type Definitions
+class CGaussianFilter
+{
+ public:
+  CGaussianFilter();
+  ~CGaussianFilter();
+  void GaussianFilter(const emxArray_real_T *x, emxArray_real_T *xlpf);
+  static void SetPdoa1dEnable(bool bEnable);
+  static bool GetPdoa1dEnable();
+  static void SetPdoa1dDist(double dist);
+  static double GetPdoa1dDist();
+private:
+	static bool s_enabled_pdoa1d;
+	static double s_pdoa1d_act_dist; // PDOA一维中高斯滤波的生效范围
+};
+
+#endif
+
+//
+// File trailer for CGaussianFilter.h
+//
+// [EOF]
+//

GaussianFilter/GaussianFilter_data.cpp

@@ -0,0 +1,19 @@
+//
+// File: GaussianFilter_data.cpp
+//
+// MATLAB Coder version            : 4.3
+// C/C++ source code generated on  : 12-Jan-2022 14:46:32
+//
+
+// Include Files
+#include "GaussianFilter_data.h"
+#include "CGaussianFilter.h"
+
+// Variable Definitions
+omp_nest_lock_t emlrtNestLockGlobal;
+
+//
+// File trailer for GaussianFilter_data.cpp
+//
+// [EOF]
+//

GaussianFilter/GaussianFilter_data.h

@@ -0,0 +1,26 @@
+//
+// File: GaussianFilter_data.h
+//
+// MATLAB Coder version            : 4.3
+// C/C++ source code generated on  : 12-Jan-2022 14:46:32
+//
+#ifndef GAUSSIANFILTER_DATA_H
+#define GAUSSIANFILTER_DATA_H
+
+// Include Files
+#include <cstddef>
+#include <cstdlib>
+#include "rtwtypes.h"
+#include <omp.h>
+#include "GaussianFilter_types.h"
+
+// Variable Declarations
+extern omp_nest_lock_t emlrtNestLockGlobal;
+
+#endif
+
+//
+// File trailer for GaussianFilter_data.h
+//
+// [EOF]
+//

GaussianFilter/GaussianFilter_emxAPI.cpp

@@ -0,0 +1,127 @@
+//
+// File: GaussianFilter_emxAPI.cpp
+//
+// MATLAB Coder version            : 4.3
+// C/C++ source code generated on  : 12-Jan-2022 14:46:32
+//
+
+// Include Files
+#include "GaussianFilter_emxAPI.h"
+#include "CGaussianFilter.h"
+#include "GaussianFilter_emxutil.h"
+#include <cstdlib>
+
+// Function Definitions
+
+//
+// Arguments    : int32_T numDimensions
+//                const int32_T *size
+// Return Type  : emxArray_real_T *
+//
+emxArray_real_T *emxCreateND_real_T(int32_T numDimensions, const int32_T *size)
+{
+  emxArray_real_T *emx;
+  int32_T numEl;
+  int32_T i;
+  emxInit_real_T(&emx, numDimensions);
+  numEl = 1;
+  for (i = 0; i < numDimensions; i++) {
+    numEl *= size[i];
+    emx->size[i] = size[i];
+  }
+
+  emx->data = (real_T *)std::calloc(static_cast<uint32_T>(numEl), sizeof(real_T));
+  emx->numDimensions = numDimensions;
+  emx->allocatedSize = numEl;
+  return emx;
+}
+
+//
+// Arguments    : real_T *data
+//                int32_T numDimensions
+//                const int32_T *size
+// Return Type  : emxArray_real_T *
+//
+emxArray_real_T *emxCreateWrapperND_real_T(real_T *data, int32_T numDimensions,
+  const int32_T *size)
+{
+  emxArray_real_T *emx;
+  int32_T numEl;
+  int32_T i;
+  emxInit_real_T(&emx, numDimensions);
+  numEl = 1;
+  for (i = 0; i < numDimensions; i++) {
+    numEl *= size[i];
+    emx->size[i] = size[i];
+  }
+
+  emx->data = data;
+  emx->numDimensions = numDimensions;
+  emx->allocatedSize = numEl;
+  emx->canFreeData = false;
+  return emx;
+}
+
+//
+// Arguments    : real_T *data
+//                int32_T rows
+//                int32_T cols
+// Return Type  : emxArray_real_T *
+//
+emxArray_real_T *emxCreateWrapper_real_T(real_T *data, int32_T rows, int32_T
+  cols)
+{
+  emxArray_real_T *emx;
+  emxInit_real_T(&emx, 2);
+  emx->size[0] = rows;
+  emx->size[1] = cols;
+  emx->data = data;
+  emx->numDimensions = 2;
+  emx->allocatedSize = rows * cols;
+  emx->canFreeData = false;
+  return emx;
+}
+
+//
+// Arguments    : int32_T rows
+//                int32_T cols
+// Return Type  : emxArray_real_T *
+//
+emxArray_real_T *emxCreate_real_T(int32_T rows, int32_T cols)
+{
+  emxArray_real_T *emx;
+  int32_T numEl;
+  emxInit_real_T(&emx, 2);
+  emx->size[0] = rows;
+  numEl = rows * cols;
+  emx->size[1] = cols;
+  emx->data = (real_T *)std::calloc(static_cast<uint32_T>(numEl), sizeof(real_T));
+  emx->numDimensions = 2;
+  emx->allocatedSize = numEl;
+  return emx;
+}
+
+//
+// Arguments    : emxArray_real_T *emxArray
+// Return Type  : void
+//
+void emxDestroyArray_real_T(emxArray_real_T *emxArray)
+{
+  emxFree_real_T(&emxArray);
+}
+
+//
+// Arguments    : emxArray_real_T **pEmxArray
+//                int32_T numDimensions
+// Return Type  : void
+//
+void emxInitArray_real_T(emxArray_real_T **pEmxArray, int32_T numDimensions)
+{
+  emxInit_real_T(pEmxArray, numDimensions);
+}
+
+//
+// File trailer for GaussianFilter_emxAPI.cpp
+//
+// [EOF]
+//

GaussianFilter/GaussianFilter_emxAPI.h

@@ -0,0 +1,35 @@
+//
+// File: GaussianFilter_emxAPI.h
+//
+// MATLAB Coder version            : 4.3
+// C/C++ source code generated on  : 12-Jan-2022 14:46:32
+//
+#ifndef GAUSSIANFILTER_EMXAPI_H
+#define GAUSSIANFILTER_EMXAPI_H
+
+// Include Files
+#include <cstddef>
+#include <cstdlib>
+#include "rtwtypes.h"
+#include <omp.h>
+#include "GaussianFilter_types.h"
+
+// Function Declarations
+extern emxArray_real_T *emxCreateND_real_T(int32_T numDimensions, const int32_T *
+  size);
+extern emxArray_real_T *emxCreateWrapperND_real_T(real_T *data, int32_T
+  numDimensions, const int32_T *size);
+extern emxArray_real_T *emxCreateWrapper_real_T(real_T *data, int32_T rows,
+  int32_T cols);
+extern emxArray_real_T *emxCreate_real_T(int32_T rows, int32_T cols);
+extern void emxDestroyArray_real_T(emxArray_real_T *emxArray);
+extern void emxInitArray_real_T(emxArray_real_T **pEmxArray, int32_T
+  numDimensions);
+
+#endif
+
+//
+// File trailer for GaussianFilter_emxAPI.h
+//
+// [EOF]
+//

GaussianFilter/GaussianFilter_emxutil.cpp

@@ -0,0 +1,105 @@
+//
+// File: GaussianFilter_emxutil.cpp
+//
+// MATLAB Coder version            : 4.3
+// C/C++ source code generated on  : 12-Jan-2022 14:46:32
+//
+
+// Include Files
+#include "GaussianFilter_emxutil.h"
+#include "CGaussianFilter.h"
+#include <cstdlib>
+#include <cstring>
+
+// Function Definitions
+
+//
+// Arguments    : emxArray_real_T *emxArray
+//                int32_T oldNumel
+// Return Type  : void
+//
+void emxEnsureCapacity_real_T(emxArray_real_T *emxArray, int32_T oldNumel)
+{
+  int32_T newNumel;
+  int32_T i;
+  void *newData;
+  if (oldNumel < 0) {
+    oldNumel = 0;
+  }
+
+  newNumel = 1;
+  for (i = 0; i < emxArray->numDimensions; i++) {
+    newNumel *= emxArray->size[i];
+  }
+
+  if (newNumel > emxArray->allocatedSize) {
+    i = emxArray->allocatedSize;
+    if (i < 16) {
+      i = 16;
+    }
+
+    while (i < newNumel) {
+      if (i > 1073741823) {
+        i = MAX_int32_T;
+      } else {
+        i *= 2;
+      }
+    }
+
+    newData = std::calloc(static_cast<uint32_T>(i), sizeof(real_T));
+    if (emxArray->data != NULL) {
+      std::memcpy(newData, emxArray->data, sizeof(real_T) * oldNumel);
+      if (emxArray->canFreeData) {
+        std::free(emxArray->data);
+      }
+    }
+
+    emxArray->data = (real_T *)newData;
+    emxArray->allocatedSize = i;
+    emxArray->canFreeData = true;
+  }
+}
+
+//
+// Arguments    : emxArray_real_T **pEmxArray
+// Return Type  : void
+//
+void emxFree_real_T(emxArray_real_T **pEmxArray)
+{
+  if (*pEmxArray != (emxArray_real_T *)NULL) {
+    if (((*pEmxArray)->data != (real_T *)NULL) && (*pEmxArray)->canFreeData) {
+      std::free((*pEmxArray)->data);
+    }
+
+    std::free((*pEmxArray)->size);
+    std::free(*pEmxArray);
+    *pEmxArray = (emxArray_real_T *)NULL;
+  }
+}
+
+//
+// Arguments    : emxArray_real_T **pEmxArray
+//                int32_T numDimensions
+// Return Type  : void
+//
+void emxInit_real_T(emxArray_real_T **pEmxArray, int32_T numDimensions)
+{
+  emxArray_real_T *emxArray;
+  int32_T i;
+  *pEmxArray = (emxArray_real_T *)std::malloc(sizeof(emxArray_real_T));
+  emxArray = *pEmxArray;
+  emxArray->data = (real_T *)NULL;
+  emxArray->numDimensions = numDimensions;
+  emxArray->size = (int32_T *)std::malloc(sizeof(int32_T) * numDimensions);
+  emxArray->allocatedSize = 0;
+  emxArray->canFreeData = true;
+  for (i = 0; i < numDimensions; i++) {
+    emxArray->size[i] = 0;
+  }
+}
+
+//
+// File trailer for GaussianFilter_emxutil.cpp
+//
+// [EOF]
+//

GaussianFilter/GaussianFilter_emxutil.h

@@ -0,0 +1,28 @@
+//
+// File: GaussianFilter_emxutil.h
+//
+// MATLAB Coder version            : 4.3
+// C/C++ source code generated on  : 12-Jan-2022 14:46:32
+//
+#ifndef GAUSSIANFILTER_EMXUTIL_H
+#define GAUSSIANFILTER_EMXUTIL_H
+
+// Include Files
+#include <cstddef>
+#include <cstdlib>
+#include "rtwtypes.h"
+#include <omp.h>
+#include "GaussianFilter_types.h"
+
+// Function Declarations
+extern void emxEnsureCapacity_real_T(emxArray_real_T *emxArray, int32_T oldNumel);
+extern void emxFree_real_T(emxArray_real_T **pEmxArray);
+extern void emxInit_real_T(emxArray_real_T **pEmxArray, int32_T numDimensions);
+
+#endif
+
+//
+// File trailer for GaussianFilter_emxutil.h
+//
+// [EOF]
+//

GaussianFilter/GaussianFilter_types.h

@@ -0,0 +1,29 @@
+//
+// File: GaussianFilter_types.h
+//
+// MATLAB Coder version            : 4.3
+// C/C++ source code generated on  : 12-Jan-2022 14:46:32
+//
+#ifndef GAUSSIANFILTER_TYPES_H
+#define GAUSSIANFILTER_TYPES_H
+
+// Include Files
+#include "rtwtypes.h"
+
+// Type Definitions
+struct emxArray_real_T
+{
+  real_T *data;
+  int32_T *size;
+  int32_T allocatedSize;
+  int32_T numDimensions;
+  bool canFreeData;
+};
+
+#endif
+
+//
+// File trailer for GaussianFilter_types.h
+//
+// [EOF]
+//

GaussianFilter/conv2AXPY.cpp

@@ -0,0 +1,101 @@
+//
+// File: conv2AXPY.cpp
+//
+// MATLAB Coder version            : 4.3
+// C/C++ source code generated on  : 12-Jan-2022 14:46:32
+//
+
+// Include Files
+#include "conv2AXPY.h"
+#include "CGaussianFilter.h"
+#include "GaussianFilter_emxutil.h"
+
+// Function Definitions
+
+//
+// Arguments    : const emxArray_real_T *a
+//                emxArray_real_T *c
+// Return Type  : void
+//
+void conv2AXPYSameCMP(const emxArray_real_T *a, emxArray_real_T *c)
+{
+  int32_T na;
+  int32_T jcut;
+  int32_T ub_loop;
+  int32_T j;
+  int32_T imax;
+  int32_T imin;
+  real_T bij;
+  int32_T jbmax;
+  int32_T jbmin;
+  real_T cj;
+  int32_T jb;
+  int32_T ib;
+  static const real_T b[25] = { 0.014418818362460822, 0.028084023356349175,
+    0.0350727008055935, 0.028084023356349175, 0.014418818362460822,
+    0.028084023356349175, 0.054700208300935887, 0.068312293270780214,
+    0.054700208300935887, 0.028084023356349175, 0.0350727008055935,
+    0.068312293270780214, 0.085311730190125085, 0.068312293270780214,
+    0.0350727008055935, 0.028084023356349175, 0.054700208300935887,
+    0.068312293270780214, 0.054700208300935887, 0.028084023356349175,
+    0.014418818362460822, 0.028084023356349175, 0.0350727008055935,
+    0.028084023356349175, 0.014418818362460822 };
+
+  int32_T i;
+  na = a->size[1];
+  if (a->size[1] == 0) {
+    c->size[0] = 1;
+    c->size[1] = 0;
+  } else {
+    jcut = a->size[1] - 2;
+    ub_loop = c->size[0] * c->size[1];
+    c->size[0] = 1;
+    c->size[1] = a->size[1];
+    emxEnsureCapacity_real_T(c, ub_loop);
+    ub_loop = a->size[1] - 1;
+
+#pragma omp parallel for \
+ num_threads(omp_get_max_threads()) \
+ private(imax,imin,bij,jbmax,jbmin,cj,jb,ib,i)
+
+    for (j = 0; j <= ub_loop; j++) {
+      cj = 0.0;
+      if (j + 1 > 2) {
+        jbmin = 0;
+      } else {
+        jbmin = 2 - j;
+      }
+
+      if (j + 1 < jcut) {
+        jbmax = 4;
+      } else {
+        jbmax = (na - j) + 1;
+      }
+
+      for (jb = jbmin; jb <= jbmax; jb++) {
+        for (ib = 0; ib < 5; ib++) {
+          bij = b[(5 * (4 - jb) - ib) + 4];
+          if (ib < 2) {
+            imin = 3 - ib;
+            imax = 1;
+          } else {
+            imin = 1;
+            imax = 3 - ib;
+          }
+
+          for (i = imin; i <= imax; i++) {
+            cj += bij * a->data[(j + jb) - 2];
+          }
+        }
+      }
+
+      c->data[j] = cj;
+    }
+  }
+}
+
+//
+// File trailer for conv2AXPY.cpp
+//
+// [EOF]
+//

GaussianFilter/conv2AXPY.h

@@ -0,0 +1,26 @@
+//
+// File: conv2AXPY.h
+//
+// MATLAB Coder version            : 4.3
+// C/C++ source code generated on  : 12-Jan-2022 14:46:32
+//
+#ifndef CONV2AXPY_H
+#define CONV2AXPY_H
+
+// Include Files
+#include <cstddef>
+#include <cstdlib>
+#include "rtwtypes.h"
+#include <omp.h>
+#include "GaussianFilter_types.h"
+
+// Function Declarations
+extern void conv2AXPYSameCMP(const emxArray_real_T *a, emxArray_real_T *c);
+
+#endif
+
+//
+// File trailer for conv2AXPY.h
+//
+// [EOF]
+//

GaussianFilter/rtwtypes.h

@@ -0,0 +1,43 @@
+/*
+ * File: rtwtypes.h
+ *
+ * MATLAB Coder version            : 4.3
+ * C/C++ source code generated on  : 12-Jan-2022 14:46:32
+ */
+
+#ifndef RTWTYPES_H
+#define RTWTYPES_H
+
+/*=======================================================================*
+ * Fixed width word size data types:                                     *
+ *   int64_T                      - signed 64 bit integers               *
+ *   uint64_T                     - unsigned 64 bit integers             *
+ *=======================================================================*/
+#if defined(__APPLE__)
+# ifndef INT64_T
+#  define INT64_T                      long
+#  define FMT64                        "l"
+#  if defined(__LP64__) && !defined(INT_TYPE_64_IS_LONG)
+#    define INT_TYPE_64_IS_LONG
+#  endif
+# endif
+#endif
+
+#if defined(__APPLE__)
+# ifndef UINT64_T
+#  define UINT64_T                     unsigned long
+#  define FMT64                        "l"
+#  if defined(__LP64__) && !defined(INT_TYPE_64_IS_LONG)
+#    define INT_TYPE_64_IS_LONG
+#  endif
+# endif
+#endif
+
+#include "tmwtypes.h"
+#endif
+
+/*
+ * File trailer for rtwtypes.h
+ *
+ * [EOF]
+ */

GaussianFilter/tmwtypes.h

@@ -0,0 +1,849 @@
+/*
+ * Copyright 1984-2018 The MathWorks, Inc.
+ */
+
+#if defined(_MSC_VER)
+# pragma once
+#endif
+#if defined(__GNUC__) && (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ > 3))
+# pragma once
+#endif
+
+#ifndef tmwtypes_h
+#define tmwtypes_h
+
+#ifndef __TMWTYPES__
+#define __TMWTYPES__
+/*
+ * File    : tmwtypes.h
+ * Abstract:
+ *      Data types for use with MATLAB/SIMULINK and the Real-Time Workshop.
+ *
+ *      When compiling stand-alone model code, data types can be overridden
+ *      via compiler switches.
+ *
+ *      Define NO_FLOATS to eliminate reference to real_T, etc.
+ */
+
+#include <limits.h>
+
+/* __STDC_VERSION__ version check below means "check for a C99 compiler".
+
+   Visual Studio (checked on versions 2015 and 2017) does
+   not define __STDC_VERSION__, however  it has stdbool.h available,
+   thus a separate check for _MSC_VER below.
+ */
+#if defined(__APPLE_CC__) \
+    || (defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L)) \
+    || (defined(_MSC_VER) && (_MSC_VER >= 1900))
+#include <stdbool.h>
+#endif
+
+#define LOGICAL_IS_A_TYPE
+#define SPARSE_GENERALIZATION
+
+#ifdef NO_FLOATS
+# define double double_not_allowed
+# define float  float_not_allowed
+#endif /*NO_FLOATS*/
+
+#ifndef NO_FLOATS
+
+#ifndef __MWERKS__
+# ifdef __STDC__
+#  include <float.h>
+# else
+#  define FLT_MANT_DIG 24
+#  define DBL_MANT_DIG 53
+# endif
+#endif
+
+#endif /*NO_FLOATS*/
+
+/*
+ *      The following data types cannot be overridden when building MEX files.
+ */
+#ifdef MATLAB_MEX_FILE
+# undef CHARACTER_T
+# undef INTEGER_T
+# undef BOOLEAN_T
+# undef REAL_T
+# undef TIME_T
+#endif
+
+/*
+ * The uchar_T, ushort_T and ulong_T types are needed for compilers which do 
+ * not allow defines to be specified, at the command line, with spaces in them.
+ */
+
+typedef unsigned char       uchar_T;
+typedef unsigned short      ushort_T;
+typedef unsigned long       ulong_T;
+
+#if (defined(_MSC_VER) && _MSC_VER >= 1500)      \
+    || defined(__x86_64__) || defined(__LP64__)  \
+    || defined(__LCC64__)
+    
+typedef unsigned long long  ulonglong_T;
+#endif
+
+
+
+/*=======================================================================*
+ * Fixed width word size data types:                                     *
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     *
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   *
+ *   real32_T, real64_T           - 32 and 64 bit floating point numbers *
+ *=======================================================================*/
+
+/* When used with Real Time Workshop generated code, this
+ * header file can be used with a variety of compilers.
+ *
+ * The compiler could be for an 8 bit embedded processor that
+ * only had 8 bits per integer and 16 bits per long.
+ * In that example, a 32 bit integer size is not even available.
+ * This header file should be robust to that.   
+ *
+ * For the case of an 8 bit processor, the preprocessor
+ * may be limited to 16 bit math like its target.  That limitation 
+ * would mean that 32 bit comparisons can't be done accurately.  
+ * To increase robustness to this, comparisons are done against
+ * smaller values first.  An inaccurate 32 bit comparison isn't
+ * attempted if the 16 bit comparison has already succeeded.
+ *
+ * Limitations on preprocessor math can also be stricter than
+ * for the target.  There are known cases where a compiler
+ * targeting processors with 64 bit longs can't do accurate
+ * preprocessor comparisons on more than 32 bits.  
+ */
+
+/* Determine the number of bits for int, long, short, and char.
+ * If one fails to be determined, set the number of bits to -1
+ */
+
+#ifndef TMW_BITS_PER_INT
+# if   INT_MAX  == 0x7FL
+#  define TMW_BITS_PER_INT 8
+# elif INT_MAX  == 0x7FFFL
+#  define TMW_BITS_PER_INT 16
+# elif INT_MAX  == 0x7FFFFFFFL
+#  define TMW_BITS_PER_INT 32
+# else
+#  define TMW_BITS_PER_INT -1
+# endif
+#endif
+
+#ifndef TMW_BITS_PER_LONG
+# if   LONG_MAX  == 0x7FL
+#  define TMW_BITS_PER_LONG 8
+# elif LONG_MAX  == 0x7FFFL
+#  define TMW_BITS_PER_LONG 16
+# elif LONG_MAX  == 0x7FFFFFFFL
+#  define TMW_BITS_PER_LONG 32
+# else
+#  define TMW_BITS_PER_LONG -1
+# endif
+#endif
+
+#ifndef TMW_BITS_PER_SHRT
+# if   SHRT_MAX  == 0x7FL
+#  define TMW_BITS_PER_SHRT 8
+# elif SHRT_MAX  == 0x7FFFL
+#  define TMW_BITS_PER_SHRT 16
+# elif SHRT_MAX  == 0x7FFFFFFFL
+#  define TMW_BITS_PER_SHRT 32
+# else
+#  define TMW_BITS_PER_SHRT -1
+# endif
+#endif
+
+#ifndef TMW_BITS_PER_SCHAR
+# if   SCHAR_MAX  == 0x7FL
+#  define TMW_BITS_PER_SCHAR 8
+# elif SCHAR_MAX  == 0x7FFFL
+#  define TMW_BITS_PER_SCHAR 16
+# elif SCHAR_MAX  == 0x7FFFFFFFL
+#  define TMW_BITS_PER_SCHAR 32
+# else
+#  define TMW_BITS_PER_SCHAR -1
+# endif
+#endif
+
+#ifndef TMW_CHAR_SIGNED
+# if SCHAR_MAX == CHAR_MAX
+#  define TMW_CHAR_SIGNED 1
+# else
+#  define TMW_CHAR_SIGNED 0
+# endif
+#endif
+
+/* It is common for one or more of the integer types
+ * to be the same size.  For example, on many embedded
+ * processors, both shorts and ints are 16 bits.  On
+ * processors used for workstations, it is quite common
+ * for both int and long to be 32 bits.  
+ *   When there is more than one choice for typdef'ing
+ * a portable type like int16_T or uint32_T, in
+ * concept, it should not matter which choice is made.
+ * However, some style guides and some code checking
+ * tools do identify and complain about seemingly
+ * irrelevant differences.  For example, a code
+ * checking tool may complain about an implicit
+ * conversion from int to short even though both
+ * are 16 bits.  To reduce these types of
+ * complaints, it is best to make int the
+ * preferred choice when more than one is available.
+ */
+
+#ifndef INT8_T
+# if   TMW_BITS_PER_INT   == 8
+#  define  INT8_T int
+# elif TMW_BITS_PER_LONG  == 8
+#  define  INT8_T long
+# elif TMW_BITS_PER_SCHAR == 8
+#  define  INT8_T signed char
+# elif TMW_BITS_PER_SHRT  == 8
+#  define  INT8_T short
+# endif
+#endif
+#ifdef INT8_T
+ typedef INT8_T int8_T;
+#endif
+
+#ifndef UINT8_T
+# if   TMW_BITS_PER_INT   == 8
+#  define  UINT8_T unsigned int
+# elif TMW_BITS_PER_LONG  == 8
+#  define  UINT8_T unsigned long
+# elif TMW_BITS_PER_SCHAR == 8
+#  define  UINT8_T unsigned char
+# elif TMW_BITS_PER_SHRT  == 8
+#  define  UINT8_T unsigned short
+# endif
+#endif
+#ifdef UINT8_T
+ typedef UINT8_T uint8_T;
+#endif
+
+
+#ifndef INT16_T
+# if   TMW_BITS_PER_INT   == 16
+#  define  INT16_T int
+# elif TMW_BITS_PER_LONG  == 16
+#  define  INT16_T long
+# elif TMW_BITS_PER_SCHAR == 16
+#  define  INT16_T signed char
+# elif TMW_BITS_PER_SHRT  == 16
+#  define  INT16_T short
+# endif
+#endif
+#ifdef INT16_T
+ typedef INT16_T int16_T;
+#endif
+
+
+#ifndef UINT16_T
+# if   TMW_BITS_PER_INT   == 16
+#  define  UINT16_T unsigned int
+# elif TMW_BITS_PER_LONG  == 16
+#  define  UINT16_T unsigned long
+# elif TMW_BITS_PER_SCHAR == 16
+#  define  UINT16_T unsigned char
+# elif TMW_BITS_PER_SHRT  == 16
+#  define  UINT16_T unsigned short
+# endif
+#endif
+#ifdef UINT16_T
+ typedef UINT16_T uint16_T;
+#endif
+
+
+#ifndef INT32_T
+# if   TMW_BITS_PER_INT   == 32
+#  define  INT32_T int
+# elif TMW_BITS_PER_LONG  == 32
+#  define  INT32_T long
+# elif TMW_BITS_PER_SCHAR == 32
+#  define  INT32_T signed char
+# elif TMW_BITS_PER_SHRT  == 32
+#  define  INT32_T short
+# endif
+#endif
+#ifdef INT32_T
+ typedef INT32_T int32_T;
+#endif
+
+
+#ifndef UINT32_T
+# if   TMW_BITS_PER_INT   == 32
+#  define  UINT32_T unsigned int
+# elif TMW_BITS_PER_LONG  == 32
+#  define  UINT32_T unsigned long
+# elif TMW_BITS_PER_SCHAR == 32
+#   define UINT32_T unsigned char
+# elif TMW_BITS_PER_SHRT  == 32
+#  define  UINT32_T unsigned short
+# endif
+#endif
+#ifdef UINT32_T
+ typedef UINT32_T uint32_T;
+#endif
+
+/* The following is used to emulate smaller integer types when only
+ * larger types are available.  For example, compilers for TI C3x/C4x DSPs 
+ * define char and short to be 32 bits, so 8 and 16 bits are not directly
+ * available.  This target is commonly used with RTW rapid prototyping.
+ * Other DSPs define char to be 16 bits, so 8 bits is not directly 
+ * available.
+ */
+#ifndef INT8_T
+# ifdef INT16_T
+#  define INT8_T INT16_T
+   typedef INT8_T int8_T;
+# else
+#  ifdef INT32_T
+#   define INT8_T INT32_T
+    typedef INT8_T int8_T;
+#  endif
+# endif
+#endif
+
+#ifndef UINT8_T
+# ifdef UINT16_T
+#   define UINT8_T UINT16_T
+    typedef UINT8_T uint8_T;
+# else
+#  ifdef UINT32_T
+#   define UINT8_T UINT32_T
+    typedef UINT8_T uint8_T;
+#  endif
+# endif
+#endif
+
+#ifndef INT16_T
+# ifdef INT32_T
+#   define INT16_T INT32_T
+    typedef INT16_T int16_T;
+# endif
+#endif
+
+#ifndef UINT16_T
+# ifdef UINT32_T
+#   define UINT16_T UINT32_T
+    typedef UINT16_T uint16_T;
+# endif
+#endif
+
+
+#ifndef NO_FLOATS
+
+#ifndef REAL32_T
+# ifndef __MWERKS__
+#  if FLT_MANT_DIG >= 23
+#   define REAL32_T float
+#  endif
+# else
+#  define REAL32_T float
+# endif
+#endif
+#ifdef REAL32_T
+ typedef REAL32_T real32_T;
+#endif
+
+
+#ifndef REAL64_T
+# ifndef __MWERKS__
+#  if DBL_MANT_DIG >= 52
+#   define REAL64_T double
+#  endif
+# else
+#  define REAL64_T double
+# endif
+#endif
+#ifdef REAL64_T
+ typedef REAL64_T real64_T;
+#endif
+
+#endif /* NO_FLOATS*/
+
+/*=======================================================================*
+ * Fixed width word size data types:                                     *
+ *   int64_T                      - signed 64 bit integers               *
+ *   uint64_T                     - unsigned 64 bit integers             *
+ *=======================================================================*/
+
+
+
+#ifndef INT64_T
+# if defined(__APPLE__)
+#  define INT64_T long long
+#  define FMT64 "ll"
+#  if defined(__LP64__) && !defined(INT_TYPE_64_IS_LONG)
+#    define INT_TYPE_64_IS_LONG
+#  endif
+# elif (defined(__x86_64__) || defined(__LP64__))&& !defined(__MINGW64__)
+#  define INT64_T long
+#  define FMT64 "l"
+#  if !defined(INT_TYPE_64_IS_LONG)
+#    define INT_TYPE_64_IS_LONG
+#  endif
+# elif defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \
+                         || (defined(__WATCOMC__)  && __WATCOMC__  >= 1100)
+#  define INT64_T __int64
+#  define FMT64 "I64"
+# elif defined(__GNUC__) || defined(TMW_ENABLE_INT64) \
+                         || defined(__LCC64__)
+#  define INT64_T long long
+#  define FMT64 "ll"
+# endif
+#endif
+
+
+
+#if defined(INT64_T)
+# if defined(__GNUC__) && \
+    ((__GNUC__ > 2) || ((__GNUC__ == 2) && (__GNUC_MINOR__ >=9)))
+  __extension__
+# endif
+ typedef INT64_T int64_T;
+#endif
+
+
+
+#ifndef UINT64_T
+# if defined(__APPLE__)
+#  define UINT64_T unsigned long long
+#  define FMT64 "ll"
+#  if defined(__LP64__) && !defined(INT_TYPE_64_IS_LONG)
+#    define INT_TYPE_64_IS_LONG
+#  endif
+# elif (defined(__x86_64__) || defined(__LP64__))&& !defined(__MINGW64__)
+#  define UINT64_T unsigned long
+#  define FMT64 "l"
+#  if !defined(INT_TYPE_64_IS_LONG)
+#    define INT_TYPE_64_IS_LONG
+#  endif
+# elif defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \
+                         || (defined(__WATCOMC__)  && __WATCOMC__  >= 1100)
+#  define UINT64_T unsigned __int64
+#  define FMT64 "I64"
+# elif defined(__GNUC__) || defined(TMW_ENABLE_INT64) \
+                         || defined(__LCC64__)
+#  define UINT64_T unsigned long long
+#  define FMT64 "ll"
+# endif
+#endif
+
+#if defined(_WIN64) || (defined(__APPLE__) && defined(__LP64__)) \
+                    || defined(__x86_64__) \
+                    || defined(__LP64__)
+#  define INT_TYPE_64_IS_SUPPORTED
+#endif
+
+#if defined(UINT64_T)
+# if defined(__GNUC__) && \
+    ((__GNUC__ > 2) || ((__GNUC__ == 2) && (__GNUC_MINOR__ >=9)))
+  __extension__
+# endif
+ typedef UINT64_T uint64_T;
+#endif
+
+/*===========================================================================*
+ * Format string modifiers for using size_t variables in printf statements.  *
+ *===========================================================================*/
+
+#ifndef FMT_SIZE_T
+#  if (defined( __GNUC__ ) || defined(_STDC_C99))&& !defined(__MINGW64__)
+#    define FMT_SIZE_T "z"
+#  elif defined (__WATCOMC__)
+#    define FMT_SIZE_T "l"
+#  elif defined (_WIN32 )
+#    define FMT_SIZE_T "I"
+#  else
+#    define FMT_SIZE_T "l"
+#  endif
+#endif
+
+#ifndef FMT_PTRDIFF_T
+#  if defined(__APPLE__)
+#    define FMT_PTRDIFF_T "l"
+#  elif defined( __GNUC__ ) || defined(_STDC_C99)
+#    define FMT_PTRDIFF_T "t"
+#  elif defined (__WATCOMC__)
+#    define FMT_PTRDIFF_T "l"
+#  elif defined (_WIN32 )
+#    define FMT_PTRDIFF_T "I"
+#  else
+#    define FMT_PTRDIFF_T "l"
+#  endif
+#endif
+
+/*===========================================================================*
+ * General or logical data types where the word size is not guaranteed.      *
+ *  real_T     - possible settings include real32_T or real64_T              *
+ *  time_T     - possible settings include real32_T or real64_T              *
+ *  boolean_T                                                                *
+ *  char_T                                                                   *
+ *  int_T                                                                    *
+ *  uint_T                                                                   *
+ *  byte_T                                                                   *
+ *===========================================================================*/
+
+#ifndef NO_FLOATS
+
+#ifndef REAL_T
+# ifdef REAL64_T
+#  define REAL_T real64_T
+# else
+#  ifdef REAL32_T
+#   define REAL_T real32_T
+#  endif
+# endif
+#endif
+#ifdef REAL_T
+ typedef REAL_T real_T;
+#endif
+
+#ifndef TIME_T
+# ifdef REAL_T
+#  define TIME_T real_T
+# endif
+#endif
+#ifdef TIME_T
+ typedef TIME_T time_T;
+#endif
+
+#endif /* NO_FLOATS */
+
+#ifndef BOOLEAN_T
+# if defined(UINT8_T)
+#  define BOOLEAN_T UINT8_T
+# else
+#  define BOOLEAN_T unsigned int
+# endif
+#endif
+typedef BOOLEAN_T booleanT;
+
+
+#ifndef CHARACTER_T
+# define CHARACTER_T char
+#endif
+typedef CHARACTER_T char_T;
+
+
+#ifndef INTEGER_T
+# define INTEGER_T int
+#endif
+typedef INTEGER_T int_T;
+
+
+#ifndef UINTEGER_T
+# define UINTEGER_T unsigned
+#endif
+typedef UINTEGER_T uint_T;
+
+
+#ifndef BYTE_T
+# define BYTE_T unsigned char
+#endif
+typedef BYTE_T byte_T;
+
+
+/*===========================================================================*
+ * Define Complex Structures                                                 *
+ *===========================================================================*/
+#ifndef NO_FLOATS
+
+#ifndef CREAL32_T
+#  ifdef REAL32_T
+    typedef struct {
+      real32_T re, im;
+    } creal32_T;
+#    define CREAL32_T creal32_T
+#  endif
+#endif
+
+#ifndef CREAL64_T
+#  ifdef REAL64_T
+    typedef struct {
+      real64_T re, im;
+    } creal64_T;
+#    define CREAL64_T creal64_T
+#  endif
+#endif
+
+#ifndef CREAL_T
+#  ifdef REAL_T
+    typedef struct {
+      real_T re, im;
+    } creal_T;
+#    define CREAL_T creal_T
+#  endif
+#endif
+
+#endif /* NO_FLOATS */
+
+#ifndef CINT8_T
+#  ifdef INT8_T
+    typedef struct {
+      int8_T re, im;
+    } cint8_T;
+#    define CINT8_T cint8_T
+#  endif
+#endif
+
+#ifndef CUINT8_T
+#  ifdef UINT8_T
+    typedef struct {
+      uint8_T re, im;
+    } cuint8_T;
+#    define CUINT8_T cuint8_T
+#  endif
+#endif
+
+#ifndef CINT16_T
+#  ifdef INT16_T
+    typedef struct {
+      int16_T re, im;
+    } cint16_T;
+#    define CINT16_T cint16_T
+#  endif
+#endif
+
+#ifndef CUINT16_T
+#  ifdef UINT16_T
+    typedef struct {
+      uint16_T re, im;
+    } cuint16_T;
+#    define CUINT16_T cuint16_T
+#  endif
+#endif
+
+#ifndef CINT32_T
+#  ifdef INT32_T
+    typedef struct {
+      int32_T re, im;
+    } cint32_T;
+#    define CINT32_T cint32_T
+#  endif
+#endif
+
+#ifndef CUINT32_T
+#  ifdef UINT32_T
+    typedef struct {
+      uint32_T re, im;
+    } cuint32_T;
+#    define CUINT32_T cuint32_T
+#  endif
+#endif
+
+#ifndef CINT64_T
+#  ifdef INT64_T
+    typedef struct {
+      int64_T re, im;
+    } cint64_T;
+#    define CINT64_T cint64_T
+#  endif
+#endif
+
+#ifndef CUINT64_T
+#  ifdef UINT64_T
+    typedef struct {
+      uint64_T re, im;
+    } cuint64_T;
+#    define CUINT64_T cuint64_T
+#  endif
+#endif
+
+/*=======================================================================*
+ * Min and Max:                                                          *
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     *
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   *
+ *=======================================================================*/
+
+#define  MAX_int8_T      ((int8_T)(127))            /* 127  */
+#define  MIN_int8_T      ((int8_T)(-128))           /* -128 */
+#define  MAX_uint8_T     ((uint8_T)(255))           /* 255  */
+#define  MIN_uint8_T     ((uint8_T)(0))
+                           
+#define  MAX_int16_T     ((int16_T)(32767))         /* 32767 */
+#define  MIN_int16_T     ((int16_T)(-32768))        /* -32768 */
+#define  MAX_uint16_T    ((uint16_T)(65535))        /* 65535 */
+#define  MIN_uint16_T    ((uint16_T)(0))
+                           
+#define  MAX_int32_T     ((int32_T)(2147483647))    /* 2147483647  */
+#define  MIN_int32_T     ((int32_T)(-2147483647-1)) /* -2147483648 */
+#define  MAX_uint32_T    ((uint32_T)(0xFFFFFFFFU))  /* 4294967295  */
+#define  MIN_uint32_T    ((uint32_T)(0))
+
+#if defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \
+                      || (defined(__WATCOMC__)  && __WATCOMC__  >= 1100) \
+                      || defined(__LCC64__)
+#  ifdef INT64_T
+#    define  MAX_int64_T     ((int64_T)(9223372036854775807LL))
+#    define  MIN_int64_T     ((int64_T)(-9223372036854775807LL-1LL))
+#  endif
+#  ifdef UINT64_T
+#    define  MAX_uint64_T    ((uint64_T)(0xFFFFFFFFFFFFFFFFULL))
+#    define  MIN_uint64_T    ((uint64_T)(0))
+#  endif
+#else
+#  ifdef INT64_T
+#    ifdef INT_TYPE_64_IS_LONG
+#      define  MAX_int64_T     ((int64_T)(9223372036854775807L))
+#      define  MIN_int64_T     ((int64_T)(-9223372036854775807L-1L))
+#    else
+#      define  MAX_int64_T     ((int64_T)(9223372036854775807LL))
+#      define  MIN_int64_T     ((int64_T)(-9223372036854775807LL-1LL))
+#    endif
+#  endif
+#  ifdef UINT64_T
+#    ifdef INT_TYPE_64_IS_LONG
+#      define  MAX_uint64_T    ((uint64_T)(0xFFFFFFFFFFFFFFFFUL))
+#      define  MIN_uint64_T    ((uint64_T)(0))
+#    else
+#      define  MAX_uint64_T    ((uint64_T)(0xFFFFFFFFFFFFFFFFULL))
+#      define  MIN_uint64_T    ((uint64_T)(0))
+#    endif
+#  endif
+#endif
+
+#ifdef _MSC_VER
+/* Conversion from unsigned __int64 to double is not implemented in windows
+ * and results in a compile error, thus the value must first be cast to
+ * signed __int64, and then to double.
+ *
+ * If the 64 bit int value is greater than 2^63-1, which is the signed int64 max,
+ * the macro below provides a workaround for casting a uint64 value to a double
+ * in windows.
+ */
+#  define uint64_to_double(u) ( ((u) > _I64_MAX) ? \
+            (double)(__int64)((u) - _I64_MAX - 1) + (double)_I64_MAX + 1: \
+            (double)(__int64)(u) )
+
+/* The following inline function should only be used in the macro double_to_uint64,
+ * as it only handles the specfic range of double between 2^63 and 2^64-1 */
+__forceinline
+uint64_T double_to_uint64_helper(double d) {
+  union double_to_uint64_union_type {
+    double dd;
+    uint64_T i64;
+  } di;
+  di.dd = d;
+  return (((di.i64 & 0x000fffffffffffff) | 0x0010000000000000) << 11);
+}
+
+/* The largest double value that can be cast to uint64 in windows is the
+ * signed int64 max, which is 2^63-1. The macro below provides
+ * a workaround for casting large double values to uint64 in windows.
+ */
+/* The magic number 18446744073709551616.0 is 2^64 */
+/* The magic number 9223372036854775808.0 is 2^63 */
+#  define double_to_uint64(d) ( ((d) >= 18446744073709551616.0) ? \
+            0xffffffffffffffffULL : \
+            ((d) >= 0.0) ? \
+            ((d) >= 9223372036854775808.0) ? \
+            double_to_uint64_helper(d) : \
+            (unsigned __int64)(d) : \
+            0ULL )
+#else
+#  define uint64_to_double(u) ((double)(u))
+#  if defined(__BORLANDC__) || defined(__WATCOMC__) || defined(__TICCSC__)
+/* double_to_uint64 defined only for MSVC and UNIX */
+#  else
+#  define double_to_uint64(d) ( ((d) >= 18446744073709551616.0) ? \
+            (unsigned long long) 0xffffffffffffffffULL : \
+            ((d) >= 0) ? (unsigned long long)(d) : (unsigned long long) 0 )
+#  endif
+#endif
+
+#if !defined(__cplusplus) && !defined(__bool_true_false_are_defined)
+
+#ifndef _bool_T
+#define _bool_T
+
+typedef bool bool;
+
+#ifndef false
+#define false (0)
+#endif
+#ifndef true 
+#define true (1)
+#endif
+
+#endif /* _bool_T */
+
+#endif /* !__cplusplus */
+
+/* 
+ * This software assumes that the code is being compiled on a target using a 
+ * 2's complement representation for signed integer values.
+ */
+#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
+#error "This code must be compiled using a 2's complement representation for signed integer values"
+#endif
+
+/*
+ * Maximum length of a MATLAB identifier (function/variable/model)
+ * including the null-termination character.
+ */
+#define TMW_NAME_LENGTH_MAX 64
+
+/*
+ * Maximum values for indices and dimensions
+ */
+#include <stddef.h>
+
+#ifdef MX_COMPAT_32
+typedef int mwSize;
+typedef int mwIndex;
+typedef int mwSignedIndex;
+#else
+typedef size_t    mwSize;         /* unsigned pointer-width integer */
+typedef size_t    mwIndex;        /* unsigned pointer-width integer */
+typedef ptrdiff_t mwSignedIndex;  /* a signed pointer-width integer */
+#endif
+
+                                  /* for the individual dim */
+#ifndef SLSIZE_SLINDEX
+#define SLSIZE_SLINDEX
+typedef int SLIndex;
+typedef int SLSize;
+#endif
+
+/* for the total size */
+#define SLIndexType size_t
+#define INVALID_SIZET_VALUE   (std::numeric_limits<SLIndexType>::max())
+#define MAX_VALID_SIZET_VALUE   (std::numeric_limits<SLIndexType>::max() -1)
+
+
+#if (defined(_LP64) || defined(_WIN64)) && !defined(MX_COMPAT_32)
+/* Currently 2^48 based on hardware limitations */
+# define MWSIZE_MAX    281474976710655UL
+# define MWINDEX_MAX   281474976710655UL
+# define MWSINDEX_MAX  281474976710655L
+# define MWSINDEX_MIN -281474976710655L
+#else
+# define MWSIZE_MAX    2147483647UL
+# define MWINDEX_MAX   2147483647UL
+# define MWSINDEX_MAX  2147483647L
+# define MWSINDEX_MIN -2147483647L
+#endif
+#define MWSIZE_MIN    0UL
+#define MWINDEX_MIN   0UL
+
+/** UTF-16 character type */
+
+#if (defined(__cplusplus) && (__cplusplus >= 201103L)) || (defined(_HAS_CHAR16_T_LANGUAGE_SUPPORT) && _HAS_CHAR16_T_LANGUAGE_SUPPORT)
+typedef char16_t CHAR16_T;
+#define U16_STRING_LITERAL_PREFIX u
+#elif defined(_MSC_VER)
+typedef wchar_t CHAR16_T;
+#define U16_STRING_LITERAL_PREFIX L
+#else
+typedef UINT16_T CHAR16_T;
+#endif
+
+#endif  /* __TMWTYPES__ */
+
+#endif /* tmwtypes_h */

MovAvgFilter/CMovAvgFilter.cpp

@@ -0,0 +1,116 @@
+//
+// File: CMovAvgFilter.cpp
+//
+// MATLAB Coder version            : 4.3
+// C/C++ source code generated on  : 12-Jan-2022 14:14:11
+//
+
+// Include Files
+#include "CMovAvgFilter.h"
+
+// Function Definitions
+
+//
+// Arguments    : void
+// Return Type  : void
+//
+CMovAvgFilter::~CMovAvgFilter()
+{
+  // (no terminate code required)
+}
+
+void CMovAvgFilter::SetPdoa1dEnable(bool bEnable)
+{
+	s_enabled_pdoa1d = bEnable;
+}
+
+bool CMovAvgFilter::GetPdoa1dEnable()
+{
+	return s_enabled_pdoa1d;
+}
+
+void CMovAvgFilter::SetPdoa2dEnable(bool bEnable)
+{
+	s_enabled_pdoa2d = bEnable;
+}
+
+bool CMovAvgFilter::GetPdoa2dEnable()
+{
+	return s_enabled_pdoa2d;
+}
+
+//
+// Arguments    : void
+// Return Type  : void
+//
+CMovAvgFilter::CMovAvgFilter()
+{
+  this->SD_.pd = &this->pd_;
+  this->firstRun_not_empty_init();
+  this->MovAvgFilter_init();
+}
+
+//
+// Arguments    : real_T x
+// Return Type  : real_T
+//
+real_T CMovAvgFilter::MovAvgFilter(real_T x)
+{
+  real_T avg;
+  int32_T m;
+  if (!this->pd_.firstRun_not_empty) {
+    this->pd_.xbuf.size[0] = 11;
+    for (m = 0; m < 11; m++) {
+      this->pd_.xbuf.data[m] = x;
+    }
+
+    this->pd_.prevAvg = x;
+    this->pd_.firstRun_not_empty = true;
+  }
+
+  for (m = 0; m < 10; m++) {
+    this->pd_.xbuf.data[m] = this->pd_.xbuf.data[m + 1];
+  }
+
+  this->pd_.xbuf.data[10] = x;
+  avg = this->pd_.prevAvg + (x - this->pd_.xbuf.data[0]) / 10.0;
+  this->pd_.prevAvg = avg;
+  return avg;
+}
+
+//
+// Arguments    : void
+// Return Type  : void
+//
+void CMovAvgFilter::MovAvgFilter_init()
+{
+  this->pd_.xbuf.size[0] = 0;
+}
+
+bool CMovAvgFilter::s_enabled_pdoa1d = false;
+
+bool CMovAvgFilter::s_enabled_pdoa2d = false;
+
+//
+// Arguments    : void
+// Return Type  : void
+//
+void CMovAvgFilter::firstRun_not_empty_init()
+{
+  this->pd_.firstRun_not_empty = false;
+}
+
+//
+// Arguments    : void
+// Return Type  : MovAvgFilterStackData *
+//
+MovAvgFilterStackData *CMovAvgFilter::getStackData()
+{
+  return &this->SD_;
+}
+
+//
+// File trailer for CMovAvgFilter.cpp
+//
+// [EOF]
+//

MovAvgFilter/CMovAvgFilter.h

@@ -0,0 +1,43 @@
+//
+// File: CMovAvgFilter.h
+//
+// MATLAB Coder version            : 4.3
+// C/C++ source code generated on  : 12-Jan-2022 14:14:11
+//
+#ifndef CMOVAVGFILTER_H
+#define CMOVAVGFILTER_H
+
+// Include Files
+#include <cstddef>
+#include <cstdlib>
+#include "rtwtypes.h"
+#include "MovAvgFilter_types.h"
+
+// Type Definitions
+class CMovAvgFilter
+{
+ public:
+  CMovAvgFilter();
+  ~CMovAvgFilter();
+  static void SetPdoa1dEnable(bool bEnable);
+  static bool GetPdoa1dEnable();
+  static void SetPdoa2dEnable(bool bEnable);
+  static bool GetPdoa2dEnable();
+  real_T MovAvgFilter(real_T x);
+  MovAvgFilterStackData *getStackData();
+ private:
+  void firstRun_not_empty_init();
+  void MovAvgFilter_init();
+  MovAvgFilterPersistentData pd_;
+  MovAvgFilterStackData SD_;
+  static bool s_enabled_pdoa1d;
+  static bool s_enabled_pdoa2d;
+};
+
+#endif
+
+//
+// File trailer for CMovAvgFilter.h
+//
+// [EOF]
+//

MovAvgFilter/MovAvgFilter_types.h

@@ -0,0 +1,38 @@
+//
+// File: MovAvgFilter_types.h
+//
+// MATLAB Coder version            : 4.3
+// C/C++ source code generated on  : 12-Jan-2022 14:14:11
+//
+#ifndef MOVAVGFILTER_TYPES_H
+#define MOVAVGFILTER_TYPES_H
+
+// Include Files
+#include "rtwtypes.h"
+
+// Type Definitions
+struct emxArray_real_T_11
+{
+  real_T data[11];
+  int32_T size[1];
+};
+
+struct MovAvgFilterPersistentData
+{
+  real_T prevAvg;
+  emxArray_real_T_11 xbuf;
+  bool firstRun_not_empty;
+};
+
+struct MovAvgFilterStackData
+{
+  MovAvgFilterPersistentData *pd;
+};
+
+#endif
+
+//
+// File trailer for MovAvgFilter_types.h
+//
+// [EOF]
+//

MovAvgFilter/rtwtypes.h

@@ -0,0 +1,43 @@
+/*
+ * File: rtwtypes.h
+ *
+ * MATLAB Coder version            : 4.3
+ * C/C++ source code generated on  : 12-Jan-2022 14:14:11
+ */
+
+#ifndef RTWTYPES_H
+#define RTWTYPES_H
+
+/*=======================================================================*
+ * Fixed width word size data types:                                     *
+ *   int64_T                      - signed 64 bit integers               *
+ *   uint64_T                     - unsigned 64 bit integers             *
+ *=======================================================================*/
+#if defined(__APPLE__)
+# ifndef INT64_T
+#  define INT64_T                      long
+#  define FMT64                        "l"
+#  if defined(__LP64__) && !defined(INT_TYPE_64_IS_LONG)
+#    define INT_TYPE_64_IS_LONG
+#  endif
+# endif
+#endif
+
+#if defined(__APPLE__)
+# ifndef UINT64_T
+#  define UINT64_T                     unsigned long
+#  define FMT64                        "l"
+#  if defined(__LP64__) && !defined(INT_TYPE_64_IS_LONG)
+#    define INT_TYPE_64_IS_LONG
+#  endif
+# endif
+#endif
+
+#include "tmwtypes.h"
+#endif
+
+/*
+ * File trailer for rtwtypes.h
+ *
+ * [EOF]
+ */

MovAvgFilter/tmwtypes.h

@@ -0,0 +1,848 @@
+/*
+ * Copyright 1984-2018 The MathWorks, Inc.
+ */
+
+#if defined(_MSC_VER)
+# pragma once
+#endif
+#if defined(__GNUC__) && (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ > 3))
+# pragma once
+#endif
+
+#ifndef tmwtypes_h
+#define tmwtypes_h
+
+#ifndef __TMWTYPES__
+#define __TMWTYPES__
+/*
+ * File    : tmwtypes.h
+ * Abstract:
+ *      Data types for use with MATLAB/SIMULINK and the Real-Time Workshop.
+ *
+ *      When compiling stand-alone model code, data types can be overridden
+ *      via compiler switches.
+ *
+ *      Define NO_FLOATS to eliminate reference to real_T, etc.
+ */
+
+#include <limits.h>
+
+/* __STDC_VERSION__ version check below means "check for a C99 compiler".
+
+   Visual Studio (checked on versions 2015 and 2017) does
+   not define __STDC_VERSION__, however  it has stdbool.h available,
+   thus a separate check for _MSC_VER below.
+ */
+#if defined(__APPLE_CC__) \
+    || (defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L)) \
+    || (defined(_MSC_VER) && (_MSC_VER >= 1900))
+#include <stdbool.h>
+#endif
+
+#define LOGICAL_IS_A_TYPE
+#define SPARSE_GENERALIZATION
+
+#ifdef NO_FLOATS
+# define double double_not_allowed
+# define float  float_not_allowed
+#endif /*NO_FLOATS*/
+
+#ifndef NO_FLOATS
+
+#ifndef __MWERKS__
+# ifdef __STDC__
+#  include <float.h>
+# else
+#  define FLT_MANT_DIG 24
+#  define DBL_MANT_DIG 53
+# endif
+#endif
+
+#endif /*NO_FLOATS*/
+
+/*
+ *      The following data types cannot be overridden when building MEX files.
+ */
+#ifdef MATLAB_MEX_FILE
+# undef CHARACTER_T
+# undef INTEGER_T
+# undef BOOLEAN_T
+# undef REAL_T
+# undef TIME_T
+#endif
+
+/*
+ * The uchar_T, ushort_T and ulong_T types are needed for compilers which do 
+ * not allow defines to be specified, at the command line, with spaces in them.
+ */
+
+typedef unsigned char       uchar_T;
+typedef unsigned short      ushort_T;
+typedef unsigned long       ulong_T;
+
+#if (defined(_MSC_VER) && _MSC_VER >= 1500)      \
+    || defined(__x86_64__) || defined(__LP64__)  \
+    || defined(__LCC64__)
+    
+typedef unsigned long long  ulonglong_T;
+#endif
+
+
+
+/*=======================================================================*
+ * Fixed width word size data types:                                     *
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     *
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   *
+ *   real32_T, real64_T           - 32 and 64 bit floating point numbers *
+ *=======================================================================*/
+
+/* When used with Real Time Workshop generated code, this
+ * header file can be used with a variety of compilers.
+ *
+ * The compiler could be for an 8 bit embedded processor that
+ * only had 8 bits per integer and 16 bits per long.
+ * In that example, a 32 bit integer size is not even available.
+ * This header file should be robust to that.   
+ *
+ * For the case of an 8 bit processor, the preprocessor
+ * may be limited to 16 bit math like its target.  That limitation 
+ * would mean that 32 bit comparisons can't be done accurately.  
+ * To increase robustness to this, comparisons are done against
+ * smaller values first.  An inaccurate 32 bit comparison isn't
+ * attempted if the 16 bit comparison has already succeeded.
+ *
+ * Limitations on preprocessor math can also be stricter than
+ * for the target.  There are known cases where a compiler
+ * targeting processors with 64 bit longs can't do accurate
+ * preprocessor comparisons on more than 32 bits.  
+ */
+
+/* Determine the number of bits for int, long, short, and char.
+ * If one fails to be determined, set the number of bits to -1
+ */
+
+#ifndef TMW_BITS_PER_INT
+# if   INT_MAX  == 0x7FL
+#  define TMW_BITS_PER_INT 8
+# elif INT_MAX  == 0x7FFFL
+#  define TMW_BITS_PER_INT 16
+# elif INT_MAX  == 0x7FFFFFFFL
+#  define TMW_BITS_PER_INT 32
+# else
+#  define TMW_BITS_PER_INT -1
+# endif
+#endif
+
+#ifndef TMW_BITS_PER_LONG
+# if   LONG_MAX  == 0x7FL
+#  define TMW_BITS_PER_LONG 8
+# elif LONG_MAX  == 0x7FFFL
+#  define TMW_BITS_PER_LONG 16
+# elif LONG_MAX  == 0x7FFFFFFFL
+#  define TMW_BITS_PER_LONG 32
+# else
+#  define TMW_BITS_PER_LONG -1
+# endif
+#endif
+
+#ifndef TMW_BITS_PER_SHRT
+# if   SHRT_MAX  == 0x7FL
+#  define TMW_BITS_PER_SHRT 8
+# elif SHRT_MAX  == 0x7FFFL
+#  define TMW_BITS_PER_SHRT 16
+# elif SHRT_MAX  == 0x7FFFFFFFL
+#  define TMW_BITS_PER_SHRT 32
+# else
+#  define TMW_BITS_PER_SHRT -1
+# endif
+#endif
+
+#ifndef TMW_BITS_PER_SCHAR
+# if   SCHAR_MAX  == 0x7FL
+#  define TMW_BITS_PER_SCHAR 8
+# elif SCHAR_MAX  == 0x7FFFL
+#  define TMW_BITS_PER_SCHAR 16
+# elif SCHAR_MAX  == 0x7FFFFFFFL
+#  define TMW_BITS_PER_SCHAR 32
+# else
+#  define TMW_BITS_PER_SCHAR -1
+# endif
+#endif
+
+#ifndef TMW_CHAR_SIGNED
+# if SCHAR_MAX == CHAR_MAX
+#  define TMW_CHAR_SIGNED 1
+# else
+#  define TMW_CHAR_SIGNED 0
+# endif
+#endif
+
+/* It is common for one or more of the integer types
+ * to be the same size.  For example, on many embedded
+ * processors, both shorts and ints are 16 bits.  On
+ * processors used for workstations, it is quite common
+ * for both int and long to be 32 bits.  
+ *   When there is more than one choice for typdef'ing
+ * a portable type like int16_T or uint32_T, in
+ * concept, it should not matter which choice is made.
+ * However, some style guides and some code checking
+ * tools do identify and complain about seemingly
+ * irrelevant differences.  For example, a code
+ * checking tool may complain about an implicit
+ * conversion from int to short even though both
+ * are 16 bits.  To reduce these types of
+ * complaints, it is best to make int the
+ * preferred choice when more than one is available.
+ */
+
+#ifndef INT8_T
+# if   TMW_BITS_PER_INT   == 8
+#  define  INT8_T int
+# elif TMW_BITS_PER_LONG  == 8
+#  define  INT8_T long
+# elif TMW_BITS_PER_SCHAR == 8
+#  define  INT8_T signed char
+# elif TMW_BITS_PER_SHRT  == 8
+#  define  INT8_T short
+# endif
+#endif
+#ifdef INT8_T
+ typedef INT8_T int8_T;
+#endif
+
+#ifndef UINT8_T
+# if   TMW_BITS_PER_INT   == 8
+#  define  UINT8_T unsigned int
+# elif TMW_BITS_PER_LONG  == 8
+#  define  UINT8_T unsigned long
+# elif TMW_BITS_PER_SCHAR == 8
+#  define  UINT8_T unsigned char
+# elif TMW_BITS_PER_SHRT  == 8
+#  define  UINT8_T unsigned short
+# endif
+#endif
+#ifdef UINT8_T
+ typedef UINT8_T uint8_T;
+#endif
+
+
+#ifndef INT16_T
+# if   TMW_BITS_PER_INT   == 16
+#  define  INT16_T int
+# elif TMW_BITS_PER_LONG  == 16
+#  define  INT16_T long
+# elif TMW_BITS_PER_SCHAR == 16
+#  define  INT16_T signed char
+# elif TMW_BITS_PER_SHRT  == 16
+#  define  INT16_T short
+# endif
+#endif
+#ifdef INT16_T
+ typedef INT16_T int16_T;
+#endif
+
+
+#ifndef UINT16_T
+# if   TMW_BITS_PER_INT   == 16
+#  define  UINT16_T unsigned int
+# elif TMW_BITS_PER_LONG  == 16
+#  define  UINT16_T unsigned long
+# elif TMW_BITS_PER_SCHAR == 16
+#  define  UINT16_T unsigned char
+# elif TMW_BITS_PER_SHRT  == 16
+#  define  UINT16_T unsigned short
+# endif
+#endif
+#ifdef UINT16_T
+ typedef UINT16_T uint16_T;
+#endif
+
+
+#ifndef INT32_T
+# if   TMW_BITS_PER_INT   == 32
+#  define  INT32_T int
+# elif TMW_BITS_PER_LONG  == 32
+#  define  INT32_T long
+# elif TMW_BITS_PER_SCHAR == 32
+#  define  INT32_T signed char
+# elif TMW_BITS_PER_SHRT  == 32
+#  define  INT32_T short
+# endif
+#endif
+#ifdef INT32_T
+ typedef INT32_T int32_T;
+#endif
+
+
+#ifndef UINT32_T
+# if   TMW_BITS_PER_INT   == 32
+#  define  UINT32_T unsigned int
+# elif TMW_BITS_PER_LONG  == 32
+#  define  UINT32_T unsigned long
+# elif TMW_BITS_PER_SCHAR == 32
+#   define UINT32_T unsigned char
+# elif TMW_BITS_PER_SHRT  == 32
+#  define  UINT32_T unsigned short
+# endif
+#endif
+#ifdef UINT32_T
+ typedef UINT32_T uint32_T;
+#endif
+
+/* The following is used to emulate smaller integer types when only
+ * larger types are available.  For example, compilers for TI C3x/C4x DSPs 
+ * define char and short to be 32 bits, so 8 and 16 bits are not directly
+ * available.  This target is commonly used with RTW rapid prototyping.
+ * Other DSPs define char to be 16 bits, so 8 bits is not directly 
+ * available.
+ */
+#ifndef INT8_T
+# ifdef INT16_T
+#  define INT8_T INT16_T
+   typedef INT8_T int8_T;
+# else
+#  ifdef INT32_T
+#   define INT8_T INT32_T
+    typedef INT8_T int8_T;
+#  endif
+# endif
+#endif
+
+#ifndef UINT8_T
+# ifdef UINT16_T
+#   define UINT8_T UINT16_T
+    typedef UINT8_T uint8_T;
+# else
+#  ifdef UINT32_T
+#   define UINT8_T UINT32_T
+    typedef UINT8_T uint8_T;
+#  endif
+# endif
+#endif
+
+#ifndef INT16_T
+# ifdef INT32_T
+#   define INT16_T INT32_T
+    typedef INT16_T int16_T;
+# endif
+#endif
+
+#ifndef UINT16_T
+# ifdef UINT32_T
+#   define UINT16_T UINT32_T
+    typedef UINT16_T uint16_T;
+# endif
+#endif
+
+
+#ifndef NO_FLOATS
+
+#ifndef REAL32_T
+# ifndef __MWERKS__
+#  if FLT_MANT_DIG >= 23
+#   define REAL32_T float
+#  endif
+# else
+#  define REAL32_T float
+# endif
+#endif
+#ifdef REAL32_T
+ typedef REAL32_T real32_T;
+#endif
+
+
+#ifndef REAL64_T
+# ifndef __MWERKS__
+#  if DBL_MANT_DIG >= 52
+#   define REAL64_T double
+#  endif
+# else
+#  define REAL64_T double
+# endif
+#endif
+#ifdef REAL64_T
+ typedef REAL64_T real64_T;
+#endif
+
+#endif /* NO_FLOATS*/
+
+/*=======================================================================*
+ * Fixed width word size data types:                                     *
+ *   int64_T                      - signed 64 bit integers               *
+ *   uint64_T                     - unsigned 64 bit integers             *
+ *=======================================================================*/
+
+
+
+#ifndef INT64_T
+# if defined(__APPLE__)
+#  define INT64_T long long
+#  define FMT64 "ll"
+#  if defined(__LP64__) && !defined(INT_TYPE_64_IS_LONG)
+#    define INT_TYPE_64_IS_LONG
+#  endif
+# elif (defined(__x86_64__) || defined(__LP64__))&& !defined(__MINGW64__)
+#  define INT64_T long
+#  define FMT64 "l"
+#  if !defined(INT_TYPE_64_IS_LONG)
+#    define INT_TYPE_64_IS_LONG
+#  endif
+# elif defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \
+                         || (defined(__WATCOMC__)  && __WATCOMC__  >= 1100)
+#  define INT64_T __int64
+#  define FMT64 "I64"
+# elif defined(__GNUC__) || defined(TMW_ENABLE_INT64) \
+                         || defined(__LCC64__)
+#  define INT64_T long long
+#  define FMT64 "ll"
+# endif
+#endif
+
+
+
+#if defined(INT64_T)
+# if defined(__GNUC__) && \
+    ((__GNUC__ > 2) || ((__GNUC__ == 2) && (__GNUC_MINOR__ >=9)))
+  __extension__
+# endif
+ typedef INT64_T int64_T;
+#endif
+
+
+
+#ifndef UINT64_T
+# if defined(__APPLE__)
+#  define UINT64_T unsigned long long
+#  define FMT64 "ll"
+#  if defined(__LP64__) && !defined(INT_TYPE_64_IS_LONG)
+#    define INT_TYPE_64_IS_LONG
+#  endif
+# elif (defined(__x86_64__) || defined(__LP64__))&& !defined(__MINGW64__)
+#  define UINT64_T unsigned long
+#  define FMT64 "l"
+#  if !defined(INT_TYPE_64_IS_LONG)
+#    define INT_TYPE_64_IS_LONG
+#  endif
+# elif defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \
+                         || (defined(__WATCOMC__)  && __WATCOMC__  >= 1100)
+#  define UINT64_T unsigned __int64
+#  define FMT64 "I64"
+# elif defined(__GNUC__) || defined(TMW_ENABLE_INT64) \
+                         || defined(__LCC64__)
+#  define UINT64_T unsigned long long
+#  define FMT64 "ll"
+# endif
+#endif
+
+#if defined(_WIN64) || (defined(__APPLE__) && defined(__LP64__)) \
+                    || defined(__x86_64__) \
+                    || defined(__LP64__)
+#  define INT_TYPE_64_IS_SUPPORTED
+#endif
+
+#if defined(UINT64_T)
+# if defined(__GNUC__) && \
+    ((__GNUC__ > 2) || ((__GNUC__ == 2) && (__GNUC_MINOR__ >=9)))
+  __extension__
+# endif
+ typedef UINT64_T uint64_T;
+#endif
+
+/*===========================================================================*
+ * Format string modifiers for using size_t variables in printf statements.  *
+ *===========================================================================*/
+
+#ifndef FMT_SIZE_T
+#  if (defined( __GNUC__ ) || defined(_STDC_C99))&& !defined(__MINGW64__)
+#    define FMT_SIZE_T "z"
+#  elif defined (__WATCOMC__)
+#    define FMT_SIZE_T "l"
+#  elif defined (_WIN32 )
+#    define FMT_SIZE_T "I"
+#  else
+#    define FMT_SIZE_T "l"
+#  endif
+#endif
+
+#ifndef FMT_PTRDIFF_T
+#  if defined(__APPLE__)
+#    define FMT_PTRDIFF_T "l"
+#  elif defined( __GNUC__ ) || defined(_STDC_C99)
+#    define FMT_PTRDIFF_T "t"
+#  elif defined (__WATCOMC__)
+#    define FMT_PTRDIFF_T "l"
+#  elif defined (_WIN32 )
+#    define FMT_PTRDIFF_T "I"
+#  else
+#    define FMT_PTRDIFF_T "l"
+#  endif
+#endif
+
+/*===========================================================================*
+ * General or logical data types where the word size is not guaranteed.      *
+ *  real_T     - possible settings include real32_T or real64_T              *
+ *  time_T     - possible settings include real32_T or real64_T              *
+ *  boolean_T                                                                *
+ *  char_T                                                                   *
+ *  int_T                                                                    *
+ *  uint_T                                                                   *
+ *  byte_T                                                                   *
+ *===========================================================================*/
+
+#ifndef NO_FLOATS
+
+#ifndef REAL_T
+# ifdef REAL64_T
+#  define REAL_T real64_T
+# else
+#  ifdef REAL32_T
+#   define REAL_T real32_T
+#  endif
+# endif
+#endif
+#ifdef REAL_T
+ typedef REAL_T real_T;
+#endif
+
+#ifndef TIME_T
+# ifdef REAL_T
+#  define TIME_T real_T
+# endif
+#endif
+#ifdef TIME_T
+ typedef TIME_T time_T;
+#endif
+
+#endif /* NO_FLOATS */
+
+#ifndef BOOLEAN_T
+# if defined(UINT8_T)
+#  define BOOLEAN_T UINT8_T
+# else
+#  define BOOLEAN_T unsigned int
+# endif
+#endif
+
+
+#ifndef CHARACTER_T
+# define CHARACTER_T char
+#endif
+typedef CHARACTER_T char_T;
+
+
+#ifndef INTEGER_T
+# define INTEGER_T int
+#endif
+typedef INTEGER_T int_T;
+
+
+#ifndef UINTEGER_T
+# define UINTEGER_T unsigned
+#endif
+typedef UINTEGER_T uint_T;
+
+
+#ifndef BYTE_T
+# define BYTE_T unsigned char
+#endif
+typedef BYTE_T byte_T;
+
+
+/*===========================================================================*
+ * Define Complex Structures                                                 *
+ *===========================================================================*/
+#ifndef NO_FLOATS
+
+#ifndef CREAL32_T
+#  ifdef REAL32_T
+    typedef struct {
+      real32_T re, im;
+    } creal32_T;
+#    define CREAL32_T creal32_T
+#  endif
+#endif
+
+#ifndef CREAL64_T
+#  ifdef REAL64_T
+    typedef struct {
+      real64_T re, im;
+    } creal64_T;
+#    define CREAL64_T creal64_T
+#  endif
+#endif
+
+#ifndef CREAL_T
+#  ifdef REAL_T
+    typedef struct {
+      real_T re, im;
+    } creal_T;
+#    define CREAL_T creal_T
+#  endif
+#endif
+
+#endif /* NO_FLOATS */
+
+#ifndef CINT8_T
+#  ifdef INT8_T
+    typedef struct {
+      int8_T re, im;
+    } cint8_T;
+#    define CINT8_T cint8_T
+#  endif
+#endif
+
+#ifndef CUINT8_T
+#  ifdef UINT8_T
+    typedef struct {
+      uint8_T re, im;
+    } cuint8_T;
+#    define CUINT8_T cuint8_T
+#  endif
+#endif
+
+#ifndef CINT16_T
+#  ifdef INT16_T
+    typedef struct {
+      int16_T re, im;
+    } cint16_T;
+#    define CINT16_T cint16_T
+#  endif
+#endif
+
+#ifndef CUINT16_T
+#  ifdef UINT16_T
+    typedef struct {
+      uint16_T re, im;
+    } cuint16_T;
+#    define CUINT16_T cuint16_T
+#  endif
+#endif
+
+#ifndef CINT32_T
+#  ifdef INT32_T
+    typedef struct {
+      int32_T re, im;
+    } cint32_T;
+#    define CINT32_T cint32_T
+#  endif
+#endif
+
+#ifndef CUINT32_T
+#  ifdef UINT32_T
+    typedef struct {
+      uint32_T re, im;
+    } cuint32_T;
+#    define CUINT32_T cuint32_T
+#  endif
+#endif
+
+#ifndef CINT64_T
+#  ifdef INT64_T
+    typedef struct {
+      int64_T re, im;
+    } cint64_T;
+#    define CINT64_T cint64_T
+#  endif
+#endif
+
+#ifndef CUINT64_T
+#  ifdef UINT64_T
+    typedef struct {
+      uint64_T re, im;
+    } cuint64_T;
+#    define CUINT64_T cuint64_T
+#  endif
+#endif
+
+/*=======================================================================*
+ * Min and Max:                                                          *
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     *
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   *
+ *=======================================================================*/
+
+#define  MAX_int8_T      ((int8_T)(127))            /* 127  */
+#define  MIN_int8_T      ((int8_T)(-128))           /* -128 */
+#define  MAX_uint8_T     ((uint8_T)(255))           /* 255  */
+#define  MIN_uint8_T     ((uint8_T)(0))
+                           
+#define  MAX_int16_T     ((int16_T)(32767))         /* 32767 */
+#define  MIN_int16_T     ((int16_T)(-32768))        /* -32768 */
+#define  MAX_uint16_T    ((uint16_T)(65535))        /* 65535 */
+#define  MIN_uint16_T    ((uint16_T)(0))
+                           
+#define  MAX_int32_T     ((int32_T)(2147483647))    /* 2147483647  */
+#define  MIN_int32_T     ((int32_T)(-2147483647-1)) /* -2147483648 */
+#define  MAX_uint32_T    ((uint32_T)(0xFFFFFFFFU))  /* 4294967295  */
+#define  MIN_uint32_T    ((uint32_T)(0))
+
+#if defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \
+                      || (defined(__WATCOMC__)  && __WATCOMC__  >= 1100) \
+                      || defined(__LCC64__)
+#  ifdef INT64_T
+#    define  MAX_int64_T     ((int64_T)(9223372036854775807LL))
+#    define  MIN_int64_T     ((int64_T)(-9223372036854775807LL-1LL))
+#  endif
+#  ifdef UINT64_T
+#    define  MAX_uint64_T    ((uint64_T)(0xFFFFFFFFFFFFFFFFULL))
+#    define  MIN_uint64_T    ((uint64_T)(0))
+#  endif
+#else
+#  ifdef INT64_T
+#    ifdef INT_TYPE_64_IS_LONG
+#      define  MAX_int64_T     ((int64_T)(9223372036854775807L))
+#      define  MIN_int64_T     ((int64_T)(-9223372036854775807L-1L))
+#    else
+#      define  MAX_int64_T     ((int64_T)(9223372036854775807LL))
+#      define  MIN_int64_T     ((int64_T)(-9223372036854775807LL-1LL))
+#    endif
+#  endif
+#  ifdef UINT64_T
+#    ifdef INT_TYPE_64_IS_LONG
+#      define  MAX_uint64_T    ((uint64_T)(0xFFFFFFFFFFFFFFFFUL))
+#      define  MIN_uint64_T    ((uint64_T)(0))
+#    else
+#      define  MAX_uint64_T    ((uint64_T)(0xFFFFFFFFFFFFFFFFULL))
+#      define  MIN_uint64_T    ((uint64_T)(0))
+#    endif
+#  endif
+#endif
+
+#ifdef _MSC_VER
+/* Conversion from unsigned __int64 to double is not implemented in windows
+ * and results in a compile error, thus the value must first be cast to
+ * signed __int64, and then to double.
+ *
+ * If the 64 bit int value is greater than 2^63-1, which is the signed int64 max,
+ * the macro below provides a workaround for casting a uint64 value to a double
+ * in windows.
+ */
+#  define uint64_to_double(u) ( ((u) > _I64_MAX) ? \
+            (double)(__int64)((u) - _I64_MAX - 1) + (double)_I64_MAX + 1: \
+            (double)(__int64)(u) )
+
+/* The following inline function should only be used in the macro double_to_uint64,
+ * as it only handles the specfic range of double between 2^63 and 2^64-1 */
+__forceinline
+uint64_T double_to_uint64_helper(double d) {
+  union double_to_uint64_union_type {
+    double dd;
+    uint64_T i64;
+  } di;
+  di.dd = d;
+  return (((di.i64 & 0x000fffffffffffff) | 0x0010000000000000) << 11);
+}
+
+/* The largest double value that can be cast to uint64 in windows is the
+ * signed int64 max, which is 2^63-1. The macro below provides
+ * a workaround for casting large double values to uint64 in windows.
+ */
+/* The magic number 18446744073709551616.0 is 2^64 */
+/* The magic number 9223372036854775808.0 is 2^63 */
+#  define double_to_uint64(d) ( ((d) >= 18446744073709551616.0) ? \
+            0xffffffffffffffffULL : \
+            ((d) >= 0.0) ? \
+            ((d) >= 9223372036854775808.0) ? \
+            double_to_uint64_helper(d) : \
+            (unsigned __int64)(d) : \
+            0ULL )
+#else
+#  define uint64_to_double(u) ((double)(u))
+#  if defined(__BORLANDC__) || defined(__WATCOMC__) || defined(__TICCSC__)
+/* double_to_uint64 defined only for MSVC and UNIX */
+#  else
+#  define double_to_uint64(d) ( ((d) >= 18446744073709551616.0) ? \
+            (unsigned long long) 0xffffffffffffffffULL : \
+            ((d) >= 0) ? (unsigned long long)(d) : (unsigned long long) 0 )
+#  endif
+#endif
+
+#if !defined(__cplusplus) && !defined(__bool_true_false_are_defined)
+
+#ifndef _bool_T
+#define _bool_T
+
+typedef bool bool;
+
+#ifndef false
+#define false (0)
+#endif
+#ifndef true 
+#define true (1)
+#endif
+
+#endif /* _bool_T */
+
+#endif /* !__cplusplus */
+
+/* 
+ * This software assumes that the code is being compiled on a target using a 
+ * 2's complement representation for signed integer values.
+ */
+#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
+#error "This code must be compiled using a 2's complement representation for signed integer values"
+#endif
+
+/*
+ * Maximum length of a MATLAB identifier (function/variable/model)
+ * including the null-termination character.
+ */
+#define TMW_NAME_LENGTH_MAX 64
+
+/*
+ * Maximum values for indices and dimensions
+ */
+#include <stddef.h>
+
+#ifdef MX_COMPAT_32
+typedef int mwSize;
+typedef int mwIndex;
+typedef int mwSignedIndex;
+#else
+typedef size_t    mwSize;         /* unsigned pointer-width integer */
+typedef size_t    mwIndex;        /* unsigned pointer-width integer */
+typedef ptrdiff_t mwSignedIndex;  /* a signed pointer-width integer */
+#endif
+
+                                  /* for the individual dim */
+#ifndef SLSIZE_SLINDEX
+#define SLSIZE_SLINDEX
+typedef int SLIndex;
+typedef int SLSize;
+#endif
+
+/* for the total size */
+#define SLIndexType size_t
+#define INVALID_SIZET_VALUE   (std::numeric_limits<SLIndexType>::max())
+#define MAX_VALID_SIZET_VALUE   (std::numeric_limits<SLIndexType>::max() -1)
+
+
+#if (defined(_LP64) || defined(_WIN64)) && !defined(MX_COMPAT_32)
+/* Currently 2^48 based on hardware limitations */
+# define MWSIZE_MAX    281474976710655UL
+# define MWINDEX_MAX   281474976710655UL
+# define MWSINDEX_MAX  281474976710655L
+# define MWSINDEX_MIN -281474976710655L
+#else
+# define MWSIZE_MAX    2147483647UL
+# define MWINDEX_MAX   2147483647UL
+# define MWSINDEX_MAX  2147483647L
+# define MWSINDEX_MIN -2147483647L
+#endif
+#define MWSIZE_MIN    0UL
+#define MWINDEX_MIN   0UL
+
+/** UTF-16 character type */
+
+#if (defined(__cplusplus) && (__cplusplus >= 201103L)) || (defined(_HAS_CHAR16_T_LANGUAGE_SUPPORT) && _HAS_CHAR16_T_LANGUAGE_SUPPORT)
+typedef char16_t CHAR16_T;
+#define U16_STRING_LITERAL_PREFIX u
+#elif defined(_MSC_VER)
+typedef wchar_t CHAR16_T;
+#define U16_STRING_LITERAL_PREFIX L
+#else
+typedef UINT16_T CHAR16_T;
+#endif
+
+#endif  /* __TMWTYPES__ */
+
+#endif /* tmwtypes_h */

select_tool.cpp

@@ -4,6 +4,8 @@
 #include "log.h"
 #include "card.h"
 #include "loc_common.h"
+#include "GaussianFilter/GaussianFilter_emxAPI.h"
+#include <deque>
 
 //opt_tool_main select_point_object::m_opt_tool;
 
@@ -522,31 +524,31 @@ bool select_point_object::select_pdoa_solution2(const std::vector<point> vp, con
 bool select_point_object::select_pdoa_1d_solution_original(const std::vector<point> vp, const std::vector<loc_message>& lm, loc_point& lp)
 {
 	uint64_t id = lm[0].m_card_type;
-	id = ((id<<32)|lm[0].m_card_id);
+	id = ((id << 32) | lm[0].m_card_id);
 
-	auto card= card_list::instance()->get(id);
-    double dist = lm[0].m_num_ticks * 15.65 * 2.996 * 1e-4;
+	auto card = card_list::instance()->get(id);
+	double dist = lm[0].m_num_ticks * 15.65 * 2.996 * 1e-4;
 
 	double pdoa = get_pdoa(lm[0].m_poa, lm[0].m_sit->m_pdoa_offset);
 	logn_info(3, "[pdoa_o] card_id=%d, pdoa=%.2f, dist=%.2f, vp0x=%.2f, vp0y=%.2f, vp1x=%.2f, vp2y=%.2f", lm[0].m_card_id, pdoa, dist, vp[0].x, vp[0].y, vp[1].x, vp[1].y);
 
-	double dt = (lm[0].m_loc_time - card->m_last_recv_time)*1.0/1000.0;
-	double speed = fabs(dist - card->m_last_dist)/dt;
+	double dt = (lm[0].m_loc_time - card->m_last_recv_time)*1.0 / 1000.0;
+	double speed = fabs(dist - card->m_last_dist) / dt;
 
-	if(!select_pdoa_solution2(vp, lm, lp, pdoa)){
+	if (!select_pdoa_solution2(vp, lm, lp, pdoa)) {
 		return false;
 	}
 
 	logn_info(3, "[pdoa_o] card_id=%d, ct=%d, useless=%d, x=%.4f, y=%.4f, speed=%.3f, d=%.2f", lm[0].m_card_id, lm[0].m_card_ct, 1, lp.x, lp.y, speed);
 
-	lp.m_useless			= true;
-	card->m_last_pdoa_diff	= pdoa;
-	card->m_last_ct			= lm[0].m_card_ct;
-	card->m_last_dist		= dist;
-	card->m_last_site_id	= lm[0].m_sit->m_id;
-	card->m_last_site_dir	= lm[0].m_sit->m_pdoa_direction;
-	card->m_last_point.x	= lp.x;
-	card->m_last_point.y	= lp.y;
+	lp.m_useless = true;
+	card->m_last_pdoa_diff = pdoa;
+	card->m_last_ct = lm[0].m_card_ct;
+	card->m_last_dist = dist;
+	card->m_last_site_id = lm[0].m_sit->m_id;
+	card->m_last_site_dir = lm[0].m_sit->m_pdoa_direction;
+	card->m_last_point.x = lp.x;
+	card->m_last_point.y = lp.y;
 	lp.m_speed = card->m_speed = speed;
 	card->m_cb_point.push_back(lp);
 	card->m_vtp_dist.push_back(dist);
@@ -554,7 +556,7 @@ bool select_point_object::select_pdoa_1d_solution_original(const std::vector<poi
 	return true;
 }
 /*
- * 选择一维定位的解
+ * 选择一维定位的解,不采用任何优化方法
  *
  * 参数
  *		const std::vector<point> vp			可能定位解
@@ -565,6 +567,16 @@ bool select_point_object::select_pdoa_1d_solution_original(const std::vector<poi
  *		true表示成功选解，false表示选解失败
  *
  * */
+/*pdoa一维定位算法
+card_location_base::on_location 调用进来的
+作用：计算pdoa值和定位结果lp
+输入：
+vp：定位数据
+lm：卡信息相关数据消息输入
+输出：
+lp，含有x、y坐标
+added by zengminguo
+*/
 bool select_point_object::select_pdoa_1d_solution(const std::vector<point> vp, const std::vector<loc_message>& lm, loc_point& lp)
 {
     uint64_t id = lm[0].m_card_type;
@@ -579,154 +591,221 @@ bool select_point_object::select_pdoa_1d_solution(const std::vector<point> vp, c
     idx = type*3 + dim - 1;
 
 	double pdoa = get_pdoa(lm[0].m_poa, lm[0].m_sit->m_pdoa_offset);
+	card->m_pdoa_diff = pdoa;
+	static std::deque<double> deqInput;
+	if (deqInput.size() >= 20)
+	{
+		deqInput.pop_front();
+	}
+	deqInput.push_back(card->m_pdoa_diff);
 
-	logn_info(3, "[pdoa] card_id=%d, pdoa=%.2f, dist=%.2f, vp0x=%.2f, vp0y=%.2f, vp1x=%.2f, vp1y=%.2f", lm[0].m_card_id, pdoa, dist, vp[0].x, vp[0].y, vp[1].x, vp[1].y);
-
-   	double dt = (lm[0].m_loc_time - card->m_last_recv_time)*1.0/1000.0;
-    double speed = fabs(dist - card->m_last_dist)/dt; 
+	double pdoaInput[20];
+	double pdoaOutput[20];
 
-	if(card->m_last_recv_time > 0 && fabs(dt) > 30.0)
+	int iIndex = 0;
+	for (std::deque<double>::iterator iter = deqInput.begin(); iter != deqInput.end(); iter++)
 	{
-		// 直接定位
-		
-		logn_info(3, "[pdoa] card_id=%d, pdoa=%.2f, dt=%.2f, speed=%.2f, last_time=%llu", lm[0].m_card_id, pdoa, dt, speed, card->m_last_recv_time);
-		
-		card->m_last_recv_time = lm[0].m_loc_time;
-
-		return false;
+		pdoaInput[iIndex] = *iter;
+		iIndex++;
 	}
 
-	card->m_last_recv_time = lm[0].m_loc_time;
-
-	int n = card->m_vtp_dist.size();
-	bool change_site = false;
-	bool over_site = false;
-	//切换基站,默认取正值
-	if(lm[0].m_sit->m_id != card->m_last_site_id){
-		// 切换基站要考虑两种情况
-		// 1）入井方向的切换
-		if(card->m_cb_stream_idx[0] < lm[0].m_sit->m_down_stream_idx){
-			if(lm[0].m_sit->m_pdoa_direction == 0){
-				pdoa *= (pdoa > 0?1:-1);
-			}else{
-				pdoa *= (pdoa < 0?-1:1);
+	//通过滤波计算出pdoa的正负值，使得左右判断准确，然后通过左右计算出最终定位坐标结果
+	//现在默认采用高斯滤波（离基站近的时候）加移动均值滤波（非离基站近距离）的方式，使得定位效果最好。
+	// added by zengminguo 20220424
+	auto pdoaGaussian = card->m_pdoa_diff;
+	emxArray_real_T* vecInput = emxCreateWrapper_real_T(pdoaInput, 1, deqInput.size());
+	emxArray_real_T* vecOutput = emxCreateWrapper_real_T(pdoaOutput, 1, deqInput.size());
+	card->m_gaussian_filter.GaussianFilter(vecInput, vecOutput);
+	pdoaGaussian = pdoaOutput[deqInput.size() - 1];
+	auto pdoaMovAvg = card->m_pdoa_diff;
+	pdoaMovAvg = card->m_mov_avg_filter.MovAvgFilter(pdoaMovAvg);
+
+	if (pdoaGaussian*card->m_last_pdoa_diff < 0)//反向
+	{
+		if (dist < CGaussianFilter::GetPdoa1dDist())//在过站区域
+		{
+			card->m_pdoa_diff = pdoaGaussian;
+		}
+		else
+		{
+			if (fabs(pdoaMovAvg) >= 0.5)
+			{
+				card->m_pdoa_diff = pdoaMovAvg;
 			}
-		}else{
-			// 2）出井方向的切换
-			if(lm[0].m_sit->m_pdoa_direction == 0){
-				pdoa *= (pdoa < 0?1:-1);
-			}else{
-				pdoa *= (pdoa > 0?-1:1);
+			else
+			{
+				card->m_pdoa_diff = card->m_last_pdoa_diff;
 			}
 		}
-			
-		if(!select_pdoa_solution2(vp, lm, lp, pdoa)){
-			return false;
-		}
+	}
+	else
+	{
+		card->m_pdoa_diff = pdoaGaussian;
+	}
+
+	pdoa = card->m_pdoa_diff;
+	card->m_last_pdoa_diff = card->m_pdoa_diff;
 	
-		card->m_cb_stream_idx.push_back(lm[0].m_sit->m_down_stream_idx);
-		card->m_last_site_id = lm[0].m_sit->m_id;	
-		card->m_vtp_dist.clear();
-		card->m_vtp_dist.resize(0);
-		change_site = true;
-		card->m_over_site = false;
-
-		logn_info(3, "[pdoa] change site, card_id=%d, last_site=%d, last_dist=%.2f, cur_site=%d, cur_dist=%.2f", lm[0].m_card_id, card->m_last_site_id, card->m_last_dist, lm[0].m_sit->m_id, dist);
-	}else if(!card->m_over_site && n >= 3){
-		// 过分站逻辑
-		if(!select_pdoa_solution2(vp, lm, lp, pdoa)){
+
+	logn_info(3, "[pdoa] card_id=%d, pdoa=%.2f, dist=%.2f, vp0x=%.2f, vp0y=%.2f, vp1x=%.2f, vp1y=%.2f", lm[0].m_card_id, pdoa, dist, vp[0].x, vp[0].y, vp[1].x, vp[1].y);
+
+	double dt = (lm[0].m_loc_time - card->m_last_recv_time)*1.0 / 1000.0;
+	double speed = fabs(dist - card->m_last_dist) / dt;
+
+	if (false)
+	{
+		if (card->m_last_recv_time > 0 && fabs(dt) > 30.0)
+		{
+			// 直接定位
+
+			logn_info(3, "[pdoa] card_id=%d, pdoa=%.2f, dt=%.2f, speed=%.2f, last_time=%llu", lm[0].m_card_id, pdoa, dt, speed, card->m_last_recv_time);
+
+			card->m_last_recv_time = lm[0].m_loc_time;
+
 			return false;
 		}
-		
-		//double dd = dist - card->m_last_dist;
-		double d = lp.dist(card->m_last_point)*lm[0].m_sit->m_scale;
-
-		logn_info(3, "[pdoa] origin info, card_id=%d, ox=%.2f, oy=%.2f, lx=%.2f, ly=%.2f, d=%.2f, dt=%.2f", lm[0].m_card_id, lp.x, lp.y, card->m_last_point.x, card->m_last_point.y, d, dt);
-		// 如果选出的解是正确解，即距离和测距距离相等
-		double k1 = (card->m_vtp_dist[n-2] - card->m_vtp_dist[n-1])/dt;
-		double k2 = (card->m_vtp_dist[n-1] - dist)/dt;
-		logn_info(3, "[pdoa] card_id=%d, k1=%.2f, k2=%.2f, d=%.2f", lm[0].m_card_id, k1, k2, d);
-
-		if(((k1 > 1 && k2 < -1) || (k1 < -1 && k2 > 1)) ||(k1*k2 < 0 && fabs(d - dist - card->m_last_dist) < 2.0)){
-			over_site = true;
-			card->m_over_site = true;		
-			//过基站还要分以下两种情况：
-			//1）入井时的从正到负
-			//2）出井时的从负到正
-			logn_info(3, "[pdoa] card_id=%d, cur_idx=%d, last_idx=%d", lm[0].m_card_id, card->m_cb_stream_idx[0], card->m_cb_stream_idx[1]);
-			if(card->m_cb_stream_idx[1] > card->m_cb_stream_idx[0]){
-				//1）入井时的从正到负
-				pdoa *= (pdoa>0?-1:1);
-				/*if(lm[0].m_sit->m_pdoa_direction == 0){
-				  pdoa *= (pdoa<0?-1:1);
-				  }else{
-				  pdoa *= (pdoa>0?1:-1);
-				  }*/
-			}else{
-				//2）出井时的从负到正
-				pdoa *= (pdoa<0?-1:1);
-				/*if(lm[0].m_sit->m_pdoa_direction == 0){
-				  pdoa *= (pdoa<0?-1:1);
-				  }else{
-				  pdoa *= (pdoa>0?-1:1);
-				  }*/
+
+		card->m_last_recv_time = lm[0].m_loc_time;
+
+		int n = card->m_vtp_dist.size();
+		bool change_site = false;
+		bool over_site = false;
+		//切换基站,默认取正值
+		if (lm[0].m_sit->m_id != card->m_last_site_id) {
+			// 切换基站要考虑两种情况
+			// 1）入井方向的切换
+			if (card->m_cb_stream_idx[0] < lm[0].m_sit->m_down_stream_idx) {
+				if (lm[0].m_sit->m_pdoa_direction == 0) {
+					pdoa *= (pdoa > 0 ? 1 : -1);
+				}
+				else {
+					pdoa *= (pdoa < 0 ? -1 : 1);
+				}
 			}
-			if(!select_pdoa_solution2(vp, lm, lp, pdoa)){
+			else {
+				// 2）出井方向的切换
+				if (lm[0].m_sit->m_pdoa_direction == 0) {
+					pdoa *= (pdoa < 0 ? 1 : -1);
+				}
+				else {
+					pdoa *= (pdoa > 0 ? -1 : 1);
+				}
+			}
+
+			if (!select_pdoa_solution2(vp, lm, lp, pdoa)) {
 				return false;
 			}
+
+			card->m_cb_stream_idx.push_back(lm[0].m_sit->m_down_stream_idx);
+			card->m_last_site_id = lm[0].m_sit->m_id;
+			card->m_vtp_dist.clear();
+			card->m_vtp_dist.resize(0);
+			change_site = true;
+			card->m_over_site = false;
+
+			logn_info(3, "[pdoa] change site, card_id=%d, last_site=%d, last_dist=%.2f, cur_site=%d, cur_dist=%.2f", lm[0].m_card_id, card->m_last_site_id, card->m_last_dist, lm[0].m_sit->m_id, dist);
 		}
-	}
+		else if (!card->m_over_site && n >= 3) {
+			// 过分站逻辑
+			if (!select_pdoa_solution2(vp, lm, lp, pdoa)) {
+				return false;
+			}
 
-	// 如果没有切换基站且没有过基站
-	if(!change_site && !over_site){
-		if(!select_pdoa_solution2(vp, lm, lp, pdoa)){
-			return false;
+			//double dd = dist - card->m_last_dist;
+			double d = lp.dist(card->m_last_point)*lm[0].m_sit->m_scale;
+
+			logn_info(3, "[pdoa] origin info, card_id=%d, ox=%.2f, oy=%.2f, lx=%.2f, ly=%.2f, d=%.2f, dt=%.2f", lm[0].m_card_id, lp.x, lp.y, card->m_last_point.x, card->m_last_point.y, d, dt);
+			// 如果选出的解是正确解，即距离和测距距离相等
+			double k1 = (card->m_vtp_dist[n - 2] - card->m_vtp_dist[n - 1]) / dt;
+			double k2 = (card->m_vtp_dist[n - 1] - dist) / dt;
+			logn_info(3, "[pdoa] card_id=%d, k1=%.2f, k2=%.2f, d=%.2f", lm[0].m_card_id, k1, k2, d);
+
+			if (((k1 > 1 && k2 < -1) || (k1 < -1 && k2 > 1)) || (k1*k2 < 0 && fabs(d - dist - card->m_last_dist) < 2.0)) {
+				over_site = true;
+				card->m_over_site = true;
+				//过基站还要分以下两种情况：
+				//1）入井时的从正到负
+				//2）出井时的从负到正
+				logn_info(3, "[pdoa] card_id=%d, cur_idx=%d, last_idx=%d", lm[0].m_card_id, card->m_cb_stream_idx[0], card->m_cb_stream_idx[1]);
+				if (card->m_cb_stream_idx[1] > card->m_cb_stream_idx[0]) {
+					//1）入井时的从正到负
+					pdoa *= (pdoa > 0 ? -1 : 1);
+					/*if(lm[0].m_sit->m_pdoa_direction == 0){
+					  pdoa *= (pdoa<0?-1:1);
+					  }else{
+					  pdoa *= (pdoa>0?1:-1);
+					  }*/
+				}
+				else {
+					//2）出井时的从负到正
+					pdoa *= (pdoa < 0 ? -1 : 1);
+					/*if(lm[0].m_sit->m_pdoa_direction == 0){
+					  pdoa *= (pdoa<0?-1:1);
+					  }else{
+					  pdoa *= (pdoa>0?-1:1);
+					  }*/
+				}
+				if (!select_pdoa_solution2(vp, lm, lp, pdoa)) {
+					return false;
+				}
+			}
 		}
-		
-		double d = lp.dist(card->m_last_point)*lm[0].m_sit->m_scale;
-
-		logn_info(3, "[pdoa] select solution, card_id=%d, x=%.2f, y =%.2f, last_x=%.2f, last_y=%.2f, d=%.2f, pd=%.2f, dt=%.2f", lm[0].m_card_id, lp.x, lp.y, card->m_last_point.x, card->m_last_point.y, dist, d, dt);
-
-		
-		if(fabs(d - fabs(dist - card->m_last_dist)) > 2.0){
-			if(!card->m_last_point.empty()&&(lm[0].m_sit->m_id == card->m_last_site_id)){
-				int idx = 0;
-				double min = 999999.0;
-				for(size_t i = 0;i < vp.size();++i){
-					double d = vp[i].dist(card->m_last_point)*lm[0].m_sit->m_scale;
-					if(fabs(d - speed*dt) < min){
-						idx = i;
-						min = fabs(d - speed*dt);
+
+		// 如果没有切换基站且没有过基站
+		if (!change_site && !over_site) {
+			if (!select_pdoa_solution2(vp, lm, lp, pdoa)) {
+				return false;
+			}
+
+			double d = lp.dist(card->m_last_point)*lm[0].m_sit->m_scale;
+
+			logn_info(3, "[pdoa] select solution, card_id=%d, x=%.2f, y =%.2f, last_x=%.2f, last_y=%.2f, d=%.2f, pd=%.2f, dt=%.2f", lm[0].m_card_id, lp.x, lp.y, card->m_last_point.x, card->m_last_point.y, dist, d, dt);
+
+
+			if (fabs(d - fabs(dist - card->m_last_dist)) > 2.0) {
+				if (!card->m_last_point.empty() && (lm[0].m_sit->m_id == card->m_last_site_id)) {
+					int idx = 0;
+					double min = 999999.0;
+					for (size_t i = 0; i < vp.size(); ++i) {
+						double d = vp[i].dist(card->m_last_point)*lm[0].m_sit->m_scale;
+						if (fabs(d - speed * dt) < min) {
+							idx = i;
+							min = fabs(d - speed * dt);
+						}
 					}
+					lp.set(vp[idx]);
 				}
-				lp.set(vp[idx]);
 			}
-		} 	
-		
-		d = lp.dist(card->m_last_point)*lm[0].m_sit->m_scale;
-		/*if(fabs(d-dist) > 0.5 && !card->m_last_point.empty()){
-			logn_info(3, "[pdoa] card_id=%d, d=%.2f, dist=%.2f, lp.x=%.2f, lp.y=%.2f , solution is not correct!", lm[0].m_card_id, d, dist, lp.x, lp.y);
-			return false;
-		}*/
 
-		logn_info(3, "[pdoa] unilateral processing, card_id=%d", lm[0].m_card_id);
-	}	
-   
-	lp.m_useless = true;
-	
-	logn_info(3, "[pdoa] card_id=%d, ct=%d, pdoa=%.2f, dd=%.2f, dt=%.2f", lm[0].m_card_id, lm[0].m_card_ct, pdoa, dt*speed, dt);
-    logn_info(3, "[pdoa] solution info: card_id=%d, ct=%d, useless=%d, locate_x=%.4f, locate_y=%.4f, speed=%.3f, d=%.2f", lm[0].m_card_id, lm[0].m_card_ct, lp.m_useless, lp.x, lp.y, speed, dt*speed);
-
-    card->m_last_pdoa_diff		= pdoa;
-    card->m_last_ct				= lm[0].m_card_ct;
-    card->m_last_dist			= dist;
-    card->m_last_site_id		= lm[0].m_sit->m_id;
-    card->m_last_site_dir		= lm[0].m_sit->m_pdoa_direction;
-    card->m_last_point.x		= lp.x;
-    card->m_last_point.y		= lp.y;
-    lp.m_speed = card->m_speed = speed;
-    card->m_cb_point.push_back(lp);
-	card->m_vtp_dist.push_back(dist);
+			d = lp.dist(card->m_last_point)*lm[0].m_sit->m_scale;
+			/*if(fabs(d-dist) > 0.5 && !card->m_last_point.empty()){
+				logn_info(3, "[pdoa] card_id=%d, d=%.2f, dist=%.2f, lp.x=%.2f, lp.y=%.2f , solution is not correct!", lm[0].m_card_id, d, dist, lp.x, lp.y);
+				return false;
+			}*/
+
+			logn_info(3, "[pdoa] unilateral processing, card_id=%d", lm[0].m_card_id);
+		}
+
+		lp.m_useless = true;
+
+		logn_info(3, "[pdoa] card_id=%d, ct=%d, pdoa=%.2f, dd=%.2f, dt=%.2f", lm[0].m_card_id, lm[0].m_card_ct, pdoa, dt*speed, dt);
+		logn_info(3, "[pdoa] solution info: card_id=%d, ct=%d, useless=%d, locate_x=%.4f, locate_y=%.4f, speed=%.3f, d=%.2f", lm[0].m_card_id, lm[0].m_card_ct, lp.m_useless, lp.x, lp.y, speed, dt*speed);
+
+	}
+   	
+	if (!(card->m_isVehicleMap&&card->is_person()))
+	{
+		card->m_last_pdoa_diff = pdoa;
+		card->m_last_ct = lm[0].m_card_ct;
+		card->m_last_dist = dist;
+		card->m_last_site_id = lm[0].m_sit->m_id;
+		card->m_last_site_dir = lm[0].m_sit->m_pdoa_direction;
+		card->m_last_point.x = lp.x;
+		card->m_last_point.y = lp.y;
+		lp.m_speed = card->m_speed = speed;
+		card->m_cb_point.push_back(lp);
+		card->m_vtp_dist.push_back(dist);
+	}
 
 	return true; 	    
 }