Line data Source code
1 : /** \file generalIntegrator.hpp
2 : * \author Jared R. Males (jaredmales@gmail.com)
3 : * \brief Declares and defines a general integrator controller class for AO control.
4 : * \ingroup mxAO_sim_files
5 : *
6 : */
7 :
8 : #ifndef __generalIntegrator_hpp__
9 : #define __generalIntegrator_hpp__
10 :
11 : #include <complex>
12 : #include <fstream>
13 : #include <iostream>
14 : #include <string>
15 : #include <vector>
16 :
17 : #include <Eigen/Dense>
18 :
19 : #include "../../error/mxErrorOld.hpp"
20 : #include "../../ioutils/stringUtils.hpp"
21 :
22 : #ifndef BREAD_CRUMB
23 : #define MXLIB_GENERAL_INTEGRATOR_LOCAL_BREAD_CRUMB
24 : #ifdef DEBUG
25 : #define BREAD_CRUMB std::cout << "DEBUG: " << __FILE__ << " " << __LINE__ << "\n";
26 : #else
27 : #define BREAD_CRUMB
28 : #endif
29 : #endif
30 :
31 : namespace mx
32 : {
33 : namespace AO
34 : {
35 : namespace sim
36 : {
37 :
38 : template <typename _realT>
39 : struct wfMeasurement
40 : {
41 : typedef _realT realT;
42 :
43 : realT iterNo;
44 :
45 : // typedef Eigen::Array< realT, Eigen::Dynamic, Eigen::Dynamic> commandT;
46 : typedef std::vector<realT> commandT;
47 :
48 : commandT measurement;
49 : };
50 :
51 : /// Implements a general integrator controller.
52 : /** \todo document the math for the G.I.
53 : * \todo document the filter coefficients, etc.
54 : *
55 : * \tparam _realT is the floating point type for all calculations.
56 : */
57 : template <typename _realT>
58 : class generalIntegrator
59 : {
60 :
61 : public:
62 : /// The real data type
63 : typedef _realT realT;
64 :
65 : /// The real data type
66 : typedef std::complex<realT> complexT;
67 :
68 : /// The wavefront data type
69 : // typedef wavefront<realT> wavefrontT;
70 :
71 : /// The command type
72 : typedef wfMeasurement<realT> commandT;
73 :
74 : /// The image type, used here as a general storage array
75 : typedef Eigen::Array<realT, Eigen::Dynamic, Eigen::Dynamic> imageT;
76 :
77 : /// Default c'tor.
78 : generalIntegrator();
79 :
80 : protected:
81 : int m_nModes{ 0 }; ///< The number of modes being filtered.
82 :
83 : bool m_openLoop{ false }; ///< If true, then commands are not integrated. Default is false.
84 :
85 : int m_openLoopDelay{ 0 }; ///< If > 0, then the loop is open for this time in time steps. Default is 0.
86 :
87 : int m_closingRamp{ 0 }; ///< If > 0, then gains are famped linearly up to m_closingGains over this interval in time
88 : ///< steps. Default is 0. This is relative m_openLoopDelay.
89 :
90 : int m_closingDelay{ 0 }; ///< If > 0, then the simple integrator,with m_closingGains is used up to this timestep.
91 : ///< This is relative m_openLoopDelay. Default = 0.
92 :
93 : imageT m_closingGains; ///< Column-vector of gains used for loop closing as simple integrator
94 :
95 : int m_lowOrders{ 0 }; ///< If > 0, then this sets the maximum mode number which is filtered. All remaining modes are
96 : ///< set to 0. Default = 0.
97 :
98 : imageT m_a;
99 : int m_currA;
100 :
101 : imageT m_b;
102 : int m_currB;
103 :
104 : imageT m_gains; ///< Column-vector of gains
105 :
106 : imageT m_commandsIn;
107 : imageT m_commandsOut;
108 :
109 : public:
110 : /// Allocate and initialize all state.
111 : /**
112 : * \returns 0 on success, a negative integer otherwise.
113 : */
114 : int initialize( int nModes /**< [in] the number of modes to be filtered */ );
115 :
116 : /// Get the number of modes
117 : /** nModes is only set by calling initialize.
118 : *
119 : * \returns the current value of m_nModes
120 : */
121 : int nModes();
122 :
123 : /// Set the m_openLoop flag.
124 : /** If m_openLoop is true, then commands are not filtered.
125 : *
126 : * \returns 0 on success, a negative integer on error.
127 : */
128 : int openLoop( bool ol /**< [in] the new value of m_openLoop */ );
129 :
130 : /// Get the value of the m_openLoop flag.
131 : /**
132 : * \returns the current value of m_openLoop.
133 : */
134 : bool openLoop();
135 :
136 : /// Set the open loop delay.
137 : /** If m_openLoopDelay > 0, then the loop is open for this period in time-steps
138 : * Requires m_closingDelay > 0.
139 : *
140 : * \returns 0 on success, a negative integer on error.
141 : */
142 : int openLoopDelay( int cr /**< [in] The new value of m_openLoopDelay */ );
143 :
144 : /// Get the value of the m_openLoopDelay.
145 : /**
146 : * \returns the current value of m_openLoopDelay.
147 : */
148 : int openLoopDelay();
149 :
150 : /// Set the closing ramp.
151 : /** If m_closingRamp > 0, then the gains are ramped linearly up to m_closingGains over this timer interval in
152 : * timesteps. Requires m_closingDelay > 0.
153 : *
154 : * \returns 0 on success, a negative integer on error.
155 : */
156 : int closingRamp( int cr /**< [in] The new value of m_closingRamp */ );
157 :
158 : /// Get the value of the m_closingRamp.
159 : /**
160 : * \returns the current value of m_closingRamp.
161 : */
162 : int closingRamp();
163 :
164 : /// Set the closing delay.
165 : /** If m_closingDelay > 0, then the simple integragor is used until this timestep.
166 : *
167 : * \returns 0 on success, a negative integer on error.
168 : */
169 : int closingDelay( int cd /**< [in] The new value of m_closingDelay */ );
170 :
171 : /// Get the value of the m_closingDelay.
172 : /**
173 : * \returns the current value of m_closingDelay.
174 : */
175 : int closingDelay();
176 :
177 : /// Set the simple integrator gains to use during closing.
178 : /**
179 : * \returns 0 on success
180 : * \returns < 0 on error
181 : */
182 : int closingGains( const std::vector<realT> &gains /**< [in] vector of gains.*/ );
183 :
184 : /// Set the simple integrator gain to use for all modes during closing.
185 : /**
186 : * \returns 0 on success
187 : * \returns < 0 on error
188 : */
189 : int closingGains( realT g );
190 :
191 : /// Set m_lowOrders.
192 : /** If m_lowOrders > 0, then this sets the maximum mode number which is filtered. All remaining modes are set to 0.
193 : *
194 : * \returns 0 on success, a negative integer on error.
195 : */
196 : int lowOrders( int lo /**< [in] The new value of m_lowOrders */ );
197 :
198 : /// Get the value of the m_lowOrders.
199 : /**
200 : * \returns the current value of m_lowOrders.
201 : */
202 : int lowOrders();
203 :
204 : /// Set the size of the IIR vector (the a coefficients)
205 : /** This allocates m_a to be m_nModes X n in size.
206 : *
207 : * \returns 0 on success, negative number on error.
208 : */
209 : int setASize( int n /**< [in] the number of IIR coefficients */ );
210 :
211 : /// Set the IIR coefficients for one mode.
212 : /**
213 : * \returns 0 on success, negative number on error.
214 : */
215 : int setA( int i, ///< [in] the mode number
216 : const imageT &a ///< [in] the IIR coefficients for this mode
217 : );
218 :
219 : /// Set the size of the FIR vector (the b coefficients)
220 : /** This allocates m_b to be m_nModes X n in size.
221 : *
222 : * \returns 0 on success, negative number on error.
223 : */
224 : int setBSize( int n /**< [in] the number of FIR coefficients */ );
225 :
226 : /// Set the FIR coefficients for one mode.
227 : /**
228 : * \returns 0 on success, negative number on error.
229 : */
230 : int setB( int i, ///< [in] the mode number
231 : const imageT &b ///< [in] the IIR coefficients for this mode
232 : );
233 :
234 : /// Get the gain for a single mode.
235 : /**
236 : * \returns the gain value if mode exists.
237 : */
238 : realT gain( int i /**< The mode number*/ );
239 :
240 : /// Set the gain for a single mode.
241 : /**
242 : * \returns 0 on success, negative number on error.
243 : */
244 : int gain( int i, ///< The mode number
245 : realT g ///< The new gain value
246 : );
247 :
248 : /// Set the gain for all modes to a single value
249 : /**
250 : * \returns 0 on success, negative number on error.
251 : */
252 : int gains( realT g /**< The new gain value*/ );
253 :
254 : /// Set the gains for all modes, using a vector to specify each gain
255 : /** The vector must be exactly as long as m_nModes.
256 : *
257 : * \returns 0 on success, negative number on error.
258 : */
259 : int gains( const std::vector<realT> &gains /**< [in] vector of gains.*/ );
260 :
261 : /// Set the gains for all modes, using a file to specify each gain
262 : /** The file format is a simple ASCII single column, with 1 gain per line.
263 : * Must be exactly as long as m_nModes.
264 : *
265 : * \returns 0 on success, negative number on error.
266 : */
267 : int gains( const std::string &ogainf /**< [in] the name of the file, full path */ );
268 :
269 : /// Allocate the provided command structures
270 : /** Used by the calling system to allocate the commands being passed between components.
271 : *
272 : * \returns 0 on success, negative number on error.
273 : */
274 : int initMeasurements( commandT &filtAmps, ///< The structure to contain the filtered commands
275 : commandT &rawAmps ///< The structure to contain the raw commands
276 : );
277 :
278 : int filterCommands( commandT &filtAmps, commandT &rawAmps, int iterNo );
279 : };
280 :
281 : template <typename realT>
282 0 : generalIntegrator<realT>::generalIntegrator()
283 : {
284 0 : }
285 :
286 : template <typename realT>
287 0 : int generalIntegrator<realT>::initialize( int nModes )
288 : {
289 0 : m_nModes = nModes;
290 :
291 : // If m_a has been sized, resize it
292 0 : if( m_a.cols() > 0 )
293 : {
294 0 : int n = m_a.cols();
295 0 : m_a.resize( m_nModes, n );
296 0 : m_a.setZero();
297 0 : m_currA = 0;
298 :
299 0 : m_commandsOut.resize( m_nModes, n );
300 0 : m_commandsOut.setZero();
301 : }
302 :
303 : // If m_b has been sized, resize it
304 0 : if( m_b.cols() > 0 )
305 : {
306 0 : int n = m_b.cols();
307 0 : m_b.resize( m_nModes, n );
308 0 : m_b.setZero();
309 0 : m_currB = 0;
310 :
311 0 : m_commandsIn.resize( m_nModes, n );
312 0 : m_commandsIn.setZero();
313 : }
314 :
315 0 : m_closingGains.resize( 1, m_nModes );
316 :
317 0 : m_closingGains.setZero();
318 :
319 0 : m_gains.resize( 1, m_nModes );
320 :
321 0 : m_gains.setZero();
322 :
323 0 : return 0;
324 : }
325 :
326 : template <typename realT>
327 0 : int generalIntegrator<realT>::nModes()
328 : {
329 0 : return m_nModes;
330 : }
331 :
332 : template <typename realT>
333 0 : int generalIntegrator<realT>::openLoop( bool ol )
334 : {
335 0 : m_openLoop = ol;
336 0 : return 0;
337 : }
338 :
339 : template <typename realT>
340 0 : bool generalIntegrator<realT>::openLoop()
341 : {
342 0 : return m_openLoop;
343 : }
344 :
345 : template <typename realT>
346 0 : int generalIntegrator<realT>::openLoopDelay( int old )
347 : {
348 0 : m_openLoopDelay = old;
349 :
350 0 : return 0;
351 : }
352 :
353 : template <typename realT>
354 0 : int generalIntegrator<realT>::openLoopDelay()
355 : {
356 0 : return m_openLoopDelay;
357 : }
358 :
359 : template <typename realT>
360 0 : int generalIntegrator<realT>::closingRamp( int cr )
361 : {
362 0 : m_closingRamp = cr;
363 :
364 0 : return 0;
365 : }
366 :
367 : template <typename realT>
368 0 : int generalIntegrator<realT>::closingRamp()
369 : {
370 0 : return m_closingRamp;
371 : }
372 :
373 : template <typename realT>
374 0 : int generalIntegrator<realT>::closingDelay( int cd )
375 : {
376 0 : m_closingDelay = cd;
377 :
378 0 : return 0;
379 : }
380 :
381 : template <typename realT>
382 0 : int generalIntegrator<realT>::closingDelay()
383 : {
384 0 : return m_closingDelay;
385 : }
386 :
387 : template <typename realT>
388 0 : int generalIntegrator<realT>::closingGains( const std::vector<realT> &gains )
389 : {
390 :
391 0 : if( gains.size() != (size_t)m_closingGains.cols() )
392 : {
393 0 : mxError( "generalIntegrator::closingGains", MXE_SIZEERR, "input gain vector not same size as number of modes" );
394 0 : return -1; ///\retval -1 on vector size mismatch
395 : }
396 :
397 0 : for( int i = 0; i < m_closingGains.cols(); ++i )
398 : {
399 0 : m_closingGains( 0, i ) = gains[i];
400 : }
401 :
402 0 : return 0;
403 : }
404 :
405 : template <typename realT>
406 0 : int generalIntegrator<realT>::closingGains( realT g )
407 : {
408 0 : for( int i = 0; i < m_closingGains.cols(); ++i )
409 : {
410 0 : m_closingGains( 0, i ) = g;
411 : }
412 :
413 0 : return 0;
414 : }
415 :
416 : template <typename realT>
417 0 : int generalIntegrator<realT>::lowOrders( int lo )
418 : {
419 0 : m_lowOrders = lo;
420 :
421 0 : return 0;
422 : }
423 :
424 : template <typename realT>
425 0 : int generalIntegrator<realT>::lowOrders()
426 : {
427 0 : return m_lowOrders;
428 : }
429 :
430 : template <typename realT>
431 0 : int generalIntegrator<realT>::setASize( int n )
432 : {
433 : // Resize with m_nModes if set
434 0 : if( m_nModes > 0 )
435 : {
436 0 : m_a.resize( m_nModes, n );
437 0 : m_commandsOut.resize( m_nModes, n );
438 : }
439 : else
440 : {
441 0 : m_a.resize( 1, n );
442 0 : m_commandsOut.resize( 1, n );
443 : }
444 :
445 0 : m_a.setZero();
446 0 : m_currA = 0;
447 0 : m_commandsOut.setZero();
448 :
449 0 : return 0;
450 : }
451 :
452 : template <typename realT>
453 0 : int generalIntegrator<realT>::setA( int i, const imageT &a )
454 : {
455 0 : m_a.row( i ) = a;
456 :
457 0 : return 0;
458 : }
459 :
460 : template <typename realT>
461 0 : int generalIntegrator<realT>::setBSize( int n )
462 : {
463 : // Resize with m_nModes if set
464 0 : if( m_nModes > 0 )
465 : {
466 0 : m_b.resize( m_nModes, n );
467 0 : m_commandsIn.resize( m_nModes, n );
468 : }
469 : else
470 : {
471 0 : m_b.resize( 1, n );
472 0 : m_commandsIn.resize( 1, n );
473 : }
474 :
475 0 : m_b.setZero();
476 0 : m_currB = 0;
477 0 : m_commandsIn.setZero();
478 :
479 0 : return 0;
480 : }
481 :
482 : template <typename realT>
483 0 : int generalIntegrator<realT>::setB( int i, const imageT &b )
484 : {
485 0 : m_b.row( i ) = b;
486 :
487 0 : return 0;
488 : }
489 :
490 : template <class realT>
491 0 : realT generalIntegrator<realT>::gain( int i )
492 : {
493 0 : if( i < 0 || i >= m_nModes )
494 : {
495 0 : mxError( "generalIntegrator::gain", MXE_INVALIDARG, "mode index out of range" );
496 0 : return 0; ///\retval 0 if the mode doesn't exist.
497 : }
498 :
499 0 : return m_gains( i );
500 : }
501 :
502 : template <typename realT>
503 0 : int generalIntegrator<realT>::gain( int i, realT g )
504 : {
505 0 : if( i < 0 || i >= m_nModes )
506 : {
507 0 : mxError( "generalIntegrator::gain", MXE_INVALIDARG, "mode index out of range" );
508 0 : return 0; ///\retval 0 if the mode doesn't exist.
509 : }
510 :
511 0 : m_gains( 0, i ) = g;
512 :
513 0 : return 0;
514 : }
515 :
516 : template <typename realT>
517 0 : int generalIntegrator<realT>::gains( realT g )
518 : {
519 0 : for( int i = 0; i < m_gains.cols(); ++i )
520 : {
521 0 : m_gains( 0, i ) = g;
522 : }
523 :
524 0 : return 0;
525 : }
526 :
527 : template <typename realT>
528 0 : int generalIntegrator<realT>::gains( const std::vector<realT> &gains )
529 : {
530 :
531 0 : if( gains.size() != (size_t)m_gains.cols() )
532 : {
533 0 : mxError( "generalIntegrator::gains", MXE_SIZEERR, "input gain vector not same size as number of modes" );
534 0 : return -1; ///\retval -1 on vector size mismatch
535 : }
536 :
537 0 : for( int i = 0; i < m_gains.cols(); ++i )
538 : {
539 0 : m_gains( 0, i ) = gains[i];
540 : }
541 :
542 0 : return 0;
543 : }
544 :
545 : template <typename realT>
546 0 : int generalIntegrator<realT>::gains( const std::string &ogainf )
547 : {
548 0 : std::ifstream fin;
549 0 : fin.open( ogainf );
550 :
551 0 : if( !fin.good() )
552 : {
553 0 : mxError( "generalIntegrator::gains", MXE_FILEOERR, "could not open gan file" );
554 0 : return -1; /// \retval -1 if file open fails
555 : }
556 :
557 0 : std::string tmpstr;
558 : realT g;
559 0 : for( int i = 0; i < m_gains.cols(); ++i )
560 : {
561 0 : fin >> tmpstr;
562 0 : g = ioutils::convertFromString<realT>( tmpstr );
563 0 : gain( i, g );
564 : }
565 :
566 0 : return 0;
567 0 : }
568 :
569 : template <class realT>
570 0 : int generalIntegrator<realT>::initMeasurements( commandT &filtAmps, commandT &rawAmps )
571 : {
572 0 : filtAmps.measurement.resize( m_nModes );
573 0 : for( size_t n = 0; n < m_nModes; ++n )
574 0 : filtAmps.measurement[n] = 0;
575 : // filtAmps.measurement.setZero();
576 :
577 0 : rawAmps.measurement.resize( m_nModes );
578 0 : for( size_t n = 0; n < m_nModes; ++n )
579 0 : rawAmps.measurement[n] = 0;
580 : // rawAmps.measurement.resize(1, m_nModes);
581 : // rawAmps.measurement.setZero();
582 :
583 0 : return 0;
584 : }
585 :
586 : template <typename realT>
587 0 : int generalIntegrator<realT>::filterCommands( commandT &filtAmps, commandT &rawAmps, int iterNo )
588 : {
589 0 : filtAmps.iterNo = rawAmps.iterNo;
590 :
591 0 : if( m_openLoop || iterNo < m_openLoopDelay )
592 : {
593 0 : for( size_t n = 0; n < m_nModes; ++n )
594 0 : filtAmps.measurement[n] = 0;
595 : // filtAmps.measurement.setZero();
596 0 : return 0;
597 : }
598 :
599 : realT aTot, bTot;
600 :
601 0 : if( iterNo - m_openLoopDelay < m_closingDelay )
602 : {
603 : BREAD_CRUMB;
604 :
605 0 : for( int i = 0; i < m_nModes; ++i )
606 : {
607 : // if( std::isnan( rawAmps.measurement(0,i) ) || !std::isfinite(rawAmps.measurement(0,i)))
608 : // rawAmps.measurement(0,i) = 0.0;
609 0 : if( std::isnan( rawAmps.measurement[i] ) || !std::isfinite( rawAmps.measurement[i] ) )
610 0 : rawAmps.measurement[i] = 0.0;
611 :
612 : // m_commandsIn(i, m_currB) = rawAmps.measurement(0,i);
613 0 : m_commandsIn( i, m_currB ) = rawAmps.measurement[i];
614 :
615 0 : aTot = m_commandsOut( i, m_currA );
616 : // std::cerr << m_currA << " " << aTot << "\n";
617 :
618 0 : int cA = m_currA + 1;
619 0 : if( cA >= m_a.cols() )
620 0 : cA = 0;
621 :
622 0 : realT gf = 1.0;
623 :
624 0 : if( iterNo - m_openLoopDelay < m_closingRamp )
625 0 : gf = ( (realT)iterNo - m_openLoopDelay ) / m_closingRamp;
626 :
627 : // m_commandsOut(i, cA) = aTot + gf*m_closingGains(i) * rawAmps.measurement(0,i);
628 0 : m_commandsOut( i, cA ) = aTot + gf * m_closingGains( i ) * rawAmps.measurement[i];
629 :
630 : // std::cerr << cA << " " << m_commandsOut(i, cA) << "\n";
631 :
632 0 : if( i <= m_lowOrders || m_lowOrders <= 0 )
633 : {
634 0 : filtAmps.measurement[i] = m_commandsOut( i, cA );
635 : }
636 : else
637 : {
638 0 : filtAmps.measurement[i] = 0;
639 : }
640 : }
641 :
642 0 : ++m_currB;
643 0 : if( m_currB >= m_b.cols() )
644 0 : m_currB = 0;
645 :
646 0 : ++m_currA;
647 0 : if( m_currA >= m_a.cols() )
648 0 : m_currA = 0;
649 :
650 0 : return 0;
651 : }
652 :
653 0 : for( int i = 0; i < m_nModes; ++i )
654 : {
655 0 : if( m_gains( i ) == 0 )
656 : {
657 : // filtAmps.measurement(0,i) = 0;
658 0 : filtAmps.measurement[i] = 0;
659 0 : continue;
660 : }
661 :
662 : // if( std::isnan( rawAmps.measurement(0,i) ) || !std::isfinite(rawAmps.measurement(0,i)))
663 : // rawAmps.measurement(0,i) = 0.0;
664 0 : if( std::isnan( rawAmps.measurement[i] ) || !std::isfinite( rawAmps.measurement[i] ) )
665 0 : rawAmps.measurement[i] = 0.0;
666 :
667 0 : aTot = 0;
668 0 : for( int j = 0; j < m_a.cols(); ++j )
669 : {
670 0 : int k = m_currA - j;
671 0 : if( k < 0 )
672 0 : k += m_a.cols();
673 0 : aTot += m_a( i, j ) * m_commandsOut( i, k );
674 : }
675 :
676 0 : m_commandsIn( i, m_currB ) = rawAmps.measurement[i];
677 :
678 0 : bTot = 0;
679 0 : for( int j = 0; j < m_b.cols(); ++j )
680 : {
681 0 : int k = m_currB - j;
682 0 : if( k < 0 )
683 0 : k += m_b.cols();
684 :
685 0 : bTot += m_b( i, j ) * m_commandsIn( i, k );
686 : }
687 :
688 0 : int cA = m_currA + 1;
689 0 : if( cA >= m_a.cols() )
690 0 : cA = 0;
691 :
692 0 : m_commandsOut( i, cA ) = aTot + m_gains( i ) * bTot;
693 :
694 0 : filtAmps.measurement[i] = m_commandsOut( i, cA );
695 : }
696 :
697 0 : ++m_currB;
698 0 : if( m_currB >= m_b.cols() )
699 0 : m_currB = 0;
700 :
701 0 : ++m_currA;
702 0 : if( m_currA >= m_a.cols() )
703 0 : m_currA = 0;
704 :
705 0 : return 0;
706 : }
707 :
708 : } // namespace sim
709 : } // namespace AO
710 : } // namespace mx
711 :
712 : #ifdef MXLIB_GENERAL_INTEGRATOR_LOCAL_BREAD_CRUMB
713 : #undef BREAD_CRUMB
714 : #undef MXLIB_GENERAL_INTEGRATOR_LOCAL_BREAD_CRUMB
715 : #endif
716 :
717 : #endif //__generalIntegrator_hpp__
|
