Line data Source code
1 : /** \file modalPSDs.hpp
2 : * \brief The MagAO-X modalPSDs app header file
3 : *
4 : * \ingroup modalPSDs_files
5 : */
6 :
7 : #ifndef modalPSDs_hpp
8 : #define modalPSDs_hpp
9 :
10 : #include "../../libMagAOX/libMagAOX.hpp" //Note this is included on command line to trigger pch
11 : #include "../../magaox_git_version.h"
12 :
13 : #include <mx/sigproc/circularBuffer.hpp>
14 : #include <mx/sigproc/signalWindows.hpp>
15 :
16 : #include <mx/math/ft/fftwEnvironment.hpp>
17 : #include <mx/math/ft/fftT.hpp>
18 :
19 : /** \defgroup modalPSDs
20 : * \brief An application to calculate rolling PSDs of modal amplitudes
21 : *
22 : * <a href="../handbook/operating/software/apps/modalPSDs.html">Application Documentation</a>
23 : *
24 : * \ingroup apps
25 : *
26 : */
27 :
28 : /** \defgroup modalPSDs_files
29 : * \ingroup modalPSDs
30 : */
31 :
32 : namespace MagAOX
33 : {
34 : namespace app
35 : {
36 :
37 : /// Class for application to calculate rolling PSDs of modal amplitudes.
38 : /**
39 : * \ingroup modalPSDs
40 : */
41 : class modalPSDs : public MagAOXApp<true>, public dev::shmimMonitor<modalPSDs>
42 : {
43 : // Give the test harness access.
44 : friend class modalPSDs_test;
45 :
46 : friend class dev::shmimMonitor<modalPSDs>;
47 :
48 : public:
49 : typedef float realT;
50 :
51 : typedef std::complex<realT> complexT;
52 :
53 : typedef int32_t cbIndexT; ///< The index for the circular buffer
54 :
55 : /// The base shmimMonitor type
56 : typedef dev::shmimMonitor<modalPSDs> shmimMonitorT;
57 :
58 : /// The amplitude circular buffer type
59 : typedef mx::sigproc::circularBufferIndex<realT *, cbIndexT> ampCircBuffT;
60 :
61 : protected:
62 : /** \name Configurable Parameters
63 : *@{
64 : */
65 :
66 : int m_loopNumber{ 1 };
67 :
68 : std::string m_shmimBase; /**< The base name for the PSD shmims. If empty, the default,
69 : then aolN where N is the loop number is used*/
70 :
71 : std::string m_shmimTag{ "cl" }; /**< The tag to apply to the front of psds in the shmim name. Default is cl. */
72 :
73 : std::string m_fpsDevice; ///< Device name for getting fps to set circular buffer length.
74 : std::string m_fpsProperty{ "fps" }; ///< Property name for getting fps to set circular buffer length.
75 : std::string m_fpsElement{ "current" }; ///< Element name for getting fps to set circular buffer length.
76 :
77 : realT m_fpsTol{ 0 }; ///< The tolerance for detecting a change in FPS.
78 :
79 : realT m_psdTime{ 1 }; ///< The length of time over which to calculate PSDs. The default is 1 sec.
80 : realT m_psdAvgTime{ 10 }; ///< The time over which to average PSDs. The default is 10 sec.
81 :
82 : // realT m_overSize {10}; ///< Multiplicative factor by which to oversize the circular buffer, to give good mean
83 : // estimates and account for time-to-calculate.
84 :
85 : realT m_psdOverlapFraction{ 0.5 }; ///< The fraction of the sample time to overlap by.
86 :
87 : int m_nPSDHistory{ 100 }; //
88 :
89 : ///@}
90 :
91 : size_t m_nModes{ 0 }; ///< the number of modes to calculate PSDs for.
92 :
93 : ampCircBuffT m_ampCircBuff;
94 :
95 : // std::vector<ampCircBuffT> m_ampCircBuffs;
96 :
97 : realT m_fps{ 0 };
98 :
99 : realT m_df{ 1 };
100 :
101 : cbIndexT m_tsSize{ 2000 }; ///< The length of the time series sample over which the PSD is calculated
102 :
103 : cbIndexT m_tsOverlapSize{ 1000 }; ///< The number of samples in the overlap
104 :
105 : cbIndexT m_meanSize{ 20000 }; ///< The length of the time series over which to calculate the mean
106 :
107 : std::vector<realT> m_win; ///< The window function. By default this is Hann.
108 :
109 : realT *m_tsWork{ nullptr };
110 : size_t m_tsWorkSize{ 0 };
111 :
112 : std::complex<realT> *m_fftWork{ nullptr };
113 : size_t m_fftWorkSize{ 0 };
114 :
115 : std::vector<realT> m_psd;
116 :
117 : mx::math::ft::fftT<realT, std::complex<realT>, 1, 0> m_fft;
118 : mx::math::ft::fftwEnvironment<realT> m_fftEnv;
119 :
120 : /** \name PSD Calculation Thread
121 : * Handling of offloads from the average woofer shape
122 : * @{
123 : */
124 : int m_psdThreadPrio{ 0 }; ///< Priority of the PSD Calculation thread.
125 :
126 : std::string m_psdThreadCpuset; ///< The cpuset to use for the PSD Calculation thread.
127 :
128 : std::thread m_psdThread; ///< The PSD Calculation thread.
129 :
130 : bool m_psdThreadInit{ true }; ///< Initialization flag for the PSD Calculation thread.
131 :
132 : bool m_psdRestarting{ true }; /**< Synchronization flag. This will only become false
133 : after a successful call to allocate.*/
134 :
135 : bool m_psdWaiting{ false }; ///< Synchronization flag. This is set to true when the PSD thread is safely waiting
136 : ///< for allocation to complete.
137 :
138 : pid_t m_psdThreadID{ 0 }; ///< PSD Calculation thread PID.
139 :
140 : pcf::IndiProperty m_psdThreadProp; ///< The property to hold the PSD Calculation thread details.
141 :
142 : /// PS Calculation thread starter function
143 : static void psdThreadStart( modalPSDs *p /**< [in] pointer to this */ );
144 :
145 : /// PSD Calculation thread function
146 : /** Runs until m_shutdown is true.
147 : */
148 : void psdThreadExec();
149 :
150 : IMAGE *m_freqStream{ nullptr }; ///< The ImageStreamIO shared memory buffer to hold the frequency scale
151 :
152 : mx::improc::eigenImage<realT> m_psdBuffer;
153 :
154 : IMAGE *m_rawpsdStream{ nullptr }; ///< The ImageStreamIO shared memory buffer to hold the raw psds
155 :
156 : IMAGE *m_avgpsdStream{ nullptr }; ///< The ImageStreamIO shared memory buffer to hold the average psds
157 :
158 : public:
159 : /// Default c'tor.
160 : modalPSDs();
161 :
162 : /// D'tor, declared and defined for noexcept.
163 9 : ~modalPSDs() noexcept
164 9 : {
165 9 : }
166 :
167 : virtual void setupConfig();
168 :
169 : /// Implementation of loadConfig logic, separated for testing.
170 : /** This is called by loadConfig().
171 : */
172 : int loadConfigImpl( mx::app::appConfigurator &_config /**< [in] an application configuration
173 : from which to load values*/ );
174 :
175 : virtual void loadConfig();
176 :
177 : /// Startup function
178 : /**
179 : *
180 : */
181 : virtual int appStartup();
182 :
183 : /// Implementation of the FSM for modalPSDs.
184 : /**
185 : * \returns 0 on no critical error
186 : * \returns -1 on an error requiring shutdown
187 : */
188 : virtual int appLogic();
189 :
190 : /// Shutdown the app.
191 : /**
192 : *
193 : */
194 : virtual int appShutdown();
195 :
196 : // shmimMonitor Interface
197 : protected:
198 : int allocate( const dev::shmimT &dummy /**< [in] tag to differentiate shmimMonitor parents.*/ );
199 :
200 : int allocatePSDStreams();
201 :
202 : int processImage( void *curr_src, ///< [in] pointer to start of current frame.
203 : const dev::shmimT &dummy ///< [in] tag to differentiate shmimMonitor parents.
204 : );
205 :
206 : // INDI Interface
207 : protected:
208 : pcf::IndiProperty m_indiP_psdTime;
209 : pcf::IndiProperty m_indiP_psdAvgTime;
210 : pcf::IndiProperty m_indiP_overSize;
211 : pcf::IndiProperty m_indiP_fpsSource;
212 : pcf::IndiProperty m_indiP_fps;
213 :
214 : public:
215 0 : INDI_NEWCALLBACK_DECL( modalPSDs, m_indiP_psdTime );
216 0 : INDI_NEWCALLBACK_DECL( modalPSDs, m_indiP_psdAvgTime );
217 : INDI_NEWCALLBACK_DECL( modalPSDs, m_indiP_overSize );
218 0 : INDI_SETCALLBACK_DECL( modalPSDs, m_indiP_fpsSource );
219 : };
220 :
221 81 : modalPSDs::modalPSDs() : MagAOXApp( MAGAOX_CURRENT_SHA1, MAGAOX_REPO_MODIFIED )
222 : {
223 9 : return;
224 0 : }
225 :
226 0 : void modalPSDs::setupConfig()
227 : {
228 0 : SHMIMMONITOR_SETUP_CONFIG( config );
229 :
230 0 : config.add( "loop.number",
231 : "",
232 : "loop.number",
233 : argType::Required,
234 : "loop",
235 : "number",
236 : false,
237 : "string",
238 : "Loop number, as in aolN" );
239 :
240 0 : config.add( "psds.shmimBase",
241 : "",
242 : "psds.shmimBase",
243 : argType::Required,
244 : "psds",
245 : "shmimBase",
246 : false,
247 : "string",
248 : "The base name for the PSD shmims. If empty, the default, "
249 : "then aolN where N is the loop number is used" );
250 :
251 0 : config.add( "psds.shmimTag",
252 : "",
253 : "psds.shmimTag",
254 : argType::Required,
255 : "psds",
256 : "shmimTag",
257 : false,
258 : "string",
259 : "The tag to apply to the front of psds in the shmim name. Default is cl. " );
260 :
261 0 : config.add( "circBuff.fpsDevice",
262 : "",
263 : "circBuff.fpsDevice",
264 : argType::Required,
265 : "circBuff",
266 : "fpsDevice",
267 : false,
268 : "string",
269 : "Device name for getting fps to set circular buffer length." );
270 :
271 0 : config.add( "circBuff.fpsProperty",
272 : "",
273 : "circBuff.fpsProperty",
274 : argType::Required,
275 : "circBuff",
276 : "fpsProperty",
277 : false,
278 : "string",
279 : "Property name for getting fps to set circular buffer length. Default is 'fps'." );
280 :
281 0 : config.add( "circBuff.fpsElement",
282 : "",
283 : "circBuff.fpsElement",
284 : argType::Required,
285 : "circBuff",
286 : "fpsElement",
287 : false,
288 : "string",
289 : "Property name for getting fps to set circular buffer length. Default is 'current'." );
290 :
291 0 : config.add( "circBuff.fpsTol",
292 : "",
293 : "circBuff.fpsTol",
294 : argType::Required,
295 : "circBuff",
296 : "fpsTol",
297 : false,
298 : "float",
299 : "Tolerance for detecting a change in FPS. Default is 0." );
300 :
301 0 : config.add( "circBuff.defaultFPS",
302 : "",
303 : "circBuff.defaultFPS",
304 : argType::Required,
305 : "circBuff",
306 : "defaultFPS",
307 : false,
308 : "realT",
309 : "Default FPS at startup, will enable changing average length with psdTime before INDI available." );
310 :
311 0 : config.add( "circBuff.psdTime",
312 : "",
313 : "circBuff.psdTime",
314 : argType::Required,
315 : "circBuff",
316 : "psdTime",
317 : false,
318 : "realT",
319 : "The length of time over which to calculate PSDs. The default is 1 sec." );
320 : }
321 :
322 0 : int modalPSDs::loadConfigImpl( mx::app::appConfigurator &_config )
323 : {
324 0 : SHMIMMONITOR_LOAD_CONFIG( _config );
325 :
326 0 : _config( m_loopNumber, "loop.number" );
327 :
328 0 : m_shmimBase = "aol" + std::to_string( m_loopNumber );
329 0 : _config( m_shmimBase, "psds.shmimBase" );
330 :
331 0 : _config( m_shmimTag, "psds.shmimTag" );
332 :
333 0 : _config( m_fpsDevice, "circBuff.fpsDevice" );
334 0 : _config( m_fpsProperty, "circBuff.fpsProperty" );
335 0 : _config( m_fpsElement, "circBuff.fpsElement" );
336 0 : _config( m_fpsTol, "circBuff.fpsTol" );
337 :
338 0 : _config( m_psdTime, "circBuff.psdTime" );
339 :
340 0 : return 0;
341 : }
342 :
343 0 : void modalPSDs::loadConfig()
344 : {
345 0 : loadConfigImpl( config );
346 0 : }
347 :
348 0 : int modalPSDs::appStartup()
349 : {
350 0 : CREATE_REG_INDI_NEW_NUMBERF( m_indiP_psdTime, "psdTime", 0, 60, 0.1, "%0.1f", "PSD time", "PSD Setup" );
351 0 : m_indiP_psdTime["current"].set( m_psdTime );
352 0 : m_indiP_psdTime["target"].set( m_psdTime );
353 :
354 0 : CREATE_REG_INDI_NEW_NUMBERU( m_indiP_psdAvgTime, "psdAvgTime", 0, 60, 0.1, "%0.1f", "PSD Avg. Time", "PSD Setup" );
355 0 : m_indiP_psdAvgTime["current"].set( m_psdAvgTime );
356 0 : m_indiP_psdAvgTime["target"].set( m_psdAvgTime );
357 :
358 0 : if( m_fpsDevice == "" )
359 : {
360 0 : return log<software_critical, -1>(
361 0 : { __FILE__, __LINE__, "FPS source is not configurated (circBuff.fpsDevice)" } );
362 : }
363 :
364 0 : REG_INDI_SETPROP( m_indiP_fpsSource, m_fpsDevice, m_fpsProperty );
365 :
366 0 : CREATE_REG_INDI_RO_NUMBER( m_indiP_fps, "fps", "current", "Circular Buffer" );
367 0 : m_indiP_fps.add( pcf::IndiElement( "current" ) );
368 0 : m_indiP_fps["current"] = m_fps;
369 :
370 0 : SHMIMMONITOR_APP_STARTUP;
371 :
372 0 : XWCAPP_THREAD_START( m_psdThread,
373 : m_psdThreadInit,
374 : m_psdThreadID,
375 : m_psdThreadProp,
376 : m_psdThreadPrio,
377 : m_psdThreadCpuset,
378 : "psdcalc",
379 : psdThreadStart );
380 :
381 0 : state( stateCodes::OPERATING );
382 :
383 0 : return 0;
384 : }
385 :
386 0 : int modalPSDs::appLogic()
387 : {
388 0 : SHMIMMONITOR_APP_LOGIC;
389 :
390 0 : XWCAPP_THREAD_CHECK( m_psdThread, "psdcalc" );
391 :
392 0 : std::unique_lock<std::mutex> lock( m_indiMutex );
393 :
394 0 : SHMIMMONITOR_UPDATE_INDI;
395 :
396 0 : return 0;
397 0 : }
398 :
399 0 : int modalPSDs::appShutdown()
400 : {
401 :
402 0 : XWCAPP_THREAD_STOP( m_psdThread );
403 :
404 0 : SHMIMMONITOR_APP_SHUTDOWN;
405 :
406 0 : if( m_rawpsdStream )
407 : {
408 0 : ImageStreamIO_destroyIm( m_rawpsdStream );
409 0 : free( m_rawpsdStream );
410 : }
411 :
412 0 : if( m_avgpsdStream )
413 : {
414 0 : ImageStreamIO_destroyIm( m_avgpsdStream );
415 0 : free( m_avgpsdStream );
416 : }
417 :
418 0 : if( m_freqStream )
419 : {
420 0 : ImageStreamIO_destroyIm( m_freqStream );
421 0 : free( m_freqStream );
422 : }
423 :
424 0 : if( m_tsWork )
425 : {
426 0 : fftw_free( m_tsWork );
427 : }
428 :
429 0 : if( m_fftWork )
430 : {
431 0 : fftw_free( m_fftWork );
432 : }
433 :
434 0 : return 0;
435 : }
436 :
437 0 : int modalPSDs::allocate( const dev::shmimT &dummy )
438 : {
439 : static_cast<void>( dummy );
440 :
441 0 : m_psdRestarting = true;
442 :
443 : // Wait for FPS to become not 0
444 : // We wait indefinitely, the other process just might not be alive
445 0 : bool logged = false;
446 0 : while( m_fps <= 0 && !shutdown() )
447 : {
448 0 : if( !logged ) // log every thirty seconds
449 : {
450 0 : log<text_log>( "waiting for FPS...", logPrio::LOG_NOTICE );
451 0 : logged = true;
452 : }
453 0 : mx::sys::sleep( 1 );
454 : }
455 :
456 0 : if( m_psdWaiting == false )
457 : {
458 0 : std::cerr << "PSD not waiting . . .\n";
459 : }
460 : // Prevent reallocation while the psd thread might be calculating
461 0 : while( m_psdWaiting == false && !shutdown() )
462 : {
463 0 : mx::sys::microSleep( 100 );
464 : }
465 :
466 0 : std::cerr << "PSD waiting \n";
467 :
468 0 : if( shutdown() )
469 : {
470 0 : return 0; // If shutdown() is true then shmimMonitor will cleanup
471 : }
472 :
473 : // Check for unsupported type (must be realT)
474 0 : if( shmimMonitorT::m_dataType != IMAGESTRUCT_FLOAT )
475 : {
476 : // must be a vector of size 1 on one axis
477 0 : log<software_error>( { __FILE__, __LINE__, "unsupported data type: must be realT" } );
478 0 : return -1;
479 : }
480 :
481 : // Check for unexpected format
482 0 : if( shmimMonitorT::m_width != 1 && shmimMonitorT::m_height != 1 )
483 : {
484 : // must be a vector of size 1 on one axis
485 0 : log<software_error>( { __FILE__, __LINE__, "unexpected shmim format" } );
486 0 : return -1;
487 : }
488 :
489 0 : std::cerr << "connected to " << shmimMonitorT::m_shmimName << " " << shmimMonitorT::m_width << " "
490 0 : << shmimMonitorT::m_height << " " << shmimMonitorT::m_depth << "\n";
491 :
492 0 : m_nModes = shmimMonitorT::m_width * shmimMonitorT::m_height;
493 :
494 0 : m_tsSize = m_fps * m_psdTime;
495 :
496 : // Adjust length if odd to ensure we get the Nyquist frequency
497 0 : if( m_tsSize % 2 == 1 )
498 : {
499 0 : m_tsSize += 1;
500 : }
501 :
502 0 : m_tsOverlapSize = m_tsSize * m_psdOverlapFraction;
503 :
504 0 : if( m_tsOverlapSize == 0 || !std::isnormal( m_tsOverlapSize ) )
505 : {
506 0 : log<software_error>(
507 0 : { __FILE__, __LINE__, "bad m_tsOverlapSize value: " + std::to_string( m_tsOverlapSize ) } );
508 0 : return -1;
509 : }
510 :
511 0 : m_meanSize = m_fps * m_psdAvgTime;
512 :
513 0 : if( static_cast<uint32_t>( m_meanSize ) > shmimMonitorT::m_depth )
514 : {
515 0 : log<software_error>( { __FILE__, __LINE__, "input circ buff is not long enough for psd avg. time" } );
516 0 : m_meanSize = shmimMonitorT::m_depth;
517 : }
518 :
519 : // Size the circ buff
520 : // we really want 2*m_meanSize but might not be able to
521 0 : if( 2 * static_cast<uint32_t>( m_meanSize ) > shmimMonitorT::m_depth )
522 : {
523 0 : m_ampCircBuff.maxEntries( shmimMonitorT::m_depth );
524 : }
525 : else
526 : {
527 0 : m_ampCircBuff.maxEntries( 2 * m_meanSize );
528 : }
529 :
530 : // Create the window
531 0 : m_win.resize( m_tsSize );
532 0 : mx::sigproc::window::hann( m_win );
533 :
534 : // Set up the FFT and working memory
535 0 : m_fft.plan( m_tsSize, mx::math::ft::dir::forward, false );
536 :
537 0 : if( m_tsWork )
538 : {
539 0 : fftw_free( m_tsWork );
540 : }
541 0 : m_tsWork = mx::math::ft::fftw_malloc<realT>( m_tsSize );
542 :
543 0 : if( m_fftWork )
544 : {
545 0 : fftw_free( m_fftWork );
546 : }
547 :
548 0 : m_fftWork = mx::math::ft::fftw_malloc<std::complex<realT>>( ( m_tsSize / 2 + 1 ) );
549 :
550 0 : m_psd.resize( m_tsSize / 2 + 1 );
551 :
552 0 : m_df = 1.0 / ( m_tsSize / m_fps );
553 :
554 : // Create the shared memory images
555 : uint32_t imsize[3];
556 :
557 : // First the frequency
558 0 : imsize[0] = 1;
559 0 : imsize[1] = m_psd.size();
560 0 : imsize[2] = 1;
561 :
562 0 : if( m_freqStream )
563 : {
564 0 : ImageStreamIO_destroyIm( m_freqStream );
565 0 : free( m_freqStream );
566 : }
567 0 : m_freqStream = static_cast<IMAGE *>( malloc( sizeof( IMAGE ) ) );
568 :
569 0 : ImageStreamIO_createIm_gpu( m_freqStream,
570 0 : ( m_shmimBase + "_freq" ).c_str(),
571 : 3,
572 : imsize,
573 : IMAGESTRUCT_FLOAT,
574 : -1,
575 : 1,
576 : IMAGE_NB_SEMAPHORE,
577 : 0,
578 : CIRCULAR_BUFFER | ZAXIS_TEMPORAL,
579 : 0 );
580 0 : m_freqStream->md->write = 1;
581 0 : for( size_t n = 0; n < m_psd.size(); ++n )
582 : {
583 0 : m_freqStream->array.F[n] = n * m_df;
584 : }
585 :
586 : // Set the time of last write
587 0 : clock_gettime( CLOCK_REALTIME, &m_freqStream->md->writetime );
588 0 : m_freqStream->md->atime = m_freqStream->md->writetime;
589 :
590 : // Update cnt1
591 0 : m_freqStream->md->cnt1 = 0;
592 :
593 : // Update cnt0
594 0 : m_freqStream->md->cnt0 = 0;
595 :
596 0 : m_freqStream->md->write = 0;
597 0 : ImageStreamIO_sempost( m_freqStream, -1 );
598 :
599 0 : allocatePSDStreams();
600 :
601 0 : std::cerr << "done restarting\n";
602 :
603 0 : m_psdRestarting = false;
604 :
605 0 : return 0;
606 : }
607 :
608 0 : int modalPSDs::allocatePSDStreams()
609 : {
610 0 : if( m_rawpsdStream )
611 : {
612 0 : ImageStreamIO_destroyIm( m_rawpsdStream );
613 0 : free( m_rawpsdStream );
614 : }
615 :
616 : uint32_t imsize[3];
617 0 : imsize[0] = m_psd.size();
618 0 : imsize[1] = m_nModes;
619 0 : imsize[2] = m_nPSDHistory;
620 :
621 0 : m_rawpsdStream = static_cast<IMAGE *>( malloc( sizeof( IMAGE ) ) );
622 :
623 0 : ImageStreamIO_createIm_gpu( m_rawpsdStream,
624 0 : ( m_shmimBase + "_raw_" + m_shmimTag + "psds" ).c_str(),
625 : 3,
626 : imsize,
627 : IMAGESTRUCT_FLOAT,
628 : -1,
629 : 1,
630 : IMAGE_NB_SEMAPHORE,
631 : 0,
632 : CIRCULAR_BUFFER | ZAXIS_TEMPORAL,
633 : 0 );
634 :
635 0 : if( m_avgpsdStream )
636 : {
637 0 : ImageStreamIO_destroyIm( m_avgpsdStream );
638 0 : free( m_avgpsdStream );
639 : }
640 :
641 0 : imsize[0] = m_psd.size();
642 0 : imsize[1] = m_nModes;
643 0 : imsize[2] = 1;
644 :
645 0 : m_avgpsdStream = static_cast<IMAGE *>( malloc( sizeof( IMAGE ) ) );
646 0 : ImageStreamIO_createIm_gpu( m_avgpsdStream,
647 0 : ( m_shmimBase + "_" + m_shmimTag + "psds" ).c_str(),
648 : 3,
649 : imsize,
650 : IMAGESTRUCT_FLOAT,
651 : -1,
652 : 1,
653 : IMAGE_NB_SEMAPHORE,
654 : 0,
655 : CIRCULAR_BUFFER | ZAXIS_TEMPORAL,
656 : 0 );
657 :
658 0 : m_psdBuffer.resize( m_psd.size(), m_nModes );
659 :
660 0 : return 0;
661 : }
662 :
663 0 : int modalPSDs::processImage( void *curr_src, const dev::shmimT &dummy )
664 : {
665 : static_cast<void>( dummy );
666 :
667 0 : float *f_src = static_cast<float *>( curr_src );
668 :
669 0 : m_ampCircBuff.nextEntry( f_src );
670 :
671 0 : return 0;
672 : }
673 :
674 0 : void modalPSDs::psdThreadStart( modalPSDs *p )
675 : {
676 0 : p->psdThreadExec();
677 0 : }
678 :
679 0 : void modalPSDs::psdThreadExec()
680 : {
681 0 : m_psdThreadID = syscall( SYS_gettid );
682 :
683 0 : while( m_psdThreadInit == true && shutdown() == 0 )
684 : {
685 0 : sleep( 1 );
686 : }
687 :
688 0 : while( shutdown() == 0 )
689 : {
690 0 : if( m_psdRestarting == true || m_ampCircBuff.maxEntries() == 0 )
691 : {
692 0 : m_psdWaiting = true;
693 : }
694 :
695 0 : while( ( m_psdRestarting == true || m_ampCircBuff.maxEntries() == 0 ) && !shutdown() )
696 : {
697 0 : mx::sys::microSleep( 100 );
698 : }
699 :
700 0 : if( shutdown() )
701 : {
702 0 : break;
703 : }
704 :
705 0 : m_psdWaiting = false;
706 :
707 0 : if( m_ampCircBuff.maxEntries() == 0 )
708 : {
709 0 : log<software_error>( { __FILE__, __LINE__, "amp circ buff has zero size" } );
710 0 : return;
711 : }
712 :
713 0 : std::cerr << "waiting to grow\n";
714 0 : while( m_ampCircBuff.size() < m_ampCircBuff.maxEntries() && m_psdRestarting == false && !shutdown() )
715 : {
716 : // shrinking sleep
717 0 : double stime = ( 1.0 * m_ampCircBuff.maxEntries() - 1.0 * m_ampCircBuff.size() ) / m_fps * 0.5 * 1e9;
718 0 : mx::sys::nanoSleep( stime );
719 : }
720 :
721 0 : std::cerr << "all grown. starting to calculate\n";
722 :
723 : ampCircBuffT::indexT ne0;
724 : ampCircBuffT::indexT mne0;
725 0 : ampCircBuffT::indexT ne1 = m_ampCircBuff.latest();
726 0 : if( ne1 >= m_tsSize )
727 : {
728 0 : ne1 -= m_tsSize;
729 : }
730 : else
731 : {
732 0 : ne1 = m_ampCircBuff.size() + ne1 - m_tsSize;
733 : }
734 : // std::cerr << __LINE__ << " " << ne1 << " " << m_tsSize << " " << m_ampCircBuff.size() << '\n';
735 :
736 0 : while( m_psdRestarting == false && !shutdown() )
737 : {
738 : // Used to check if we are getting too behind
739 0 : uint64_t mono0 = m_ampCircBuff.mono();
740 :
741 : // Calc PSDs here
742 0 : ne0 = ne1;
743 :
744 0 : if( ne0 >= m_meanSize )
745 : {
746 0 : mne0 = ne0 - m_meanSize;
747 : }
748 : else
749 : {
750 0 : mne0 = m_ampCircBuff.size() + ne0 - m_meanSize;
751 : }
752 :
753 : // std::cerr << "calculating: " << ne0 << " " << " " << m_tsSize << ' ' << m_ampCircBuff.size() << '\n';
754 : // double t0 = mx::sys::get_curr_time();
755 :
756 0 : for( size_t m = 0; m < m_nModes; ++m ) // Loop over each mode
757 : {
758 : // get mean going over avg time
759 0 : realT mn = 0;
760 0 : for( cbIndexT n = 0; n < m_meanSize; ++n )
761 : {
762 0 : mn += ( m_ampCircBuff.at( mne0, n ) )[m];
763 : }
764 0 : mn /= m_meanSize;
765 :
766 0 : double var = 0;
767 :
768 0 : for( cbIndexT n = 0; n < m_tsSize; ++n )
769 : {
770 0 : m_tsWork[n] = ( m_ampCircBuff.at( ne0, n )[m] - mn ); // load mean subtracted chunk
771 :
772 0 : var += pow( m_tsWork[n], 2 );
773 :
774 0 : m_tsWork[n] *= m_win[n];
775 : }
776 0 : var /= m_tsSize;
777 :
778 0 : m_fft( m_fftWork, m_tsWork );
779 :
780 0 : double nm = 0;
781 0 : for( size_t n = 0; n < m_psd.size(); ++n )
782 : {
783 0 : m_psd[n] = norm( m_fftWork[n] );
784 0 : nm += m_psd[n] * m_df;
785 : }
786 :
787 : // Put it in the buffer for uploading to shmim
788 0 : for( size_t n = 0; n < m_psd.size(); ++n )
789 : {
790 : // m_psd[n] *= ( var / nm );
791 0 : m_psdBuffer( n, m ) = m_psd[n] * ( var / nm );
792 : }
793 : }
794 :
795 : //------------------------- the raw psds ---------------------------
796 0 : m_rawpsdStream->md->write = 1;
797 :
798 : // Set the time of last write
799 0 : clock_gettime( CLOCK_REALTIME, &m_rawpsdStream->md->writetime );
800 0 : m_rawpsdStream->md->atime = m_rawpsdStream->md->writetime;
801 :
802 0 : uint64_t cnt1 = m_rawpsdStream->md->cnt1 + 1;
803 0 : if( cnt1 >= m_rawpsdStream->md->size[2] )
804 : {
805 0 : cnt1 = 0;
806 : }
807 :
808 : // Move to next pointer
809 0 : float *F = m_rawpsdStream->array.F + m_psdBuffer.rows() * m_psdBuffer.cols() * cnt1;
810 :
811 0 : memcpy( F, m_psdBuffer.data(), m_psdBuffer.rows() * m_psdBuffer.cols() * sizeof( float ) );
812 :
813 : // Update cnt1
814 0 : m_rawpsdStream->md->cnt1 = cnt1;
815 :
816 : // Update cnt0
817 0 : ++m_rawpsdStream->md->cnt0;
818 :
819 0 : m_rawpsdStream->md->write = 0;
820 0 : ImageStreamIO_sempost( m_rawpsdStream, -1 );
821 :
822 : //-------------------------- now average the psds ----------------------------
823 :
824 0 : int nPSDAverage = ( m_psdAvgTime / m_psdTime ) / m_psdOverlapFraction;
825 :
826 0 : if( nPSDAverage <= 0 )
827 : {
828 0 : nPSDAverage = 1;
829 : }
830 0 : else if( static_cast<uint64_t>( nPSDAverage ) > m_rawpsdStream->md->size[2] )
831 : {
832 0 : nPSDAverage = m_rawpsdStream->md->size[2];
833 : }
834 :
835 0 : memcpy( m_psdBuffer.data(), F, m_psdBuffer.rows() * m_psdBuffer.cols() * sizeof( float ) );
836 :
837 0 : for( int n = 1; n < nPSDAverage; ++n )
838 : {
839 0 : if( cnt1 == 0 )
840 : {
841 0 : cnt1 = m_rawpsdStream->md->size[2] - 1;
842 : }
843 : else
844 : {
845 0 : --cnt1;
846 : }
847 :
848 0 : F = m_rawpsdStream->array.F + m_psdBuffer.rows() * m_psdBuffer.cols() * cnt1;
849 :
850 0 : m_psdBuffer += Eigen::Map<Eigen::Array<float, -1, -1>>( F, m_psdBuffer.rows(), m_psdBuffer.cols() );
851 : }
852 :
853 0 : m_psdBuffer /= nPSDAverage;
854 :
855 0 : m_avgpsdStream->md->write = 1;
856 :
857 : // Set the time of last write
858 0 : clock_gettime( CLOCK_REALTIME, &m_avgpsdStream->md->writetime );
859 0 : m_avgpsdStream->md->atime = m_avgpsdStream->md->writetime;
860 :
861 : // Move to next pointer
862 0 : F = m_avgpsdStream->array.F;
863 :
864 0 : memcpy( F, m_psdBuffer.data(), m_psdBuffer.rows() * m_psdBuffer.cols() * sizeof( float ) );
865 :
866 : // Update cnt1
867 0 : m_avgpsdStream->md->cnt1 = 0;
868 :
869 : // Update cnt0
870 0 : ++m_avgpsdStream->md->cnt0;
871 :
872 0 : m_avgpsdStream->md->write = 0;
873 0 : ImageStreamIO_sempost( m_avgpsdStream, -1 );
874 :
875 : // double t1 = mx::sys::get_curr_time();
876 : // std::cerr << "done " << t1 - t0 << "\n";
877 :
878 : // Have to be cycling within the overlap
879 0 : if( m_ampCircBuff.mono() - mono0 >= static_cast<uint32_t>( m_tsOverlapSize ) )
880 : {
881 0 : log<text_log>( "PSD calculations getting behind, skipping ahead.", logPrio::LOG_WARNING );
882 : }
883 : else
884 : {
885 0 : while( m_ampCircBuff.mono() - mono0 < static_cast<uint32_t>( m_tsOverlapSize ) && !m_psdRestarting )
886 : {
887 0 : mx::sys::microSleep( 0.2 * 1000000.0 / m_fps );
888 : }
889 : }
890 :
891 0 : if( m_psdRestarting )
892 : {
893 0 : continue;
894 : }
895 :
896 0 : ne1 = m_ampCircBuff.latest();
897 0 : if( ne1 > m_tsSize )
898 : {
899 0 : ne1 -= m_tsSize;
900 : }
901 : else
902 : {
903 0 : ne1 = m_ampCircBuff.size() + ne1 - m_tsSize;
904 : }
905 : }
906 : }
907 : }
908 :
909 3 : INDI_NEWCALLBACK_DEFN( modalPSDs, m_indiP_psdTime )( const pcf::IndiProperty &ipRecv )
910 : {
911 3 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_psdTime, ipRecv );
912 :
913 : realT target;
914 :
915 : if( indiTargetUpdate( m_indiP_psdTime, target, ipRecv, true ) < 0 )
916 : {
917 : log<software_error>( { __FILE__, __LINE__ } );
918 : return -1;
919 : }
920 :
921 : if( m_psdTime != target )
922 : {
923 : std::lock_guard<std::mutex> guard( m_indiMutex );
924 :
925 : m_psdTime = target;
926 :
927 : updateIfChanged( m_indiP_psdTime, "current", m_psdTime, INDI_IDLE );
928 : updateIfChanged( m_indiP_psdTime, "target", m_psdTime, INDI_IDLE );
929 :
930 : shmimMonitorT::m_restart = true;
931 :
932 : log<text_log>( "set psdTime to " + std::to_string( m_psdTime ), logPrio::LOG_NOTICE );
933 : }
934 :
935 : return 0;
936 : } // INDI_NEWCALLBACK_DEFN(modalPSDs, m_indiP_psdTime)
937 :
938 3 : INDI_NEWCALLBACK_DEFN( modalPSDs, m_indiP_psdAvgTime )( const pcf::IndiProperty &ipRecv )
939 : {
940 3 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_psdAvgTime, ipRecv );
941 :
942 : realT target;
943 :
944 : if( indiTargetUpdate( m_indiP_psdAvgTime, target, ipRecv, true ) < 0 )
945 : {
946 : log<software_error>( { __FILE__, __LINE__ } );
947 : return -1;
948 : }
949 :
950 : if( m_psdAvgTime != target )
951 : {
952 : std::lock_guard<std::mutex> guard( m_indiMutex );
953 :
954 : m_psdAvgTime = target;
955 :
956 : updateIfChanged( m_indiP_psdAvgTime, "current", m_psdAvgTime, INDI_IDLE );
957 : updateIfChanged( m_indiP_psdAvgTime, "target", m_psdAvgTime, INDI_IDLE );
958 :
959 : log<text_log>( "set psdAvgTime to " + std::to_string( m_psdAvgTime ), logPrio::LOG_NOTICE );
960 : }
961 :
962 : return 0;
963 : } // INDI_NEWCALLBACK_DEFN(modalPSDs, m_indiP_psdAvgTime)
964 :
965 3 : INDI_SETCALLBACK_DEFN( modalPSDs, m_indiP_fpsSource )( const pcf::IndiProperty &ipRecv )
966 : {
967 3 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_fpsSource, ipRecv );
968 :
969 : if( ipRecv.find( m_fpsElement ) != true ) // this isn't valid
970 : {
971 : log<software_error>( { __FILE__, __LINE__, "No current property in fps source." } );
972 : return 0;
973 : }
974 :
975 : std::lock_guard<std::mutex> guard( m_indiMutex );
976 :
977 : realT fps = ipRecv[m_fpsElement].get<realT>();
978 :
979 : if( fabs( fps - m_fps ) > m_fpsTol + .0001)
980 : {
981 : m_fps = fps;
982 : log<text_log>( "set fps to " + std::to_string( m_fps ), logPrio::LOG_NOTICE );
983 : updateIfChanged( m_indiP_fps, "current", m_fps, INDI_IDLE );
984 :
985 : shmimMonitorT::m_restart = true;
986 : }
987 :
988 : return 0;
989 :
990 : } // INDI_SETCALLBACK_DEFN(modalPSDs, m_indiP_fpsSource)
991 :
992 : } // namespace app
993 : } // namespace MagAOX
994 :
995 : #endif // modalPSDs_hpp
|
