Line data Source code
1 : /** \file modalFilter.hpp
2 : * \brief The MagAO-X modal filter header file
3 : *
4 : * \ingroup modalFilter_files
5 : */
6 :
7 : #ifndef modalFilter_hpp
8 : #define modalFilter_hpp
9 :
10 : #include <mutex>
11 : #include <shared_mutex>
12 :
13 : #include "../../libMagAOX/libMagAOX.hpp" //Note this is included on command line to trigger pch
14 : #include "../../magaox_git_version.h"
15 :
16 : #include <mx/sigproc/circularBuffer.hpp>
17 :
18 : /** \defgroup modalFilter
19 : * \brief The MagAO-X application to perform modal filtering
20 : *
21 : * <a href="../handbook/operating/software/apps/modalFilter.html">Application Documentation</a>
22 : *
23 : * \ingroup apps
24 : *
25 : */
26 :
27 : /** \defgroup modalFilter_files
28 : * \ingroup modalFilter
29 : */
30 :
31 : namespace MagAOX
32 : {
33 : namespace app
34 : {
35 :
36 : struct gainFactShmimT
37 : {
38 0 : static std::string configSection()
39 : {
40 0 : return "gainFactShmim";
41 : };
42 :
43 0 : static std::string indiPrefix()
44 : {
45 0 : return "gainFact";
46 : };
47 : };
48 :
49 : struct multFactShmimT
50 : {
51 0 : static std::string configSection()
52 : {
53 0 : return "multFactShmim";
54 : };
55 :
56 0 : static std::string indiPrefix()
57 : {
58 0 : return "multFact";
59 : };
60 : };
61 :
62 : struct pcGainFactShmimT
63 : {
64 0 : static std::string configSection()
65 : {
66 0 : return "pcGainFactShmim";
67 : };
68 :
69 0 : static std::string indiPrefix()
70 : {
71 0 : return "pcGainFact";
72 : };
73 : };
74 :
75 : struct pcMultFactShmimT
76 : {
77 0 : static std::string configSection()
78 : {
79 0 : return "pcMultFactShmim";
80 : };
81 :
82 0 : static std::string indiPrefix()
83 : {
84 0 : return "pcMultFact";
85 : };
86 : };
87 :
88 : struct acoeffShmimT
89 : {
90 0 : static std::string configSection()
91 : {
92 0 : return "acoeffShmim";
93 : };
94 :
95 0 : static std::string indiPrefix()
96 : {
97 0 : return "acoeff";
98 : };
99 : };
100 :
101 : struct bcoeffShmimT
102 : {
103 0 : static std::string configSection()
104 : {
105 0 : return "bcoeffShmim";
106 : };
107 :
108 0 : static std::string indiPrefix()
109 : {
110 0 : return "bcoeff";
111 : };
112 : };
113 :
114 : struct modevalShmimT
115 : {
116 0 : static std::string configSection()
117 : {
118 0 : return "modevalShmim";
119 : };
120 :
121 0 : static std::string indiPrefix()
122 : {
123 0 : return "modeval";
124 : };
125 : };
126 :
127 : /// The MagAO-X modal filter
128 : /**
129 : * \ingroup modalFilter
130 : */
131 : class modalFilter : public MagAOXApp<true>,
132 : dev::shmimMonitor<modalFilter, gainFactShmimT>,
133 : dev::shmimMonitor<modalFilter, multFactShmimT>,
134 : dev::shmimMonitor<modalFilter, pcGainFactShmimT>,
135 : dev::shmimMonitor<modalFilter, pcMultFactShmimT>,
136 : dev::shmimMonitor<modalFilter, acoeffShmimT>,
137 : dev::shmimMonitor<modalFilter, bcoeffShmimT>,
138 : dev::shmimMonitor<modalFilter, modevalShmimT>,
139 : dev::frameGrabber<modalFilter>,
140 : dev::telemeter<modalFilter>
141 : {
142 :
143 : // Give the test harness access.
144 : friend class modalFilter_test;
145 :
146 : friend class dev::shmimMonitor<modalFilter, gainFactShmimT>;
147 : friend class dev::shmimMonitor<modalFilter, multFactShmimT>;
148 : friend class dev::shmimMonitor<modalFilter, pcGainFactShmimT>;
149 : friend class dev::shmimMonitor<modalFilter, pcMultFactShmimT>;
150 : friend class dev::shmimMonitor<modalFilter, acoeffShmimT>;
151 : friend class dev::shmimMonitor<modalFilter, bcoeffShmimT>;
152 : friend class dev::shmimMonitor<modalFilter, modevalShmimT>;
153 : friend class dev::frameGrabber<modalFilter>;
154 : friend class dev::telemeter<modalFilter>;
155 :
156 : typedef dev::shmimMonitor<modalFilter, gainFactShmimT> gainFactShmimMonitorT;
157 : typedef dev::shmimMonitor<modalFilter, multFactShmimT> multFactShmimMonitorT;
158 : typedef dev::shmimMonitor<modalFilter, pcGainFactShmimT> pcGainFactShmimMonitorT;
159 : typedef dev::shmimMonitor<modalFilter, pcMultFactShmimT> pcMultFactShmimMonitorT;
160 : typedef dev::shmimMonitor<modalFilter, acoeffShmimT> acoeffShmimMonitorT;
161 : typedef dev::shmimMonitor<modalFilter, bcoeffShmimT> bcoeffShmimMonitorT;
162 : typedef dev::shmimMonitor<modalFilter, modevalShmimT> modevalShmimMonitorT;
163 : typedef dev::frameGrabber<modalFilter> frameGrabberT;
164 : typedef dev::telemeter<modalFilter> telemeterT;
165 :
166 : static constexpr bool c_frameGrabber_flippable = false; /**< app:dev config to tell framegrabber these images
167 : can not be flipped*/
168 :
169 : protected:
170 : /** \name Configurable Parameters
171 : *@{
172 : */
173 :
174 : std::string m_fpsDevice; ///< Device name for getting fps to set circular buffer length.
175 : std::string m_fpsProperty{ "fps" }; ///< Property name for getting fps to set circular buffer length.
176 : std::string m_fpsElement{ "current" }; ///< Element name for getting fps to set circular buffer length.
177 :
178 : float m_fpsTol{ 0 }; ///< The tolerance for detecting a change in FPS.
179 :
180 : int m_loopNum{ 1 }; ///< The number of the loop. Used to set shmim names, as in aolN_mgainfact. Default is 1.
181 :
182 : std::string m_loopName{ "ho" }; ///< The name of the loop control INDI device name. Defalt is "ho".
183 :
184 : int32_t m_modevalCBLength{ 1000 }; ///< The length of the modeval circular buffers. Default is 1000 entries.
185 :
186 : ///@}
187 :
188 : float m_fps{ 0 };
189 :
190 : bool m_loop{ false };
191 :
192 : float m_gain{ 0 };
193 :
194 : float m_mc{ 1 };
195 :
196 : bool m_pcOn{ false };
197 :
198 : float m_pcGain{ 0 };
199 :
200 : float m_pcMc{ 1 };
201 :
202 : std::vector<float> m_gainfacts;
203 :
204 : std::vector<float> m_multfacts;
205 :
206 : std::vector<float> m_pcGainfacts;
207 :
208 : std::vector<float> m_pcMultfacts;
209 :
210 : std::vector<int> m_Na;
211 :
212 : std::vector<int> m_Nb;
213 :
214 : eigenImage<float> m_as;
215 :
216 : eigenImage<float> m_bs;
217 :
218 : uint32_t m_modevalSz{ 0 };
219 :
220 : mx::sigproc::circularBufferIndex<std::vector<float>, int32_t> m_modevalWFS;
221 :
222 : mx::sigproc::circularBufferIndex<std::vector<float>, int32_t> m_modevalDM;
223 :
224 : sem_t m_filtSem; ///< Semaphore used to signal that fresh modevals are waiting to be filtered
225 :
226 : bool m_sizesMatch{ false }; ///< Flag indicating that all sizes are consistent
227 :
228 : bool m_pcSizesMatch{ false }; ///< Flag indicating that all sizes are consistent
229 :
230 : std::shared_mutex m_filtMutex; ///< Mutex for locking access to filter parameters
231 :
232 : timespec m_atime; ///< The acq time of the WFS modevals
233 : public:
234 : /// Default c'tor.
235 : modalFilter();
236 :
237 : /// D'tor, declared and defined for noexcept.
238 0 : ~modalFilter() noexcept
239 0 : {
240 0 : }
241 :
242 : virtual void setupConfig();
243 :
244 : /// Implementation of loadConfig logic, separated for testing.
245 : /** This is called by loadConfig().
246 : */
247 : int loadConfigImpl( mx::app::appConfigurator &_config /**< [in] an application configuration
248 : from which to load values*/ );
249 :
250 : virtual void loadConfig();
251 :
252 : /// Startup function
253 : /**
254 : *
255 : */
256 : virtual int appStartup();
257 :
258 : /// Implementation of the FSM for modalFilter.
259 : /**
260 : * \returns 0 on no critical error
261 : * \returns -1 on an error requiring shutdown
262 : */
263 : virtual int appLogic();
264 :
265 : /// Shutdown the app.
266 : /**
267 : *
268 : */
269 : virtual int appShutdown();
270 :
271 : int allocate( const gainFactShmimT & ///< [in] tag to differentiate shmimMonitor parents.
272 : );
273 :
274 : int processImage( void *curr_src, ///< [in] pointer to the start of the current frame
275 : const gainFactShmimT & ///< [in] tag to differentiate shmimMonitor parents.
276 : );
277 :
278 : int allocate( const multFactShmimT & ///< [in] tag to differentiate shmimMonitor parents.
279 : );
280 :
281 : int processImage( void *curr_src, ///< [in] pointer to the start of the current frame
282 : const multFactShmimT & ///< [in] tag to differentiate shmimMonitor parents.
283 : );
284 :
285 : int allocate( const pcGainFactShmimT & ///< [in] tag to differentiate shmimMonitor parents.
286 : );
287 :
288 : int processImage( void *curr_src, ///< [in] pointer to the start of the current frame
289 : const pcGainFactShmimT & ///< [in] tag to differentiate shmimMonitor parents.
290 : );
291 :
292 : int allocate( const pcMultFactShmimT & ///< [in] tag to differentiate shmimMonitor parents.
293 : );
294 :
295 : int processImage( void *curr_src, ///< [in] pointer to the start of the current frame
296 : const pcMultFactShmimT & ///< [in] tag to differentiate shmimMonitor parents.
297 : );
298 :
299 : int allocate( const acoeffShmimT & ///< [in] tag to differentiate shmimMonitor parents.
300 : );
301 :
302 : int processImage( void *curr_src, ///< [in] pointer to the start of the current frame
303 : const acoeffShmimT & ///< [in] tag to differentiate shmimMonitor parents.
304 : );
305 :
306 : int allocate( const bcoeffShmimT & ///< [in] tag to differentiate shmimMonitor parents.
307 : );
308 :
309 : int processImage( void *curr_src, ///< [in] pointer to the start of the current frame
310 : const bcoeffShmimT & ///< [in] tag to differentiate shmimMonitor parents.
311 : );
312 :
313 : int allocate( const modevalShmimT & ///< [in] tag to differentiate shmimMonitor parents.
314 : );
315 :
316 : int processImage( void *curr_src, ///< [in] pointer to the start of the current frame
317 : const modevalShmimT & ///< [in] tag to differentiate shmimMonitor parents.
318 : );
319 :
320 : /** \name frameGrabber interface
321 : * @{
322 : */
323 :
324 : /// Implementation of the framegrabber configureAcquisition interface
325 : /**
326 : *
327 : * \returns 0 on success
328 : * \returns -1 on error
329 : */
330 : int configureAcquisition();
331 :
332 : /// Implementation of the frameGrabber fps interface
333 : /** Just returns the value of m_fps
334 : */
335 : float fps();
336 :
337 : /// Implementation of the framegrabber startAcquisition interface
338 : /**
339 : *
340 : * \returns 0 on success
341 : * \returns -1 on error
342 : */
343 : int startAcquisition();
344 :
345 : /// Implementation of the framegrabber acquireAndCheckValid interface
346 : /**
347 : *
348 : * \returns 0 on success
349 : * \returns -1 on error
350 : */
351 : int acquireAndCheckValid();
352 :
353 : /// Implementation of the framegrabber loadImageIntoStream interface
354 : /**
355 : *
356 : * \returns 0 on success
357 : * \returns -1 on error
358 : */
359 : int loadImageIntoStream( void *dest /**< [in] */ );
360 :
361 : /// Implementation of the framegrabber reconfig interface
362 : /**
363 : * \returns 0 on success
364 : * \returns -1 on error
365 : */
366 : int reconfig();
367 :
368 : ///@}
369 :
370 : void checkSizes();
371 :
372 : /** \name Telemeter Interface
373 : *
374 : * @{
375 : */
376 : int checkRecordTimes();
377 :
378 : int recordTelem( const telem_fgtimings * );
379 :
380 : ///@}
381 :
382 : /** \name INDI
383 : * @{
384 : */
385 :
386 : pcf::IndiProperty m_indiP_fpsSource;
387 :
388 : pcf::IndiProperty m_indiP_loop;
389 :
390 : pcf::IndiProperty m_indiP_gain;
391 :
392 : pcf::IndiProperty m_indiP_mult;
393 :
394 : pcf::IndiProperty m_indiP_pcGain;
395 :
396 : pcf::IndiProperty m_indiP_pcMult;
397 :
398 : pcf::IndiProperty m_indiP_pcOn;
399 :
400 0 : INDI_SETCALLBACK_DECL( modalFilter, m_indiP_fpsSource );
401 :
402 0 : INDI_NEWCALLBACK_DECL( modalFilter, m_indiP_loop );
403 :
404 0 : INDI_NEWCALLBACK_DECL( modalFilter, m_indiP_gain );
405 :
406 0 : INDI_NEWCALLBACK_DECL( modalFilter, m_indiP_mult );
407 :
408 0 : INDI_NEWCALLBACK_DECL( modalFilter, m_indiP_pcGain );
409 :
410 0 : INDI_NEWCALLBACK_DECL( modalFilter, m_indiP_pcMult );
411 :
412 0 : INDI_NEWCALLBACK_DECL( modalFilter, m_indiP_pcOn );
413 :
414 : ///@}
415 : };
416 :
417 0 : modalFilter::modalFilter() : MagAOXApp( MAGAOX_CURRENT_SHA1, MAGAOX_REPO_MODIFIED )
418 : {
419 :
420 0 : gainFactShmimMonitorT::m_getExistingFirst = true;
421 0 : multFactShmimMonitorT::m_getExistingFirst = true;
422 0 : pcGainFactShmimMonitorT::m_getExistingFirst = true;
423 0 : pcMultFactShmimMonitorT::m_getExistingFirst = true;
424 0 : acoeffShmimMonitorT::m_getExistingFirst = true;
425 0 : bcoeffShmimMonitorT::m_getExistingFirst = true;
426 0 : modevalShmimMonitorT::m_getExistingFirst = true;
427 0 : return;
428 0 : }
429 :
430 0 : void modalFilter::setupConfig()
431 : {
432 0 : config.add( "circBuff.fpsDevice",
433 : "",
434 : "circBuff.fpsDevice",
435 : argType::Required,
436 : "circBuff",
437 : "fpsDevice",
438 : false,
439 : "string",
440 : "Device name for getting fps to set circular buffer length." );
441 0 : config.add( "circBuff.fpsProperty",
442 : "",
443 : "circBuff.fpsProperty",
444 : argType::Required,
445 : "circBuff",
446 : "fpsProperty",
447 : false,
448 : "string",
449 : "Property name for getting fps to set circular buffer length. Default is 'fps'." );
450 0 : config.add( "circBuff.fpsElement",
451 : "",
452 : "circBuff.fpsElement",
453 : argType::Required,
454 : "circBuff",
455 : "fpsElement",
456 : false,
457 : "string",
458 : "Property name for getting fps to set circular buffer length. Default is 'current'." );
459 0 : config.add( "circBuff.fpsTol",
460 : "",
461 : "circBuff.fpsTol",
462 : argType::Required,
463 : "circBuff",
464 : "fpsTol",
465 : false,
466 : "float",
467 : "Tolerance for detecting a change in FPS. Default is 0." );
468 :
469 0 : config.add( "loop.number",
470 : "",
471 : "loop.number",
472 : argType::Required,
473 : "loop",
474 : "number",
475 : false,
476 : "int",
477 : "The number of the loop. Used to set shmim names, as in aolN_mgainfact. Default is 1." );
478 :
479 0 : config.add( "loop.name",
480 : "",
481 : "loop.name",
482 : argType::Required,
483 : "loop",
484 : "name",
485 : false,
486 : "string",
487 : "The name of the loop control INDI device name. Default is \"ho\"." );
488 :
489 0 : SHMIMMONITORT_SETUP_CONFIG( gainFactShmimMonitorT, config );
490 0 : SHMIMMONITORT_SETUP_CONFIG( multFactShmimMonitorT, config );
491 0 : SHMIMMONITORT_SETUP_CONFIG( pcGainFactShmimMonitorT, config );
492 0 : SHMIMMONITORT_SETUP_CONFIG( pcMultFactShmimMonitorT, config );
493 0 : SHMIMMONITORT_SETUP_CONFIG( acoeffShmimMonitorT, config );
494 0 : SHMIMMONITORT_SETUP_CONFIG( bcoeffShmimMonitorT, config );
495 0 : SHMIMMONITORT_SETUP_CONFIG( modevalShmimMonitorT, config );
496 0 : FRAMEGRABBER_SETUP_CONFIG( config );
497 0 : TELEMETER_SETUP_CONFIG( config );
498 : }
499 :
500 0 : int modalFilter::loadConfigImpl( mx::app::appConfigurator &_config )
501 : {
502 0 : _config( m_fpsDevice, "circBuff.fpsDevice" );
503 0 : _config( m_fpsProperty, "circBuff.fpsProperty" );
504 0 : _config( m_fpsElement, "circBuff.fpsElement" );
505 0 : _config( m_fpsTol, "circBuff.fpsTol" );
506 :
507 0 : _config( m_loopNum, "loop.number" );
508 0 : _config( m_loopName, "loop.name" );
509 :
510 : char shmim[1024];
511 :
512 0 : snprintf( shmim, sizeof( shmim ), "aol%d_mgainfact", m_loopNum );
513 0 : gainFactShmimMonitorT::m_shmimName = shmim;
514 0 : SHMIMMONITORT_LOAD_CONFIG( gainFactShmimMonitorT, _config );
515 :
516 0 : snprintf( shmim, sizeof( shmim ), "aol%d_mmultfact", m_loopNum );
517 0 : multFactShmimMonitorT::m_shmimName = shmim;
518 0 : SHMIMMONITORT_LOAD_CONFIG( multFactShmimMonitorT, _config );
519 :
520 0 : snprintf( shmim, sizeof( shmim ), "aol%d_mpcgainfact", m_loopNum );
521 0 : pcGainFactShmimMonitorT::m_shmimName = shmim;
522 0 : SHMIMMONITORT_LOAD_CONFIG( pcGainFactShmimMonitorT, _config );
523 :
524 0 : snprintf( shmim, sizeof( shmim ), "aol%d_mpcmultfact", m_loopNum );
525 0 : pcMultFactShmimMonitorT::m_shmimName = shmim;
526 0 : SHMIMMONITORT_LOAD_CONFIG( pcMultFactShmimMonitorT, _config );
527 :
528 0 : snprintf( shmim, sizeof( shmim ), "aol%d_acoeff", m_loopNum );
529 0 : acoeffShmimMonitorT::m_shmimName = shmim;
530 0 : SHMIMMONITORT_LOAD_CONFIG( acoeffShmimMonitorT, _config );
531 :
532 0 : snprintf( shmim, sizeof( shmim ), "aol%d_bcoeff", m_loopNum );
533 0 : bcoeffShmimMonitorT::m_shmimName = shmim;
534 0 : SHMIMMONITORT_LOAD_CONFIG( bcoeffShmimMonitorT, _config );
535 :
536 0 : snprintf( shmim, sizeof( shmim ), "aol%d_modevalWFS", m_loopNum );
537 0 : modevalShmimMonitorT::m_shmimName = shmim;
538 0 : SHMIMMONITORT_LOAD_CONFIG( modevalShmimMonitorT, _config );
539 :
540 0 : snprintf( shmim, sizeof( shmim ), "aol%d_modevalDM", m_loopNum );
541 0 : frameGrabberT::m_shmimName = shmim;
542 0 : FRAMEGRABBER_LOAD_CONFIG( _config );
543 :
544 0 : TELEMETER_LOAD_CONFIG( _config );
545 :
546 0 : return 0;
547 : }
548 :
549 0 : void modalFilter::loadConfig()
550 : {
551 0 : loadConfigImpl( config );
552 0 : }
553 :
554 0 : int modalFilter::appStartup()
555 : {
556 :
557 0 : if( sem_init( &m_filtSem, 0, 0 ) < 0 )
558 : {
559 0 : return log<software_critical, -1>( { __FILE__, __LINE__, errno, 0, "Initializing filter semaphore" } );
560 : }
561 :
562 0 : SHMIMMONITORT_APP_STARTUP( gainFactShmimMonitorT );
563 0 : SHMIMMONITORT_APP_STARTUP( multFactShmimMonitorT );
564 0 : SHMIMMONITORT_APP_STARTUP( pcGainFactShmimMonitorT );
565 0 : SHMIMMONITORT_APP_STARTUP( pcMultFactShmimMonitorT );
566 0 : SHMIMMONITORT_APP_STARTUP( acoeffShmimMonitorT );
567 0 : SHMIMMONITORT_APP_STARTUP( bcoeffShmimMonitorT );
568 0 : SHMIMMONITORT_APP_STARTUP( modevalShmimMonitorT );
569 0 : FRAMEGRABBER_APP_STARTUP;
570 0 : TELEMETER_APP_STARTUP;
571 :
572 0 : if( m_fpsDevice == "" )
573 : {
574 0 : return log<software_critical, -1>(
575 0 : { __FILE__, __LINE__, "FPS source is not configurated (circBuff.fpsDevice)" } );
576 : }
577 :
578 0 : REG_INDI_SETPROP( m_indiP_fpsSource, m_fpsDevice, m_fpsProperty );
579 :
580 0 : CREATE_REG_INDI_NEW_TOGGLESWITCH( m_indiP_loop, "loop_state" );
581 0 : CREATE_REG_INDI_NEW_NUMBERF( m_indiP_gain, "loop_gain", 0, 1, 0.01, "%0.01f", "Gain", "Loop" );
582 0 : CREATE_REG_INDI_NEW_NUMBERF( m_indiP_mult, "loop_multcoeff", 0, 1, 0.01, "%0.01f", "Mult. Coeff.", "Loop" );
583 0 : CREATE_REG_INDI_NEW_NUMBERF( m_indiP_pcGain, "loop_pcgain", 0, 1, 0.01, "%0.01f", "PC Gain", "Loop" );
584 0 : CREATE_REG_INDI_NEW_NUMBERF( m_indiP_pcMult, "loop_pcmultcoeff", 0, 1, 0.01, "%0.01f", "PC Mult. Coeff.", "Loop" );
585 :
586 0 : CREATE_REG_INDI_NEW_TOGGLESWITCH( m_indiP_pcOn, "loop_pcon" );
587 :
588 0 : state( stateCodes::OPERATING );
589 :
590 0 : return 0;
591 : }
592 :
593 0 : int modalFilter::appLogic()
594 : {
595 0 : SHMIMMONITORT_APP_LOGIC( gainFactShmimMonitorT );
596 0 : SHMIMMONITORT_APP_LOGIC( multFactShmimMonitorT );
597 0 : SHMIMMONITORT_APP_LOGIC( pcGainFactShmimMonitorT );
598 0 : SHMIMMONITORT_APP_LOGIC( pcMultFactShmimMonitorT );
599 0 : SHMIMMONITORT_APP_LOGIC( acoeffShmimMonitorT );
600 0 : SHMIMMONITORT_APP_LOGIC( bcoeffShmimMonitorT );
601 0 : SHMIMMONITORT_APP_LOGIC( modevalShmimMonitorT );
602 0 : FRAMEGRABBER_APP_LOGIC;
603 0 : TELEMETER_APP_LOGIC;
604 :
605 0 : SHMIMMONITORT_UPDATE_INDI( gainFactShmimMonitorT );
606 0 : SHMIMMONITORT_UPDATE_INDI( multFactShmimMonitorT );
607 0 : SHMIMMONITORT_UPDATE_INDI( pcGainFactShmimMonitorT );
608 0 : SHMIMMONITORT_UPDATE_INDI( pcMultFactShmimMonitorT );
609 0 : SHMIMMONITORT_UPDATE_INDI( acoeffShmimMonitorT );
610 0 : SHMIMMONITORT_UPDATE_INDI( bcoeffShmimMonitorT );
611 0 : SHMIMMONITORT_UPDATE_INDI( modevalShmimMonitorT );
612 0 : FRAMEGRABBER_UPDATE_INDI;
613 :
614 0 : if( m_loop )
615 : {
616 0 : updateSwitchIfChanged( m_indiP_loop, "toggle", pcf::IndiElement::On, INDI_OK );
617 : }
618 : else
619 : {
620 0 : updateSwitchIfChanged( m_indiP_loop, "toggle", pcf::IndiElement::Off, INDI_IDLE );
621 : }
622 :
623 0 : if( m_pcOn )
624 : {
625 0 : updateSwitchIfChanged( m_indiP_pcOn, "toggle", pcf::IndiElement::On, INDI_OK );
626 : }
627 : else
628 : {
629 0 : updateSwitchIfChanged( m_indiP_pcOn, "toggle", pcf::IndiElement::Off, INDI_IDLE );
630 : }
631 :
632 0 : updatesIfChanged<float>( m_indiP_gain, { "current", "target" }, { m_gain, m_gain } );
633 0 : updatesIfChanged<float>( m_indiP_mult, { "current", "target" }, { m_mc, m_mc } );
634 :
635 0 : updatesIfChanged<float>( m_indiP_pcGain, { "current", "target" }, { m_pcGain, m_pcGain } );
636 0 : updatesIfChanged<float>( m_indiP_pcMult, { "current", "target" }, { m_pcMc, m_pcMc } );
637 :
638 0 : return 0;
639 : }
640 :
641 0 : int modalFilter::appShutdown()
642 : {
643 0 : SHMIMMONITORT_APP_SHUTDOWN( gainFactShmimMonitorT );
644 0 : SHMIMMONITORT_APP_SHUTDOWN( multFactShmimMonitorT );
645 0 : SHMIMMONITORT_APP_SHUTDOWN( pcGainFactShmimMonitorT );
646 0 : SHMIMMONITORT_APP_SHUTDOWN( pcMultFactShmimMonitorT );
647 0 : SHMIMMONITORT_APP_SHUTDOWN( acoeffShmimMonitorT );
648 0 : SHMIMMONITORT_APP_SHUTDOWN( bcoeffShmimMonitorT );
649 0 : SHMIMMONITORT_APP_SHUTDOWN( modevalShmimMonitorT );
650 0 : FRAMEGRABBER_APP_SHUTDOWN;
651 0 : TELEMETER_APP_SHUTDOWN;
652 :
653 0 : return 0;
654 : }
655 :
656 0 : int modalFilter::allocate( const gainFactShmimT &dummy )
657 : {
658 : static_cast<void>( dummy );
659 :
660 0 : if( gainFactShmimMonitorT::m_height != 1 )
661 : {
662 0 : return log<software_error, -1>( { __FILE__, __LINE__, "got gainFacts with height not 1" } );
663 : }
664 :
665 : // If there's a size change we have to lock
666 0 : if( gainFactShmimMonitorT::m_width != m_gainfacts.size() )
667 : {
668 0 : std::shared_lock<std::shared_mutex> lock( m_filtMutex );
669 :
670 0 : m_gainfacts.resize( gainFactShmimMonitorT::m_width );
671 :
672 0 : checkSizes();
673 0 : }
674 :
675 0 : return 0;
676 : }
677 :
678 0 : int modalFilter::processImage( void *curr_src, const gainFactShmimT &dummy )
679 : {
680 : static_cast<void>( dummy );
681 :
682 : // We just update without a lock. Size change handled in allocate.
683 0 : for( uint32_t n = 0; n < gainFactShmimMonitorT::m_width; ++n )
684 : {
685 0 : m_gainfacts[n] = reinterpret_cast<float *>( curr_src )[n];
686 : }
687 :
688 0 : return 0;
689 : }
690 :
691 0 : int modalFilter::allocate( const multFactShmimT &dummy )
692 : {
693 : static_cast<void>( dummy );
694 :
695 0 : if( multFactShmimMonitorT::m_height != 1 )
696 : {
697 0 : return log<software_error, -1>( { __FILE__, __LINE__, "got multFacts with height not 1" } );
698 : }
699 :
700 : // If there's a size change we have to lock
701 0 : if( multFactShmimMonitorT::m_width != m_multfacts.size() )
702 : {
703 0 : std::shared_lock<std::shared_mutex> lock( m_filtMutex );
704 :
705 0 : m_multfacts.resize( multFactShmimMonitorT::m_width );
706 :
707 0 : checkSizes();
708 0 : }
709 :
710 0 : return 0;
711 : }
712 :
713 0 : int modalFilter::processImage( void *curr_src, const multFactShmimT &dummy )
714 : {
715 : static_cast<void>( dummy );
716 :
717 : // We just update without a lock. Size change handled in allocate.
718 0 : for( uint32_t n = 0; n < multFactShmimMonitorT::m_width; ++n )
719 : {
720 0 : m_multfacts[n] = reinterpret_cast<float *>( curr_src )[n];
721 : }
722 :
723 0 : return 0;
724 : }
725 :
726 0 : int modalFilter::allocate( const pcGainFactShmimT &dummy )
727 : {
728 : static_cast<void>( dummy );
729 :
730 0 : if( pcGainFactShmimMonitorT::m_height != 1 )
731 : {
732 0 : return log<software_error, -1>( { __FILE__, __LINE__, "got pcGainFacts with height not 1" } );
733 : }
734 :
735 : // If there's a size change we have to lock
736 0 : if( pcGainFactShmimMonitorT::m_width != m_pcGainfacts.size() )
737 : {
738 0 : std::shared_lock<std::shared_mutex> lock( m_filtMutex );
739 :
740 0 : m_pcGainfacts.resize( pcGainFactShmimMonitorT::m_width );
741 :
742 0 : checkSizes();
743 0 : }
744 :
745 0 : return 0;
746 : }
747 :
748 0 : int modalFilter::processImage( void *curr_src, const pcGainFactShmimT &dummy )
749 : {
750 : static_cast<void>( dummy );
751 :
752 : // We just update without a lock. Size change handled in allocate.
753 0 : for( uint32_t n = 0; n < pcGainFactShmimMonitorT::m_width; ++n )
754 : {
755 0 : m_pcGainfacts[n] = reinterpret_cast<float *>( curr_src )[n];
756 : }
757 :
758 0 : return 0;
759 : }
760 :
761 0 : int modalFilter::allocate( const pcMultFactShmimT &dummy )
762 : {
763 : static_cast<void>( dummy );
764 :
765 0 : if( pcMultFactShmimMonitorT::m_height != 1 )
766 : {
767 0 : return log<software_error, -1>( { __FILE__, __LINE__, "got pcMultFacts with height not 1" } );
768 : }
769 :
770 : // If there's a size change we have to lock
771 0 : if( pcMultFactShmimMonitorT::m_width != m_pcMultfacts.size() )
772 : {
773 0 : std::shared_lock<std::shared_mutex> lock( m_filtMutex );
774 :
775 0 : m_pcMultfacts.resize( pcMultFactShmimMonitorT::m_width );
776 :
777 0 : checkSizes();
778 0 : }
779 :
780 0 : return 0;
781 : }
782 :
783 0 : int modalFilter::processImage( void *curr_src, const pcMultFactShmimT &dummy )
784 : {
785 : static_cast<void>( dummy );
786 :
787 : // We just update without a lock. Size change handled in allocate.
788 0 : for( uint32_t n = 0; n < pcMultFactShmimMonitorT::m_width; ++n )
789 : {
790 0 : m_pcMultfacts[n] = reinterpret_cast<float *>( curr_src )[n];
791 : }
792 :
793 0 : return 0;
794 : }
795 :
796 0 : int modalFilter::allocate( const acoeffShmimT &dummy )
797 : {
798 : static_cast<void>( dummy );
799 :
800 0 : uint32_t w = acoeffShmimMonitorT::m_width;
801 0 : uint32_t h = acoeffShmimMonitorT::m_height;
802 :
803 : // If there's a size change we lock
804 0 : if( w - 1 != m_as.rows() || h != m_as.cols() || h != m_Na.size() )
805 : {
806 0 : std::shared_lock<std::shared_mutex> lock( m_filtMutex );
807 :
808 0 : m_Na.resize( h );
809 0 : m_as.resize( w - 1, h );
810 :
811 0 : checkSizes();
812 0 : }
813 :
814 0 : return 0;
815 : }
816 :
817 0 : int modalFilter::processImage( void *curr_src, const acoeffShmimT &dummy )
818 : {
819 : static_cast<void>( dummy );
820 :
821 0 : uint32_t w = acoeffShmimMonitorT::m_width;
822 0 : uint32_t h = acoeffShmimMonitorT::m_height;
823 :
824 0 : eigenMap<float> ac( reinterpret_cast<float *>( curr_src ), w, h );
825 :
826 0 : for( uint32_t cc = 0; cc < h; ++cc )
827 : {
828 0 : m_Na[cc] = ac( 0, cc );
829 0 : for( uint32_t rr = 1; rr < w; ++rr )
830 : {
831 0 : m_as( rr - 1, cc ) = ac( rr, cc );
832 : }
833 : }
834 :
835 0 : std::cerr << "Got pc a-coeffs. Mode 0 has " << m_Na[0] << '\n';
836 :
837 0 : return 0;
838 : }
839 :
840 0 : int modalFilter::allocate( const bcoeffShmimT &dummy )
841 : {
842 : static_cast<void>( dummy );
843 :
844 0 : uint32_t w = bcoeffShmimMonitorT::m_width;
845 0 : uint32_t h = bcoeffShmimMonitorT::m_height;
846 :
847 : // If there's a size change we lock
848 0 : if( w - 1 != m_bs.rows() || h != m_bs.cols() || h != m_Nb.size() )
849 : {
850 0 : std::shared_lock<std::shared_mutex> lock( m_filtMutex );
851 :
852 0 : m_Nb.resize( h );
853 0 : m_bs.resize( w - 1, h );
854 :
855 0 : checkSizes();
856 0 : }
857 :
858 0 : std::cerr << "Got pc b-coeffs. Mode 0 has " << m_Nb[0] << '\n';
859 :
860 0 : return 0;
861 : }
862 :
863 0 : int modalFilter::processImage( void *curr_src, const bcoeffShmimT &dummy )
864 : {
865 : static_cast<void>( dummy );
866 :
867 0 : uint32_t w = bcoeffShmimMonitorT::m_width;
868 0 : uint32_t h = bcoeffShmimMonitorT::m_height;
869 :
870 0 : eigenMap<float> bc( reinterpret_cast<float *>( curr_src ), w, h );
871 :
872 0 : for( uint32_t cc = 0; cc < h; ++cc )
873 : {
874 0 : m_Nb[cc] = bc( 0, cc );
875 0 : for( uint32_t rr = 1; rr < w; ++rr )
876 : {
877 0 : m_bs( rr - 1, cc ) = bc( rr, cc );
878 : }
879 : }
880 0 : return 0;
881 : }
882 :
883 0 : int modalFilter::allocate( const modevalShmimT &dummy )
884 : {
885 : static_cast<void>( dummy );
886 :
887 0 : if( modevalShmimMonitorT::m_height != 1 )
888 : {
889 0 : return log<software_error, -1>( { __FILE__, __LINE__, "got modevals with height not 1" } );
890 : }
891 :
892 0 : if( modevalShmimMonitorT::m_width != m_modevalSz ) // This invalidates the whole c.b.
893 : {
894 0 : std::shared_lock<std::shared_mutex> lock( m_filtMutex );
895 :
896 0 : m_modevalSz = modevalShmimMonitorT::m_width;
897 0 : m_modevalWFS.maxEntries( m_modevalCBLength );
898 0 : m_modevalDM.maxEntries( m_modevalCBLength );
899 :
900 0 : checkSizes();
901 0 : }
902 :
903 0 : std::cerr << "Allocated modevals: " << modevalShmimMonitorT::m_width << " x " << modevalShmimMonitorT::m_height
904 0 : << '\n';
905 0 : return 0;
906 : }
907 :
908 0 : int modalFilter::processImage( void *curr_src, const modevalShmimT &dummy )
909 : {
910 : static_cast<void>( dummy );
911 :
912 0 : clock_gettime( CLOCK_ISIO, &m_atime );
913 :
914 0 : float *F = reinterpret_cast<float *>( curr_src );
915 :
916 0 : m_modevalWFS.nextEntry( std::vector<float>( F, F + m_modevalSz ) );
917 :
918 : // Now tell the writer to get going
919 0 : if( sem_post( &m_filtSem ) < 0 )
920 : {
921 0 : return log<software_critical, -1>( { __FILE__, __LINE__, errno, 0, "Error posting to filter semaphore" } );
922 : }
923 :
924 0 : return 0;
925 : }
926 :
927 0 : int modalFilter::configureAcquisition()
928 : {
929 :
930 0 : while( ( modevalShmimMonitorT::m_width < 1 || modevalShmimMonitorT::m_height != 1 ) && !m_shutdown )
931 : {
932 0 : sleep( 1 );
933 : }
934 :
935 0 : frameGrabberT::m_width = modevalShmimMonitorT::m_width;
936 0 : frameGrabberT::m_height = modevalShmimMonitorT::m_height;
937 0 : frameGrabberT::m_dataType = _DATATYPE_FLOAT;
938 :
939 0 : return 0;
940 : }
941 :
942 0 : float modalFilter::fps()
943 : {
944 0 : return m_fps;
945 : }
946 :
947 0 : int modalFilter::startAcquisition()
948 : {
949 0 : return 0;
950 : }
951 :
952 0 : int modalFilter::acquireAndCheckValid()
953 : {
954 : timespec ts;
955 :
956 0 : XWC_SEM_WAIT_TS( ts, 1, 0 );
957 :
958 0 : if( sem_timedwait( &m_filtSem, &ts ) != 0 )
959 : {
960 : /* Check for why we timed out */
961 : /* EINTER probably indicates time to shutdown, loop will exit if m_shutdown is set */
962 : /* ETIMEDOUT just means keep waiting */
963 0 : if( errno == EINTR || errno == ETIMEDOUT )
964 : {
965 0 : return 1;
966 : }
967 :
968 : /*Otherwise, report an error.*/
969 0 : return log<software_error, -1>( { __FILE__, __LINE__, errno, "sem_timedwait" } );
970 : }
971 : else
972 : {
973 0 : std::unique_lock<std::shared_mutex> lock( m_filtMutex ); // gotta be the only one
974 :
975 0 : if( !m_sizesMatch )
976 : {
977 0 : return 1;
978 : }
979 :
980 0 : m_modevalDM.nextEntry(); // move to the next entry without modifying what is there
981 0 : m_modevalDM[-1].resize( m_modevalSz ); // needed while growing, no-op once grown
982 0 : if( !m_loop )
983 : {
984 0 : for( uint32_t mode = 0; mode < m_modevalSz; ++mode )
985 : {
986 0 : m_modevalDM[-1][mode] = 0;
987 : }
988 0 : return 1; // we update the cbuff but we don't push to the DM
989 : }
990 : else
991 : {
992 0 : if( !m_pcOn )
993 : {
994 0 : for( uint32_t mode = 0; mode < m_modevalSz; ++mode )
995 : {
996 : // index [0] is 1the latest entry in the circular buffer
997 0 : m_modevalDM[-1][mode] = -1.0 * m_gain * m_gainfacts[mode] * m_modevalWFS[-1][mode] +
998 0 : m_mc * m_multfacts[mode] * m_modevalDM[-2][mode];
999 : }
1000 : }
1001 : else
1002 : {
1003 0 : for( uint32_t mode = 0; mode < m_modevalSz; ++mode )
1004 : {
1005 0 : float a = 0;
1006 0 : float b = 0;
1007 0 : for(int c = 0; c < m_Na[mode]; ++c)
1008 : {
1009 0 : a += m_as(c,mode)*m_modevalWFS[-1-c][mode];
1010 : }
1011 :
1012 0 : for(int c = 0; c < m_Nb[mode]; ++c)
1013 : {
1014 0 : b += m_bs(c,mode)*m_modevalDM[-2-c][mode];
1015 : }
1016 :
1017 0 : a = m_modevalWFS[-1][mode];
1018 0 : b = m_modevalDM[-2][mode];
1019 0 : m_modevalDM[-1][mode] = -1.0 * m_pcGain * m_pcGainfacts[mode] * a +
1020 0 : m_pcMc * m_pcMultfacts[mode] * b;
1021 : }
1022 : }
1023 :
1024 0 : frameGrabberT::m_currImageTimestamp = m_atime;
1025 : }
1026 0 : }
1027 :
1028 0 : return 0;
1029 : }
1030 :
1031 0 : int modalFilter::loadImageIntoStream( void *dest )
1032 : {
1033 :
1034 0 : memcpy( dest, m_modevalDM[-1].data(), m_modevalDM[-1].size() * sizeof( float ) );
1035 :
1036 0 : return 0;
1037 : }
1038 :
1039 0 : int modalFilter::reconfig()
1040 : {
1041 0 : return 0;
1042 : }
1043 :
1044 0 : void modalFilter::checkSizes()
1045 : {
1046 0 : m_sizesMatch = true;
1047 0 : m_pcSizesMatch = true;
1048 :
1049 0 : if( m_gainfacts.size() != m_modevalSz )
1050 : {
1051 0 : m_sizesMatch = false;
1052 : }
1053 :
1054 0 : if( m_multfacts.size() != m_modevalSz )
1055 : {
1056 0 : m_sizesMatch = false;
1057 : }
1058 :
1059 0 : if( static_cast<size_t>( m_as.cols() ) != m_modevalSz )
1060 : {
1061 0 : m_pcSizesMatch = false;
1062 : }
1063 :
1064 0 : if( static_cast<size_t>( m_bs.cols() ) != m_modevalSz )
1065 : {
1066 0 : m_pcSizesMatch = false;
1067 : }
1068 :
1069 0 : if( m_modevalWFS.size() != m_modevalCBLength )
1070 : {
1071 0 : m_pcSizesMatch = false;
1072 : }
1073 :
1074 0 : if( m_modevalDM.size() != m_modevalCBLength )
1075 : {
1076 0 : m_pcSizesMatch = false;
1077 : }
1078 0 : }
1079 :
1080 0 : int modalFilter::checkRecordTimes()
1081 : {
1082 0 : return telemeterT::checkRecordTimes( telem_fgtimings() );
1083 : }
1084 :
1085 0 : int modalFilter::recordTelem( const telem_fgtimings * )
1086 : {
1087 0 : return recordFGTimings( true );
1088 : }
1089 :
1090 0 : INDI_SETCALLBACK_DEFN( modalFilter, m_indiP_fpsSource )( const pcf::IndiProperty &ipRecv )
1091 : {
1092 0 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_fpsSource, ipRecv );
1093 :
1094 0 : if( ipRecv.find( m_fpsElement ) != true ) // this isn't valid
1095 : {
1096 0 : log<software_error>( { __FILE__, __LINE__, "No current property in fps source." } );
1097 0 : return 0;
1098 : }
1099 :
1100 0 : std::lock_guard<std::mutex> guard( m_indiMutex );
1101 :
1102 0 : float fps = ipRecv[m_fpsElement].get<float>();
1103 :
1104 0 : if( fabs( fps - m_fps ) > m_fpsTol )
1105 : {
1106 0 : m_fps = fps;
1107 0 : log<text_log>( "set fps to " + std::to_string( m_fps ), logPrio::LOG_NOTICE );
1108 0 : frameGrabberT::m_reconfig = true;
1109 : }
1110 :
1111 0 : return 0;
1112 :
1113 0 : } // INDI_SETCALLBACK_DEFN(modalFilter, m_indiP_fpsSource)
1114 :
1115 0 : INDI_NEWCALLBACK_DEFN( modalFilter, m_indiP_loop )( const pcf::IndiProperty &ipRecv )
1116 : {
1117 0 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_loop, ipRecv );
1118 :
1119 0 : if( ipRecv.find( "toggle" ) )
1120 : {
1121 0 : if( ipRecv["toggle"].getSwitchState() == pcf::IndiElement::On )
1122 : {
1123 0 : if( !m_loop )
1124 : {
1125 0 : log<loop_closed>();
1126 : }
1127 0 : m_loop = true;
1128 : }
1129 : else
1130 : {
1131 0 : if( m_loop )
1132 : {
1133 0 : log<loop_open>();
1134 : }
1135 0 : m_loop = false;
1136 : }
1137 : }
1138 :
1139 0 : return 0;
1140 : }
1141 :
1142 0 : INDI_NEWCALLBACK_DEFN( modalFilter, m_indiP_gain )( const pcf::IndiProperty &ipRecv )
1143 : {
1144 0 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_gain, ipRecv );
1145 :
1146 : float target;
1147 0 : if( indiTargetUpdate( m_indiP_gain, target, ipRecv, true ) < 0 )
1148 : {
1149 0 : log<software_error>( { __FILE__, __LINE__ } );
1150 0 : return -1;
1151 : }
1152 :
1153 0 : m_gain = target;
1154 :
1155 0 : std::cerr << "Got global gain: " << m_gain << '\n';
1156 :
1157 0 : return 0;
1158 : }
1159 :
1160 0 : INDI_NEWCALLBACK_DEFN( modalFilter, m_indiP_mult )( const pcf::IndiProperty &ipRecv )
1161 : {
1162 0 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_mult, ipRecv );
1163 :
1164 : float target;
1165 0 : if( indiTargetUpdate( m_indiP_mult, target, ipRecv, true ) < 0 )
1166 : {
1167 0 : log<software_error>( { __FILE__, __LINE__ } );
1168 0 : return -1;
1169 : }
1170 :
1171 0 : m_mc = target;
1172 :
1173 0 : std::cerr << "Got global mc: " << m_mc << '\n';
1174 :
1175 0 : return 0;
1176 : }
1177 :
1178 0 : INDI_NEWCALLBACK_DEFN( modalFilter, m_indiP_pcGain )( const pcf::IndiProperty &ipRecv )
1179 : {
1180 0 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_pcGain, ipRecv );
1181 :
1182 : float target;
1183 0 : if( indiTargetUpdate( m_indiP_pcGain, target, ipRecv, true ) < 0 )
1184 : {
1185 0 : log<software_error>( { __FILE__, __LINE__ } );
1186 0 : return -1;
1187 : }
1188 :
1189 0 : m_pcGain = target;
1190 :
1191 0 : std::cerr << "Got global pc gain: " << m_pcGain << '\n';
1192 0 : return 0;
1193 : }
1194 :
1195 0 : INDI_NEWCALLBACK_DEFN( modalFilter, m_indiP_pcMult )( const pcf::IndiProperty &ipRecv )
1196 : {
1197 0 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_pcMult, ipRecv );
1198 :
1199 : float target;
1200 0 : if( indiTargetUpdate( m_indiP_pcMult, target, ipRecv, true ) < 0 )
1201 : {
1202 0 : log<software_error>( { __FILE__, __LINE__ } );
1203 0 : return -1;
1204 : }
1205 :
1206 0 : m_pcMc = target;
1207 :
1208 0 : std::cerr << "Got global pc mc: " << m_mc << '\n';
1209 :
1210 0 : return 0;
1211 : }
1212 :
1213 0 : INDI_NEWCALLBACK_DEFN( modalFilter, m_indiP_pcOn )( const pcf::IndiProperty &ipRecv )
1214 : {
1215 0 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_pcOn, ipRecv );
1216 :
1217 0 : if( ipRecv.find( "toggle" ) )
1218 : {
1219 0 : if( ipRecv["toggle"].getSwitchState() == pcf::IndiElement::On )
1220 : {
1221 :
1222 0 : m_pcOn = true;
1223 : }
1224 : else
1225 : {
1226 0 : m_pcOn = false;
1227 : }
1228 : }
1229 :
1230 0 : return 0;
1231 : }
1232 :
1233 : } // namespace app
1234 : } // namespace MagAOX
1235 :
1236 : #endif // modalFilter_hpp
|
