Linux GNU 11.4.0 Code Coverage Report

Directory: ./
Coverage: low: ≥ 0% medium: ≥ 75.0% high: ≥ 90.0%
Coverage Exec / Excl / Total
Lines: 0.0% 0 / 0 / 205
Functions: 0.0% 0 / 1 / 10
Branches: 0.0% 0 / 0 / 89
OMCompiler/SimulationRuntime/c/simulation/solver/gbode_ctrl.c
Line Branch Exec Source
1 /*
2 * This file belongs to the OpenModelica Run-Time System
3 *
4 * Copyright (c) 1998-2026, Open Source Modelica Consortium (OSMC), c/o Linköpings
5 * universitet, Department of Computer and Information Science, SE-58183 Linköping, Sweden. All rights
6 * reserved.
7 *
8 * THIS PROGRAM IS PROVIDED UNDER THE TERMS OF THE BSD NEW LICENSE OR THE
9 * AGPL VERSION 3 LICENSE OR THE OSMC PUBLIC LICENSE (OSMC-PL) VERSION 1.8. ANY
10 * USE, REPRODUCTION OR DISTRIBUTION OF THIS PROGRAM CONSTITUTES RECIPIENT'S
11 * ACCEPTANCE OF THE BSD NEW LICENSE OR THE OSMC PUBLIC LICENSE OR THE AGPL
12 * VERSION 3, ACCORDING TO RECIPIENTS CHOICE.
13 *
14 * The OpenModelica software and the OSMC (Open Source Modelica Consortium) Public License
15 * (OSMC-PL) are obtained from OSMC, either from the above address, from the URLs:
16 * http://www.openmodelica.org or https://github.com/OpenModelica/ or
17 * http://www.ida.liu.se/projects/OpenModelica, and in the OpenModelica distribution. GNU
18 * AGPL version 3 is obtained from: https://www.gnu.org/licenses/licenses.html#GPL. The BSD NEW
19 * License is obtained from: http://www.opensource.org/licenses/BSD-3-Clause.
20 *
21 * This program is distributed WITHOUT ANY WARRANTY; without even the implied warranty of
22 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, EXCEPT AS EXPRESSLY
23 * SET FORTH IN THE BY RECIPIENT SELECTED SUBSIDIARY LICENSE CONDITIONS OF
24 * OSMC-PL.
25 *
26 */
27
28 /*! \file gbode_ctrl.c
29 */
30
31 #include "../options.h"
32 #include "gbode_ctrl.h"
33 #include "gbode_conf.h"
34
35 modelica_boolean use_fhr = FALSE;
36 double use_filter = 1.0;
37
38 static inline void swap(int *a, int *b)
39 {
40 int tmp = *a;
41 ✗ *a = *b;
42 ✗ *b = tmp;
43 ✗ }
44
45 /**
46 * @brief Partitions an index array around a pivot value using Hoare's scheme in ascending order.
47 * @see https://en.wikipedia.org/wiki/Quicksort#Hoare_partition_scheme
48 *
49 * @param idx Index array being rearranged
50 * @param value Array of values
51 * @param left Left boundary of partition range (inclusive)
52 * @param right Right boundary of partition range (inclusive)
53 * @return Split point j, such that no element in [left, ... , j] is greater
54 * than any element in [j+1, ... , right]
55 */
56 ✗ static int partition(int *idx, const double *value, int left, int right)
57 {
58 ✗ double pivot = value[idx[(left + right) / 2]];
59
60 ✗ int i = left - 1;
61 ✗ int j = right + 1;
62
63 while (1)
64 {
65 ✗ do { i++; } while (value[idx[i]] < pivot);
66 ✗ do { j--; } while (value[idx[j]] > pivot);
67
68 ✗ if (i >= j) return j;
69
70 swap(&idx[i], &idx[j]);
71 }
72 }
73
74 /**
75 * @brief Finds the error threshold at the given percentage of fast states
76 * to all states using a quickselect algorithm.
77 *
78 * Returns the error value such that "percentage" of states have a higher error.
79 * Runs in O(n) best and average time without fully sorting the array.
80 *
81 * @param gbData GBODE data object
82 * @return Error threshold value, or -1.0 if percentage >= 1.0
83 */
84 ✗ double getErrorThreshold(DATA_GBODE *gbData)
85 {
86 ✗ if (gbData->percentage >= 1.0) return -1.0;
87
88 ✗ int length = gbData->nStates;
89 ✗ int last = length - 1;
90
91 // make percentage fit the ascending order of partition()
92 ✗ int target = last - (int)round(length * gbData->percentage);
93
94 if (target < 0) target = 0;
95 ✗ if (target >= length) target = last;
96
97 int left = 0;
98 int right = last;
99
100 ✗ while (left < right)
101 {
102 ✗ int split = partition(gbData->sortedStatesIdx, gbData->err, left, right);
103
104 ✗ if (target <= split)
105 {
106 right = split;
107 }
108 else
109 {
110 ✗ left = split + 1;
111 }
112 }
113
114 ✗ return gbData->err[gbData->sortedStatesIdx[target]];
115 }
116
117 /**
118 * @brief PI step size control (see Hairer, etc.)
119 *
120 * @param err_values
121 * @param step_values
122 * @param err_order
123 * @return double
124 */
125 ✗ double PIController(double* err_values, double* step_values, int err_order, enum GB_CTRL_METHOD ctrl_method)
126 {
127 ✗ int k = err_order + 1;
128 double beta1, beta2;
129 ✗ double err_n = err_values[0];
130 ✗ double err_n1 = err_values[1];
131
132 // Fallback for incomplete history
133 ✗ if (err_n1 < DBL_EPSILON) {
134 ✗ return pow(1. / err_n, 1. / k);
135 }
136
137 ✗ switch (ctrl_method) {
138 ✗ case GB_CTRL_PI_34:
139 ✗ beta1 = 0.7 / k; // current error (P)
140 ✗ beta2 = -0.4 / k; // previous error (I)
141 ✗ break;
142 ✗ case GB_CTRL_PI_33:
143 ✗ beta1 = (2.0 / 3.0) / k; // current error (P)
144 ✗ beta2 = (-1.0 / 3.0) / k; // previous error (I)
145 ✗ break;
146 ✗ case GB_CTRL_PI_42:
147 ✗ beta1 = 0.6 / k; // current error (P)
148 ✗ beta2 = -0.2 / k; // previous error (I)
149 ✗ break;
150 ✗ default:
151 ✗ throwStreamPrint(NULL, "Unknown step size control method.");
152 }
153
154 ✗ return pow(1.0 / err_n, beta1) * pow(1.0 / err_n1, beta2);
155 }
156
157 /**
158 * @brief PID step size control (see Hairer, etc.)
159 *
160 * @param err_values
161 * @param step_values
162 * @param err_order
163 * @return double
164 */
165 ✗ double PIDController(double* err_values, double* step_values, int err_order, enum GB_CTRL_METHOD ctrl_method)
166 {
167 ✗ int k = err_order + 1;
168 double beta1, beta2, beta3;
169
170 ✗ double err_n = err_values[0];
171 ✗ double err_n1 = err_values[1];
172 ✗ double err_n2 = err_values[2];
173
174 // Fallback for incomplete history
175 ✗ if (err_n1 < DBL_EPSILON || err_n2 < DBL_EPSILON) {
176 ✗ return pow(1.0 / err_n, 1.0 / k);
177 }
178
179 ✗ switch (ctrl_method) {
180 ✗ case GB_CTRL_PID_H312:
181 ✗ beta1 = 1./18/k; // current error (P)
182 ✗ beta2 = 1./9/k; // previous error (I)
183 beta3 = 1./18/k; // second previous error (D)
184 ✗ break;
185 ✗ case GB_CTRL_PID_SOEDERLIND:
186 ✗ beta1 = 0.1 / k; // current error (P)
187 ✗ beta2 = 0.2 / k; // previous error (I)
188 beta3 = 0.1 / k; // second previous error (D)
189 ✗ break;
190 ✗ case GB_CTRL_PID_STIFF:
191 ✗ beta1 = 0.58 / k; // current error (P)
192 ✗ beta2 = 0.21 / k; // previous error (I)
193 beta3 = 0.21 / k; // second previous error (D)
194 ✗ break;
195 ✗ default:
196 ✗ throwStreamPrint(NULL, "Unknown step size control method.");
197 }
198
199 ✗ return pow(1.0 / err_n, beta1) * pow(1.0 / err_n1, beta2) * pow(1.0 / err_n2, beta3);
200 }
201
202 /**
203 * @brief Preditive PI controller of the form hfac := (1/err_0)^(alpha_1/k) * (1/err_{-1})^(alpha_2/k) * (h/n_{-1})^ratio
204 * where ratio, alpha1 and alpha2 are DOF for the specific controller.
205 */
206 ✗ double PredictivePIController(double* err_values, double* step_values, int err_order, enum GB_CTRL_METHOD ctrl_method)
207 {
208 ✗ int k = err_order + 1;
209 double beta1, beta2, ratio;
210
211 ✗ double err_n = err_values[0];
212 ✗ double err_n1 = err_values[1];
213
214 ✗ double h = step_values[0];
215 ✗ double h_n1 = step_values[1];
216
217 // Fallback for incomplete history
218 ✗ if (err_n1 < DBL_EPSILON || h_n1 < DBL_EPSILON) {
219 ✗ return pow(1.0 / err_n, 1.0 / k);
220 }
221
222 ✗ switch (ctrl_method) {
223 ✗ case GB_CTRL_PI_PC_HYBRID:
224 case GB_CTRL_PI_PC:
225 ✗ beta1 = 2.0/k; // current error (P)
226 ✗ beta2 = -1.0/k; // previous error (I)
227 ratio = 1.0; // factor for ratio (h / h_n1)
228 ✗ break;
229 ✗ case GB_CTRL_PI_H211:
230 ✗ beta1 = 0.25/k; // current error (P)
231 beta2 = 0.25/k; // previous error (I)
232 ratio = -0.25; // factor for ratio (h / h_n1)
233 ✗ break;
234 ✗ case GB_CTRL_PI_H0_211:
235 ✗ beta1 = 0.5/k; // current error (P)
236 beta2 = 0.5/k; // previous error (I)
237 ratio = -0.5; // factor for ratio (h / h_n1)
238 ✗ break;
239 ✗ default:
240 ✗ throwStreamPrint(NULL, "Unknown step size control method.");
241 }
242
243 ✗ double pi_pc = pow(1.0 / err_n, beta1) * pow(1.0 / err_n1, beta2) * pow(h / h_n1, ratio);
244
245 ✗ if (ctrl_method == GB_CTRL_PI_PC_HYBRID)
246 {
247 ✗ double i = pow(1.0 / err_n, 1.0 / k);
248 ✗ return fmin(pi_pc, i);
249 }
250 else
251 {
252 return pi_pc;
253 }
254 }
255
256 /**
257 * @brief Preditive PID controller of the form
258 * hfac := (1/err_0)^(alpha_1/k) * (1/err_{-1})^(alpha_2/k) * (1/err_{-1})^(alpha_3/k) * (h/n_{-1})^ratio1 * (h_{-1}/n_{-2})^ratio2
259 * where ratio1, ratio2, alpha1, alpha2, alpha3 are DOF for the specific controller.
260 */
261 ✗ double PredictivePIDController(double* err_values, double* step_values, int err_order, enum GB_CTRL_METHOD ctrl_method)
262 {
263 ✗ int k = err_order + 1;
264 double beta1, beta2, beta3, ratio1, ratio2;
265
266 ✗ double err_n = err_values[0];
267 ✗ double err_n1 = err_values[1];
268 ✗ double err_n2 = err_values[2];
269
270 ✗ double h = step_values[0];
271 ✗ double h_n1 = step_values[1];
272 ✗ double h_n2 = step_values[2];
273
274 // Fallback for incomplete history
275 ✗ if (err_n1 < DBL_EPSILON || h_n1 < DBL_EPSILON || err_n2 < DBL_EPSILON || h_n2 < DBL_EPSILON ) {
276 ✗ return pow(1.0 / err_n, 1.0 / k);
277 }
278
279 ✗ switch (ctrl_method) {
280 ✗ case GB_CTRL_PID_H0_312:
281 ✗ beta1 = 0.25/k; // current error (P)
282 ✗ beta2 = 0.5/k; // previous error (I)
283 beta3 = 0.25/k; // second previous error (D)
284 ratio1 = -0.75;
285 ratio2 = -0.25;
286 ✗ break;
287 ✗ case GB_CTRL_PID_H0_321:
288 ✗ beta1 = 1.25/k; // current error (P)
289 ✗ beta2 = 0.5/k; // previous error (I)
290 ✗ beta3 = -0.75/k; // second previous error (D)
291 ratio1 = 0.25;
292 ratio2 = 0.75;
293 ✗ break;
294 ✗ case GB_CTRL_PPID:
295 ✗ beta1 = (6. / 20.)/k; // current error (P)
296 ✗ beta2 = (1. / 20.)/k; // previous error (I)
297 ✗ beta3 = (-5. / 20.)/k; // second previous error (D)
298 ratio1 = 1.0;
299 ratio2 = 0.0;
300 ✗ break;
301 ✗ default:
302 ✗ throwStreamPrint(NULL, "Unknown step size control method.");
303 }
304
305 ✗ return pow(1.0 / err_n, beta1) * pow(1.0 / err_n1, beta2) * pow(1.0 / err_n2, beta3) * pow(h / h_n1, ratio1) * pow(h_n1 / h_n2, ratio2) ;
306 }
307
308 /**
309 * @brief Compute adaptive gamma for FHR controller
310 *
311 * @param err_now Current error estimate
312 * @param err_prev Previous error estimate
313 * @param h_now Current step size
314 * @param h_prev Previous step size
315 * @param eta Scaling factor (e.g. 0.1)
316 * @return double Adaptive gamma value
317 */
318 ✗ double computeGamma(double err_now, double err_prev, double h_now, double h_prev, double eta)
319 {
320 ✗ double log_h_ratio = log(h_now / h_prev);
321 ✗ double log_e_ratio = log((err_now + DBL_EPSILON) / (err_prev + DBL_EPSILON)); // avoid division by zero
322
323 ✗ return eta * log_h_ratio / (log_e_ratio + DBL_EPSILON);
324 }
325
326 /**
327 * @brief PID step size control (see Hairer, etc.)
328 *
329 * @param err_values
330 * @param step_values
331 * @param err_order
332 * @return double
333 */
334 ✗ double GenericController(double* err_values, double* step_values, int err_order, enum GB_CTRL_METHOD ctrl_method)
335 {
336 const double fac = 0.9;
337 const double facmax = 2.5;
338 const double facmin = 0.2;
339
340 ✗ int k = err_order + 1;
341
342 ✗ double err_n = err_values[0];
343 ✗ double err_n1 = err_values[1];
344
345 ✗ double h_n = step_values[0];
346 ✗ double h_n1 = step_values[1];
347
348 double h_fac;
349
350 // Handle pathological zero error
351 ✗ if (err_n < DBL_EPSILON)
352 return facmax;
353
354 ✗ switch (ctrl_method) {
355 case GB_CTRL_CNST:
356 h_fac = 1.0; // Constant step size
357 break;
358 ✗ case GB_CTRL_I:
359 ✗ h_fac = pow(1./err_n, 1./k);
360 ✗ break;
361 ✗ case GB_CTRL_PI_33:
362 case GB_CTRL_PI_34:
363 case GB_CTRL_PI_42:
364 ✗ h_fac = PIController(err_values, step_values, err_order, ctrl_method);
365 ✗ break;
366 ✗ case GB_CTRL_PID_H312:
367 case GB_CTRL_PID_SOEDERLIND:
368 case GB_CTRL_PID_STIFF:
369 ✗ h_fac = PIDController(err_values, step_values, err_order, ctrl_method);
370 ✗ break;
371 ✗ case GB_CTRL_PI_PC:
372 case GB_CTRL_PI_PC_HYBRID:
373 case GB_CTRL_PI_H211:
374 case GB_CTRL_PI_H0_211:
375 ✗ h_fac = PredictivePIController(err_values, step_values, err_order, ctrl_method);
376 ✗ break;
377 ✗ case GB_CTRL_PID_H0_312:
378 case GB_CTRL_PID_H0_321:
379 case GB_CTRL_PPID:
380 ✗ h_fac = PredictivePIDController(err_values, step_values, err_order, ctrl_method);
381 ✗ break;
382 ✗ default:
383 ✗ throwStreamPrint(NULL, "Unknown step size control method.");
384 }
385
386 // Applies Fuehrer-style adaptive damping to the step size factor:
387 // If the last step was rejected, gamma > 0 increases conservatism by reducing h_fac.
388 // If accepted, gamma = 0 has little effect. The formula h_fac *= (h_n / h_n1)^gamma
389 // discourages oscillatory step behavior by penalizing instability in recent steps.
390 ✗ if (use_fhr && h_n1 > DBL_EPSILON) {
391 // Compute gamma adaptively (Needs to be looked up in the literature), not suitable for PID Controller?!
392 double eta = 0.1;
393 ✗ double gamma = computeGamma(err_n, err_n1, h_n, h_n1, eta);
394 ✗ h_fac = h_fac * pow(h_n / h_n1, gamma);
395 }
396
397 // Applies exponential smoothing to the step size factor:
398 // use_filter = 0 -> constant step size,
399 // use_filter = 1 -> full adaptation without smoothing.
400 ✗ if (use_filter>0) {
401 ✗ h_fac = use_filter * h_fac + (1.0 - use_filter);
402 }
403 ✗ h_fac *= fac;
404
405 // Keep step size constant, if there are only small changes
406 ✗ if ((0.99 < h_fac) && (h_fac < 1.2)) {
407 return 1.0;
408 } else
409 ✗ return fmin(facmax, fmax(facmin, h_fac));
410 }
411
412
413 /**
414 * @brief Calculate initial step size.
415 *
416 * Called at the beginning of simulation or after an event occurred.
417 *
418 * Book Reference:
419 * E. Hairer, S. P. Nørsett, G. Wanner
420 * Solving Ordinary Differential Equations I, Nonstiff Problems, page 169
421 *
422 * @param data Runtime data struct.
423 * @param threadData Thread data for error handling.
424 * @param gbData Storing Runge-Kutta solver data.
425 */
426 ✗ void getInitStepSize(DATA* data, threadData_t* threadData, DATA_GBODE* gbData, SOLVER_INFO* solverInfo)
427 {
428 ✗ SIMULATION_DATA *sData = (SIMULATION_DATA*)data->localData[0];
429 SIMULATION_DATA *sDataOld = (SIMULATION_DATA*)data->localData[1];
430 ✗ int nStates = data->modelData->nStates;
431 ✗ modelica_real* fODE = &sData->realVars[nStates];
432
433 int i;
434 double sc, safety = 0.01;
435 double d0 = 0.0; // norm of y0 weighted
436 double d1 = 0.0; // norm of f0 weighted
437 double d2 = 0.0; // norm of slope difference weighted
438
439 double h0, h1;
440 ✗ double absTol = data->simulationInfo->tolerance;
441 double relTol = absTol;
442 ✗ const double oldStep = gbData->stepSize;
443
444 // Increase initialFailures counter on repeated failures (for adaptive reduction)
445 ✗ gbData->initialFailures++;
446
447 // Store current time and state
448 ✗ gbData->time = sData->timeValue;
449 ✗ memcpy(gbData->yOld, sData->realVars, nStates * sizeof(double));
450
451 // Compute f(t0, y0)
452 ✗ gbode_fODE(data, threadData, &(gbData->stats.nCallsODE), NULL);
453
454 ✗ if (gbData->initialStepSize < 0) {
455 ✗ memcpy(gbData->f, fODE, nStates * sizeof(double));
456
457 // Compute weighted norms of y0 and f0
458 ✗ for (i = 0; i < nStates; i++) {
459 ✗ sc = absTol + fabs(gbData->yOld[i]) * relTol;
460 ✗ d0 += (gbData->yOld[i] * gbData->yOld[i]) / (sc * sc);
461 ✗ d1 += (fODE[i] * fODE[i]) / (sc * sc);
462 }
463 ✗ d0 = sqrt(d0 / nStates);
464 ✗ d1 = sqrt(d1 / nStates);
465
466 // Initial guess for h0 based on ratio
467 ✗ if (d0 < 1e-5 || d1 < 1e-5) {
468 h0 = 1e-6;
469 } else {
470 ✗ h0 = safety * d0 / d1;
471 }
472
473 // If repeated failures happened, reduce h0 accordingly
474 ✗ if (gbData->initialFailures > 0) {
475 ✗ h0 /= pow(10, gbData->initialFailures);
476 }
477 // Security condition, if h0 is nonsense
478 ✗ h0 = fmin(h0, 0.1*data->simulationInfo->stepSize);
479
480 // Trial explicit Euler step: y1 = y0 + h0 * f0
481 ✗ for (i = 0; i < nStates; i++) {
482 ✗ sData->realVars[i] = gbData->yOld[i] + fODE[i] * h0;
483 }
484 ✗ sData->timeValue = gbData->time + h0;
485
486 // Compute f(t0+h0, y1)
487 ✗ gbode_fODE(data, threadData, &(gbData->stats.nCallsODE), NULL);
488
489 // Compute weighted norm of slope difference
490 ✗ for (i = 0; i < nStates; i++) {
491 ✗ sc = absTol + fabs(gbData->yOld[i]) * relTol;
492 ✗ double diff = fODE[i] - gbData->f[i];
493 ✗ d2 += (diff * diff) / (sc * sc);
494 }
495 ✗ d2 = sqrt(d2 / nStates) / h0;
496
497 // Combine the slopes to refine step size estimate
498 ✗ double d = fmax(d1, d2);
499
500 ✗ if (d > 1e-15) {
501 ✗ h1 = sqrt(safety / d);
502 } else {
503 ✗ h1 = fmax(1e-6, h0 * 1e-3);
504 }
505
506 // Final step size: blend h0 and h1 with some limits
507 ✗ gbData->stepSize = fmin(100.0 * h0, h1);
508 ✗ gbData->optStepSize = gbData->stepSize;
509 ✗ gbData->lastStepSize = 0.0;
510
511 // Restore original state and time
512 ✗ sData->timeValue = gbData->time;
513 ✗ memcpy(sData->realVars, gbData->yOld, nStates * sizeof(double));
514 ✗ memcpy(fODE, gbData->f, nStates * sizeof(double));
515 } else {
516 ✗ gbData->stepSize = gbData->initialStepSize;
517 ✗ gbData->lastStepSize = 0.0;
518 }
519
520 ✗ if (solverInfo->didEventStep && !omc_flag[FLAG_SR_CTRL_EVNT_REINIT])
521 {
522 ✗ gbData->stepSize = fmax(oldStep * 1e-1, gbData->stepSize);
523 }
524
525 ✗ infoStreamPrint(OMC_LOG_SOLVER, 0, "Initial step size = %e at time %g", gbData->stepSize, gbData->time);
526
527 // Reset failure count on success
528 ✗ gbData->initialFailures = -1;
529 ✗ }
530