2 Copyright (C) 2008 Torben Hohn
4 This program is free software; you can redistribute it and/or modify
5 it under the terms of the GNU General Public License as published by
6 the Free Software Foundation; either version 2 of the License, or
7 (at your option) any later version.
9 This program is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU General Public License for more details.
14 You should have received a copy of the GNU General Public License
15 along with this program; if not, write to the Free Software
16 Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
23 #include "ardour/pi_controller.h"
25 static inline double hann(double x
) {
26 return 0.5 * (1.0 - cos(2 * M_PI
* x
));
29 PIController::PIController (double resample_factor
, int fir_size
)
31 resample_mean
= resample_factor
;
32 static_resample_factor
= resample_factor
;
33 offset_array
= new double[fir_size
];
34 window_array
= new double[fir_size
];
35 offset_differential_index
= 0;
36 offset_integral
= 0.0;
37 smooth_size
= fir_size
;
39 for (int i
= 0; i
< fir_size
; i
++) {
40 offset_array
[i
] = 0.0;
41 window_array
[i
] = hann(double(i
) / (double(fir_size
) - 1.0));
44 // These values could be configurable
48 controlquant
= 10000.0;
52 PIController::~PIController ()
54 delete [] offset_array
;
55 delete [] window_array
;
59 PIController::get_ratio (int fill_level
, int period_size
)
61 double offset
= fill_level
;
62 double this_catch_factor
= catch_factor
;
63 double this_catch_factor2
= catch_factor2
* 4096.0/(double)period_size
;
68 for (int i
= 0; i
< smooth_size
; i
++) {
69 offset_array
[i
] = offset
;
73 offset_array
[(offset_differential_index
++) % smooth_size
] = offset
;
76 // Build the mean of the windowed offset array basically fir lowpassing.
78 for (int i
= 0; i
< smooth_size
; i
++) {
79 smooth_offset
+= offset_array
[(i
+ offset_differential_index
- 1) % smooth_size
] * window_array
[i
];
81 smooth_offset
/= double(smooth_size
);
83 // This is the integral of the smoothed_offset
84 offset_integral
+= smooth_offset
;
86 std::cerr
<< smooth_offset
<< " ";
88 // Clamp offset : the smooth offset still contains unwanted noise which would go straigth onto the resample coeff.
89 // It only used in the P component and the I component is used for the fine tuning anyways.
91 if (fabs(smooth_offset
) < pclamp
)
94 smooth_offset
+= (static_resample_factor
- resample_mean
) * this_catch_factor
;
96 // Ok, now this is the PI controller.
97 // u(t) = K * (e(t) + 1/T \int e(t') dt')
98 // Kp = 1/catch_factor and T = catch_factor2 Ki = Kp/T
99 current_resample_factor
100 = static_resample_factor
- smooth_offset
/ this_catch_factor
- offset_integral
/ this_catch_factor
/ this_catch_factor2
;
102 // Now quantize this value around resample_mean, so that the noise which is in the integral component doesnt hurt.
103 current_resample_factor
= floor((current_resample_factor
- resample_mean
) * controlquant
+ 0.5) / controlquant
+ resample_mean
;
105 // Calculate resample_mean so we can init ourselves to saner values.
106 // resample_mean = 0.9999 * resample_mean + 0.0001 * current_resample_factor;
107 resample_mean
= (1.0-0.01) * resample_mean
+ 0.01 * current_resample_factor
;
108 std::cerr
<< fill_level
<< " " << smooth_offset
<< " " << offset_integral
<< " " << current_resample_factor
<< " " << resample_mean
<< "\n";
109 return current_resample_factor
;
113 PIController::out_of_bounds()
116 // Set the resample_rate... we need to adjust the offset integral, to do this.
117 // first look at the PI controller, this code is just a special case, which should never execute once
118 // everything is swung in.
119 offset_integral
= - (resample_mean
- static_resample_factor
) * catch_factor
* catch_factor2
;
120 // Also clear the array. we are beginning a new control cycle.
121 for (i
= 0; i
< smooth_size
; i
++) {
122 offset_array
[i
] = 0.0;
128 PIChaser::PIChaser() {
129 pic
= new PIController( 1.0, 16 );
131 for( int i
=0; i
<ESTIMATOR_SIZE
; i
++ ) {
132 realtime_stamps
[i
] = 0;
133 chasetime_stamps
[i
] = 0;
136 speed_threshold
= 0.2;
137 pos_threshold
= 4000;
144 for( int i
=0; i
<ESTIMATOR_SIZE
; i
++ ) {
145 realtime_stamps
[i
] = 0;
146 chasetime_stamps
[i
] = 0;
150 PIChaser::~PIChaser() {
155 PIChaser::get_ratio(framepos_t chasetime_measured
, framepos_t chasetime
, framepos_t slavetime_measured
, framepos_t slavetime
, bool in_control
, int period_size
) {
157 feed_estimator( chasetime_measured
, chasetime
);
158 std::cerr
<< (double)chasetime_measured
/48000.0 << " " << chasetime
<< " " << slavetime
<< " ";
159 double crude
= get_estimate();
161 framepos_t massaged_chasetime
= chasetime
+ (framepos_t
)( (double)(slavetime_measured
- chasetime_measured
) * crude
);
163 fine
= pic
->get_ratio (slavetime
- massaged_chasetime
, period_size
);
165 if (fabs(fine
-crude
) > crude
*speed_threshold
) {
166 std::cout
<< "reset to " << crude
<< " fine = " << fine
<< "\n";
173 if (abs(chasetime
-slavetime
) > pos_threshold
) {
176 want_locate_val
= chasetime
;
177 std::cout
<< "we are off by " << chasetime
-slavetime
<< " want_locate:" << chasetime
<< "\n";
182 std::cout
<< "not in control..." << crude
<< "\n";
191 PIChaser::feed_estimator (framepos_t realtime
, framepos_t chasetime
) {
193 realtime_stamps
[ array_index
%ESTIMATOR_SIZE
] = realtime
;
194 chasetime_stamps
[ array_index
%ESTIMATOR_SIZE
] = chasetime
;
198 PIChaser::get_estimate() {
202 framepos_t n1_realtime
;
203 framepos_t n1_chasetime
;
204 for( i
=(array_index
+ 1); i
<=(array_index
+ ESTIMATOR_SIZE
); i
++ ) {
205 if( realtime_stamps
[(i
)%ESTIMATOR_SIZE
] ) {
206 n1_realtime
= realtime_stamps
[(i
)%ESTIMATOR_SIZE
];
207 n1_chasetime
= chasetime_stamps
[(i
)%ESTIMATOR_SIZE
];
213 for( ; i
<=(array_index
+ ESTIMATOR_SIZE
); i
++ ) {
214 if( realtime_stamps
[(i
)%ESTIMATOR_SIZE
] ) {
215 if( (realtime_stamps
[(i
)%ESTIMATOR_SIZE
] - n1_realtime
) > 200 ) {
216 framepos_t n_realtime
= realtime_stamps
[(i
)%ESTIMATOR_SIZE
];
217 framepos_t n_chasetime
= chasetime_stamps
[(i
)%ESTIMATOR_SIZE
];
218 est
+= ((double)( n_chasetime
- n1_chasetime
))
219 / ((double)( n_realtime
- n1_realtime
));
220 n1_realtime
= n_realtime
;
221 n1_chasetime
= n_chasetime
;
228 return est
/(double)num
;