Improvements to the pi controller and first prototype of the pid cont…

…roller (#10407)
OpenModelica · Mar 15, 2023 · 4937455 · 4937455
1 parent fa0ef7f
commit 4937455
Show file tree

Hide file tree

Showing 3 changed files with 45 additions and 4 deletions.
diff --git a/OMCompiler/SimulationRuntime/c/simulation/solver/gbode_ctrl.c b/OMCompiler/SimulationRuntime/c/simulation/solver/gbode_ctrl.c
@@ -114,8 +114,8 @@ double PIController(double* err_values, double* stepSize_values, unsigned int er
   double facmax = 3.5;
   double facmin = 0.5;
   double beta  = 1./(err_order+1);
-  double beta1 = 1./(err_order+1);
-  double beta2 = 1./(err_order+1);
+  double beta1 = 0.7/(err_order+1);
+  double beta2 = -0.4/(err_order+1);
 
   double estimate;
 
@@ -125,7 +125,43 @@ double PIController(double* err_values, double* stepSize_values, unsigned int er
   if (err_values[1] < DBL_EPSILON)
     estimate = pow(1./err_values[0], beta);
   else
-    estimate = stepSize_values[0]/stepSize_values[1]*pow(1./err_values[0], beta1)*pow(err_values[1]/err_values[0], beta2);
+    estimate = pow(1./err_values[0], beta1)*pow(1./err_values[1], beta2);
+
+  return fmin(facmax, fmax(facmin, fac*estimate));
+}
+
+/**
+ * @brief PID step size control (see Hairer, etc.)
+ *
+ * @param err_values
+ * @param stepSize_values
+ * @param err_order
+ * @return double
+ */
+double PIDController(double* err_values, double* stepSize_values, unsigned int err_order)
+{
+  double fac = 0.9;
+  double facmax = 3.5;
+  double facmin = 0.5;
+  double beta  = 1./(err_order+1);
+  double beta1 = 0.7/(err_order+1);
+  double beta2 = -0.4/(err_order+1);
+  double alpha1 = 1./18/(err_order+1);
+  double alpha2 = 1./9/(err_order+1);
+  double alpha3 = 1./18/(err_order+1);
+
+  double estimate;
+
+  if (err_values[0] < DBL_EPSILON)
+    return facmax;
+
+  if (err_values[1] < DBL_EPSILON)
+    estimate = pow(1./err_values[0], beta);
+  else
+    if (err_values[2] < DBL_EPSILON)
+      estimate = pow(1./err_values[0], beta1)*pow(1./err_values[1], beta2);
+    else
+      estimate = pow(1./err_values[0], alpha1)*pow(1./err_values[1], alpha2)*pow(1./err_values[2], alpha3);
 
   return fmin(facmax, fmax(facmin, fac*estimate));
 }
@@ -143,6 +179,8 @@ gm_stepSize_control_function getControllFunc(enum GB_CTRL_METHOD ctrl_method) {
     return IController;
   case GB_CTRL_PI:
     return PIController;
+  case GB_CTRL_PID:
+    return PIDController;
   case GB_CTRL_CNST:
     return CController;
   default:

diff --git a/OMCompiler/SimulationRuntime/c/util/simulation_options.c b/OMCompiler/SimulationRuntime/c/util/simulation_options.c
@@ -1005,13 +1005,15 @@ const char *GB_CTRL_METHOD_NAME[GB_CTRL_MAX] = {
   /* GB_CTRL_UNKNOWN */   "unknown",
   /* GB_CTRL_I */         "i",
   /* GB_CTRL_PI */        "pi",
+  /* GB_CTRL_PID */       "pid",
   /* GB_CTRL_CNST */      "const"
 };
 
 const char *GB_CTRL_METHOD_DESC[GB_CTRL_MAX] = {
   /* GB_CTRL_UNKNOWN */   "unknown",
   /* GB_CTRL_I */         "I controller for step size",
   /* GB_CTRL_PI */        "PI controller for step size",
+  /* GB_CTRL_PID */       "PID controller for step size",
   /* GB_CTRL_CNST */      "Constant step size"
 };
 

diff --git a/OMCompiler/SimulationRuntime/c/util/simulation_options.h b/OMCompiler/SimulationRuntime/c/util/simulation_options.h
@@ -276,7 +276,8 @@ enum GB_CTRL_METHOD {
   GB_CTRL_UNKNOWN = 0,  /* Unknown controller */
   GB_CTRL_I = 1,        /* I controller */
   GB_CTRL_PI = 2,       /* PI controller */
-  GB_CTRL_CNST = 3,     /* Constant step size */
+  GB_CTRL_PID = 3,       /* PID controller */
+  GB_CTRL_CNST = 4,     /* Constant step size */
 
   GB_CTRL_MAX
 };