Line data Source code
1 : /** \file t2wOffloader.hpp
2 : * \brief The MagAO-X tweeter to woofer offloading manager
3 : *
4 : * \ingroup app_files
5 : */
6 :
7 : #ifndef t2wOffloader_hpp
8 : #define t2wOffloader_hpp
9 :
10 : #include <limits>
11 :
12 : #include <mx/improc/eigenCube.hpp>
13 : #include <mx/improc/eigenImage.hpp>
14 : #include <mx/improc/milkImage.hpp>
15 : #include <mx/sigproc/gramSchmidt.hpp>
16 : #include <mx/math/templateBLAS.hpp>
17 :
18 : #include "../../libMagAOX/libMagAOX.hpp" //Note this is included on command line to trigger pch
19 : #include "../../magaox_git_version.h"
20 :
21 : namespace MagAOX
22 : {
23 : namespace app
24 : {
25 :
26 : /** \defgroup t2wOffloader Tweeter to Woofer Offloading
27 : * \brief Monitors the averaged tweeter shape, and sends it to the woofer.
28 : *
29 : * <a href="../handbook/operating/software/apps/t2wOffloader.html">Application Documentation</a>
30 : *
31 : * \ingroup apps
32 : *
33 : */
34 :
35 : /** \defgroup t2wOffloader_files Tweeter to Woofer Offloading
36 : * \ingroup t2wOffloader
37 : */
38 :
39 : /** MagAO-X application to control offloading the tweeter to the woofer.
40 : *
41 : * \ingroup t2wOffloader
42 : *
43 : */
44 : class t2wOffloader : public MagAOXApp<true>, public dev::shmimMonitor<t2wOffloader>, public dev::telemeter<t2wOffloader>
45 : {
46 :
47 : // Give the test harness access.
48 : friend class t2wOffloader_test;
49 :
50 : friend class dev::shmimMonitor<t2wOffloader>;
51 :
52 : // The base shmimMonitor type
53 : typedef dev::shmimMonitor<t2wOffloader> shmimMonitorT;
54 :
55 : friend class dev::telemeter<t2wOffloader>;
56 :
57 : typedef dev::telemeter<t2wOffloader> telemeterT;
58 :
59 : /// Floating point type in which to do all calculations.
60 : typedef float realT;
61 :
62 : protected:
63 : /** \name Configurable Parameters
64 : *@{
65 : */
66 :
67 : std::string m_twRespMPath;
68 :
69 : std::string m_dmChannel;
70 :
71 : std::string m_fpsSource{ "camwfs" };
72 : std::string m_navgSource{ "dmtweeter-avg" };
73 : float m_gain{ 0.1 };
74 : float m_leak{ 0.0 };
75 :
76 : float m_actLim{ 7.0 }; ///< the upper limit on woofer actuator commands. default is 7.0.
77 :
78 : std::string m_tweeterModeFile; ///< File containing the tweeter modes to use for offloading
79 : std::string m_tweeterMaskFile;
80 :
81 : std::string m_wooferMaskFile;
82 :
83 : uint32_t m_maxModes{ 50 };
84 :
85 : uint32_t m_numModes{ 0 };
86 :
87 : int m_loopNumber{ 0 };
88 : ///@}
89 :
90 : mx::improc::eigenImage<realT> m_twRespM;
91 : mx::improc::eigenImage<realT> m_tweeter;
92 : mx::improc::eigenImage<realT> m_woofer;
93 : mx::improc::eigenImage<realT> m_wooferDelta;
94 : mx::improc::eigenImage<realT> m_modeDeltaAmps;
95 :
96 : mx::improc::milkImage<realT> m_modevalDM;
97 : mx::improc::milkImage<realT> m_modevalDMf;
98 :
99 : uint64_t m_lastDMf_cnt0{ 0 };
100 : uint64_t m_updatedDMf {0};
101 :
102 : mx::improc::eigenImage<realT> m_tweeterMask;
103 :
104 : mx::improc::eigenCube<float> m_tModesOrtho;
105 :
106 : mx::improc::milkImage<float> m_wooferMask;
107 :
108 : mx::improc::eigenCube<float> m_wModes;
109 :
110 : IMAGE *m_wModesStream{ nullptr };
111 :
112 : float m_fps{ 0 }; ///< Current FPS from the FPS source.
113 : uint32_t m_navg{ 0 }; ///< Current navg from the averager
114 :
115 : float m_effFPS{ 0 };
116 :
117 : IMAGE m_dmStream;
118 : uint32_t m_dmWidth{ 0 }; ///< The width of the image
119 : uint32_t m_dmHeight{ 0 }; ///< The height of the image.
120 :
121 : uint8_t m_dmDataType{ 0 }; ///< The ImageStreamIO type code.
122 : size_t m_dmTypeSize{ 0 }; ///< The size of the type, in bytes.
123 :
124 : bool m_dmOpened{ false };
125 : bool m_dmRestart{ false };
126 :
127 : bool m_offloading{ false };
128 :
129 : /// Mutex for locking shared memory access.
130 : std::mutex m_shmimMutex;
131 :
132 : public:
133 : /// Default c'tor.
134 : t2wOffloader();
135 :
136 : /// D'tor, declared and defined for noexcept.
137 0 : ~t2wOffloader() noexcept
138 0 : {
139 0 : }
140 :
141 : virtual void setupConfig();
142 :
143 : /// Implementation of loadConfig logic, separated for testing.
144 : /** This is called by loadConfig().
145 : */
146 : int loadConfigImpl(
147 : mx::app::appConfigurator &_config /**< [in] an application configuration from which to load values*/ );
148 :
149 : virtual void loadConfig();
150 :
151 : /// Startup function
152 : /**
153 : *
154 : */
155 : virtual int appStartup();
156 :
157 : /// Implementation of the FSM for t2wOffloader.
158 : /**
159 : * \returns 0 on no critical error
160 : * \returns -1 on an error requiring shutdown
161 : */
162 : virtual int appLogic();
163 :
164 : /// Shutdown the app.
165 : /**
166 : *
167 : */
168 : virtual int appShutdown();
169 :
170 : /// Update the effective FPS after an navg or fps change
171 : int updateFPS();
172 :
173 : int allocate( const dev::shmimT &dummy /**< [in] tag to differentiate shmimMonitor parents.*/ );
174 :
175 : int processImage( void *curr_src, ///< [in] pointer to start of current frame.
176 : const dev::shmimT &dummy ///< [in] tag to differentiate shmimMonitor parents.
177 : );
178 :
179 : int zero();
180 :
181 : int prepareModes();
182 :
183 : protected:
184 : /** \name INDI Interface
185 : *
186 : * @{
187 : */
188 : pcf::IndiProperty m_indiP_gain;
189 : pcf::IndiProperty m_indiP_leak;
190 : pcf::IndiProperty m_indiP_actLim;
191 :
192 : pcf::IndiProperty m_indiP_zero;
193 :
194 : pcf::IndiProperty m_indiP_numModes;
195 :
196 : pcf::IndiProperty m_indiP_offloadToggle;
197 :
198 0 : INDI_NEWCALLBACK_DECL( t2wOffloader, m_indiP_gain );
199 0 : INDI_NEWCALLBACK_DECL( t2wOffloader, m_indiP_leak );
200 0 : INDI_NEWCALLBACK_DECL( t2wOffloader, m_indiP_actLim );
201 :
202 0 : INDI_NEWCALLBACK_DECL( t2wOffloader, m_indiP_zero );
203 :
204 0 : INDI_NEWCALLBACK_DECL( t2wOffloader, m_indiP_numModes );
205 :
206 0 : INDI_NEWCALLBACK_DECL( t2wOffloader, m_indiP_offloadToggle );
207 :
208 : pcf::IndiProperty m_indiP_fpsSource;
209 0 : INDI_SETCALLBACK_DECL( t2wOffloader, m_indiP_fpsSource );
210 :
211 : pcf::IndiProperty m_indiP_navgSource;
212 0 : INDI_SETCALLBACK_DECL( t2wOffloader, m_indiP_navgSource );
213 :
214 : pcf::IndiProperty m_indiP_fps;
215 :
216 : ///@}
217 :
218 : /** \name Telemeter Interface
219 : *
220 : * @{
221 : */
222 : int checkRecordTimes();
223 :
224 : int recordTelem( const telem_loopgain * );
225 :
226 : int recordLoopGain( bool force = false );
227 :
228 : int recordTelem( const telem_offloading * );
229 :
230 : int recordOffloading( bool force = false );
231 :
232 : ///@}
233 : };
234 :
235 : inline t2wOffloader::t2wOffloader() : MagAOXApp( MAGAOX_CURRENT_SHA1, MAGAOX_REPO_MODIFIED )
236 : {
237 : return;
238 : }
239 :
240 0 : inline void t2wOffloader::setupConfig()
241 : {
242 0 : SHMIMMONITOR_SETUP_CONFIG( config );
243 :
244 0 : TELEMETER_SETUP_CONFIG( config );
245 :
246 0 : config.add( "integrator.fpsSource",
247 : "",
248 : "integrator.fpsSource",
249 : argType::Required,
250 : "integrator",
251 : "fpsSource",
252 : false,
253 : "string",
254 : "Device name for getting fps of the loop. This device should have *.fps.current. Default is camwfs" );
255 :
256 0 : config.add( "integrator.navgSource",
257 : "",
258 : "integrator.navgSource",
259 : argType::Required,
260 : "integrator",
261 : "navgSource",
262 : false,
263 : "string",
264 : "Device name for getting navg of tweeter-ave. This device should have *.fps.current. Default is "
265 : "dmtweeter-avg." );
266 :
267 0 : config.add( "offload.respMPath",
268 : "",
269 : "offload.respMPath",
270 : argType::Required,
271 : "offload",
272 : "respMPath",
273 : false,
274 : "string",
275 : "The path to the response matrix." );
276 :
277 0 : config.add( "offload.channel",
278 : "",
279 : "offload.channel",
280 : argType::Required,
281 : "offload",
282 : "channel",
283 : false,
284 : "string",
285 : "The DM channel to offload to." );
286 :
287 0 : config.add( "offload.gain",
288 : "",
289 : "offload.gain",
290 : argType::Required,
291 : "offload",
292 : "gain",
293 : false,
294 : "float",
295 : "The starting offload gain. Default is 0.1." );
296 :
297 0 : config.add( "offload.leak",
298 : "",
299 : "offload.leak",
300 : argType::Required,
301 : "offload",
302 : "leak",
303 : false,
304 : "float",
305 : "The starting offload leak. Default is 0.0." );
306 :
307 0 : config.add( "offload.startupOffloading",
308 : "",
309 : "offload.startupOffloading",
310 : argType::Required,
311 : "offload",
312 : "startupOffloading",
313 : false,
314 : "bool",
315 : "Flag controlling whether offloading is on at startup. Default is false." );
316 :
317 0 : config.add( "offload.actLim",
318 : "",
319 : "offload.actLim",
320 : argType::Required,
321 : "offload",
322 : "actLim",
323 : false,
324 : "float",
325 : "The woofer actuator command limit. Default is 7.0." );
326 :
327 0 : config.add( "offload.tweeterModes",
328 : "",
329 : "offload.tweeterModes",
330 : argType::Required,
331 : "offload",
332 : "tweeterModes",
333 : false,
334 : "string",
335 : "File containing the tweeter modes to use for offloading" );
336 :
337 0 : config.add( "offload.tweeterMask",
338 : "",
339 : "offload.tweeterMask",
340 : argType::Required,
341 : "offload",
342 : "tweeterMask",
343 : false,
344 : "string",
345 : "File containing the tweeter mask." );
346 :
347 0 : config.add( "offload.wooferMask",
348 : "",
349 : "offload.wooferMask",
350 : argType::Required,
351 : "offload",
352 : "wooferMask",
353 : false,
354 : "string",
355 : "File containing the woofer mask." );
356 :
357 0 : config.add( "offload.maxModes",
358 : "",
359 : "offload.maxModes",
360 : argType::Required,
361 : "offload",
362 : "maxModes",
363 : false,
364 : "string",
365 : "Maximum number of modes for modal offloading." );
366 :
367 0 : config.add( "offload.numModes",
368 : "",
369 : "offload.numModes",
370 : argType::Required,
371 : "offload",
372 : "numModes",
373 : false,
374 : "string",
375 : "Number of modes to offload. 0 means use actuator offloading." );
376 :
377 0 : config.add( "offload.loopNumber",
378 : "",
379 : "offload.loopNUmber",
380 : argType::Required,
381 : "offload",
382 : "loopNumber",
383 : false,
384 : "string",
385 : "The aol loop number to use. Default is 0." );
386 : }
387 :
388 0 : inline int t2wOffloader::loadConfigImpl( mx::app::appConfigurator &_config )
389 : {
390 :
391 0 : SHMIMMONITOR_LOAD_CONFIG( _config );
392 :
393 0 : TELEMETER_LOAD_CONFIG( _config );
394 :
395 0 : _config( m_fpsSource, "integrator.fpsSource" );
396 0 : _config( m_navgSource, "integrator.navgSource" );
397 :
398 0 : _config( m_twRespMPath, "offload.respMPath" );
399 0 : _config( m_dmChannel, "offload.channel" );
400 0 : _config( m_gain, "offload.gain" );
401 0 : _config( m_leak, "offload.leak" );
402 0 : _config( m_actLim, "offload.actLim" );
403 0 : _config( m_tweeterModeFile, "offload.tweeterModes" );
404 0 : _config( m_tweeterMaskFile, "offload.tweeterMask" );
405 0 : _config( m_wooferMaskFile, "offload.wooferMask" );
406 0 : _config( m_maxModes, "offload.maxModes" );
407 0 : _config( m_numModes, "offload.numModes" );
408 :
409 0 : bool startupOffloading = false;
410 :
411 0 : if( _config.isSet( "offload.startupOffloading" ) )
412 : {
413 0 : _config( startupOffloading, "offload.startupOffloading" );
414 : }
415 0 : m_offloading = startupOffloading;
416 :
417 0 : return 0;
418 : }
419 :
420 0 : inline void t2wOffloader::loadConfig()
421 : {
422 0 : loadConfigImpl( config );
423 0 : }
424 :
425 0 : inline int t2wOffloader::appStartup()
426 : {
427 :
428 0 : createStandardIndiNumber<float>( m_indiP_gain, "gain", 0, 1, 0, "%0.2f" );
429 0 : m_indiP_gain["current"] = m_gain;
430 0 : m_indiP_gain["target"] = m_gain;
431 :
432 0 : if( registerIndiPropertyNew( m_indiP_gain, INDI_NEWCALLBACK( m_indiP_gain ) ) < 0 )
433 : {
434 0 : log<software_error>( { "" } );
435 0 : return -1;
436 : }
437 :
438 0 : createStandardIndiNumber<float>( m_indiP_leak, "leak", 0, 1, 0, "%0.2f" );
439 0 : m_indiP_leak["current"] = m_leak;
440 0 : m_indiP_leak["target"] = m_leak;
441 :
442 0 : if( registerIndiPropertyNew( m_indiP_leak, INDI_NEWCALLBACK( m_indiP_leak ) ) < 0 )
443 : {
444 0 : log<software_error>( { "" } );
445 0 : return -1;
446 : }
447 :
448 0 : createStandardIndiNumber<float>( m_indiP_actLim, "actLim", 0, 8, 0, "%0.2f" );
449 0 : m_indiP_actLim["current"] = m_actLim;
450 0 : m_indiP_actLim["target"] = m_actLim;
451 :
452 0 : if( registerIndiPropertyNew( m_indiP_actLim, INDI_NEWCALLBACK( m_indiP_actLim ) ) < 0 )
453 : {
454 0 : log<software_error>( { "" } );
455 0 : return -1;
456 : }
457 :
458 0 : if( prepareModes() < 0 )
459 : {
460 0 : log<software_error>( { "" } );
461 0 : return -1;
462 : }
463 :
464 0 : SHMIMMONITOR_APP_STARTUP;
465 :
466 0 : createStandardIndiRequestSw( m_indiP_zero, "zero", "zero loop" );
467 0 : if( registerIndiPropertyNew( m_indiP_zero, INDI_NEWCALLBACK( m_indiP_zero ) ) < 0 )
468 : {
469 0 : log<software_error>( { "" } );
470 0 : return -1;
471 : }
472 :
473 0 : createStandardIndiNumber<int>( m_indiP_numModes, "numModes", 0, 97, 0, "%d" );
474 0 : m_indiP_numModes["current"] = m_numModes;
475 0 : m_indiP_numModes["target"] = m_numModes;
476 :
477 0 : if( registerIndiPropertyNew( m_indiP_numModes, INDI_NEWCALLBACK( m_indiP_numModes ) ) < 0 )
478 : {
479 0 : log<software_error>( { "" } );
480 0 : return -1;
481 : }
482 :
483 0 : createStandardIndiToggleSw( m_indiP_offloadToggle, "offload" );
484 0 : if( registerIndiPropertyNew( m_indiP_offloadToggle, INDI_NEWCALLBACK( m_indiP_offloadToggle ) ) < 0 )
485 : {
486 0 : log<software_error>( { "" } );
487 0 : return -1;
488 : }
489 :
490 0 : REG_INDI_SETPROP( m_indiP_fpsSource, m_fpsSource, std::string( "fps" ) );
491 0 : REG_INDI_SETPROP( m_indiP_navgSource, m_navgSource, std::string( "nAverage" ) );
492 :
493 0 : createROIndiNumber( m_indiP_fps, "fps" );
494 0 : m_indiP_fps.add( pcf::IndiElement( "current" ) );
495 0 : if( registerIndiPropertyReadOnly( m_indiP_fps ) < 0 )
496 : {
497 0 : log<software_error>( { "" } );
498 0 : return -1;
499 : }
500 :
501 0 : TELEMETER_APP_STARTUP;
502 :
503 0 : state( stateCodes::OPERATING );
504 :
505 0 : return 0;
506 : }
507 :
508 0 : inline int t2wOffloader::appLogic()
509 : {
510 : static uint64_t lastUpdateDMf = 0;
511 :
512 0 : SHMIMMONITOR_APP_LOGIC;
513 :
514 0 : TELEMETER_APP_LOGIC;
515 :
516 0 : std::unique_lock<std::mutex> lock( m_indiMutex );
517 :
518 0 : SHMIMMONITOR_UPDATE_INDI;
519 :
520 0 : if(lastUpdateDMf != m_updatedDMf)
521 : {
522 0 : std::cerr << std::format("DMf updated {} times\n", m_updatedDMf - lastUpdateDMf);
523 0 : lastUpdateDMf = m_updatedDMf;
524 : }
525 :
526 0 : return 0;
527 0 : }
528 :
529 0 : inline int t2wOffloader::appShutdown()
530 : {
531 0 : SHMIMMONITOR_APP_SHUTDOWN;
532 :
533 0 : TELEMETER_APP_SHUTDOWN;
534 :
535 0 : return 0;
536 : }
537 :
538 0 : inline int t2wOffloader::updateFPS()
539 : {
540 : float effFPS;
541 :
542 0 : if( m_navg < 1 )
543 : {
544 0 : effFPS = 0;
545 : }
546 : else
547 0 : effFPS = m_fps / m_navg;
548 :
549 0 : if( effFPS != m_effFPS )
550 : {
551 0 : recordOffloading( true );
552 0 : m_effFPS = effFPS;
553 0 : recordOffloading();
554 : }
555 :
556 0 : updateIfChanged( m_indiP_fps, "current", m_effFPS );
557 :
558 0 : return 0;
559 : }
560 :
561 0 : inline int t2wOffloader::allocate( const dev::shmimT &dummy )
562 : {
563 : static_cast<void>( dummy ); // be unused
564 :
565 0 : m_tweeter.resize( shmimMonitorT::m_width, shmimMonitorT::m_height );
566 :
567 0 : if( m_dmOpened )
568 : {
569 0 : ImageStreamIO_closeIm( &m_dmStream );
570 : }
571 :
572 0 : m_dmOpened = false;
573 0 : m_dmRestart = false; // Set this up front, since we're about to restart.
574 :
575 0 : if( ImageStreamIO_openIm( &m_dmStream, m_dmChannel.c_str() ) == 0 )
576 : {
577 0 : if( m_dmStream.md[0].sem < 10 )
578 : {
579 0 : ImageStreamIO_closeIm( &m_dmStream );
580 : }
581 : else
582 : {
583 0 : m_dmOpened = true;
584 : }
585 : }
586 :
587 0 : if( !m_dmOpened )
588 : {
589 0 : log<software_error>( { m_dmChannel + " not opened." } );
590 0 : return -1;
591 : }
592 : else
593 : {
594 0 : m_dmWidth = m_dmStream.md->size[0];
595 0 : m_dmHeight = m_dmStream.md->size[1];
596 :
597 0 : m_dmDataType = m_dmStream.md->datatype;
598 0 : m_dmTypeSize = ImageStreamIO_typesize( m_dataType );
599 :
600 0 : log<text_log>( "Opened " + m_dmChannel + " " + std::to_string( m_dmWidth ) + " x " +
601 0 : std::to_string( m_dmHeight ) + " with data type: " + std::to_string( m_dmDataType ) );
602 :
603 0 : m_woofer.resize( m_dmWidth, m_dmHeight );
604 0 : m_woofer.setZero();
605 : }
606 :
607 0 : m_modeDeltaAmps.resize( 1, m_tModesOrtho.planes() );
608 :
609 0 : m_modevalDM.create( "aol0_modevalDM", m_tModesOrtho.planes(), 1 );
610 0 : m_modevalDMf.create( "aol0_modevalDMf", m_tModesOrtho.planes(), 1 );
611 :
612 0 : m_lastDMf_cnt0 = m_modevalDMf.cnt0();
613 :
614 : ///\todo size checks here.
615 :
616 0 : return 0;
617 : }
618 :
619 0 : inline int t2wOffloader::processImage( void *curr_src, const dev::shmimT &dummy )
620 : {
621 : static_cast<void>( dummy ); // be unused
622 :
623 0 : if( !m_offloading )
624 : {
625 0 : return 0;
626 : }
627 :
628 0 : if( m_numModes == 0 )
629 : {
630 : m_wooferDelta =
631 0 : m_twRespM.matrix() * Eigen::Map<Eigen::Matrix<float, -1, -1>>( (float *)curr_src, m_width * m_height, 1 );
632 :
633 0 : std::lock_guard<std::mutex> guard( m_shmimMutex );
634 :
635 0 : size_t n = 0;
636 0 : while( m_dmStream.md[0].write == 1 && n < 10000 ) // Check if zero() is running
637 : {
638 0 : ++n;
639 0 : mx::sys::microSleep( 1 );
640 : }
641 :
642 0 : if( m_dmStream.md[0].write == 1 || n > 10000 - 1 )
643 : {
644 0 : log<software_warning>( { "timed out with write==1" } );
645 0 : return 0;
646 : }
647 :
648 0 : m_woofer = m_gain * Eigen::Map<Eigen::Array<float, -1, -1>>( m_wooferDelta.data(), m_dmWidth, m_dmHeight ) +
649 0 : ( 1.0 - m_leak ) * m_woofer;
650 :
651 0 : for( int jj = 0; jj < m_woofer.cols(); ++jj )
652 : {
653 0 : for( int ii = 0; ii < m_woofer.rows(); ++ii )
654 : {
655 0 : float val = m_woofer( ii, jj );
656 0 : if( fabs( val ) > m_actLim )
657 : {
658 0 : if( val > 0 )
659 : {
660 0 : m_woofer( ii, jj ) = m_actLim;
661 : }
662 : else
663 : {
664 0 : m_woofer( ii, jj ) = -m_actLim;
665 : }
666 : }
667 : }
668 : }
669 :
670 0 : m_dmStream.md[0].write = 1;
671 :
672 0 : memcpy( m_dmStream.array.raw, m_woofer.data(), m_woofer.rows() * m_woofer.cols() * m_typeSize );
673 :
674 0 : m_dmStream.md[0].cnt0++;
675 :
676 0 : m_dmStream.md->write = 0;
677 0 : ImageStreamIO_sempost( &m_dmStream, -1 );
678 0 : }
679 : else // modal offloading
680 : {
681 0 : m_modeDeltaAmps = Eigen::Map<Eigen::Matrix<float, -1, -1>>( (float *)curr_src, 1, m_width * m_height ) *
682 0 : Eigen::Map<Eigen::Matrix<float, -1, -1>>( m_tModesOrtho.data(),
683 0 : m_tModesOrtho.rows() * m_tModesOrtho.cols(),
684 0 : m_tModesOrtho.planes() );
685 :
686 0 : m_modevalDM.setWrite( true );
687 :
688 0 : if( m_modevalDMf.cnt0() > m_lastDMf_cnt0 )
689 : {
690 0 : for( uint32_t p = 0; p < m_modevalDM.rows() && p < m_modevalDMf.rows(); ++p )
691 : {
692 0 : m_modevalDM( p, 0 ) = m_modevalDMf(p,0);
693 : }
694 :
695 0 : ++m_updatedDMf;
696 : }
697 :
698 0 : m_lastDMf_cnt0 = m_modevalDMf.cnt0();
699 :
700 0 : uint32_t p = 0;
701 0 : for( ; p < m_numModes && p < m_maxModes; ++p )
702 : {
703 0 : m_modevalDM( p, 0 ) = m_gain * m_modeDeltaAmps( 0, p ) + ( 1.0 - m_leak ) * m_modevalDM( p, 0 );
704 : }
705 0 : for( ; p < m_numModes; ++p )
706 : {
707 0 : m_modevalDM( p, 0 ) = 0;
708 : }
709 :
710 0 : m_modevalDM.post();
711 :
712 0 : std::lock_guard<std::mutex> guard( m_shmimMutex );
713 :
714 0 : size_t n = 0;
715 0 : while( m_dmStream.md[0].write == 1 && n < 10000 ) // Check if zero() is running
716 : {
717 0 : ++n;
718 0 : mx::sys::microSleep( 1 );
719 : }
720 :
721 0 : if( m_dmStream.md[0].write == 1 || n > 10000 - 1 )
722 : {
723 0 : log<software_warning>( { "timed out with write==1" } );
724 0 : return 0;
725 : }
726 :
727 0 : m_woofer = m_modevalDM( 0, 0 ) * m_wModes.image( 0 );
728 0 : for( uint32_t p = 1; p < m_numModes && p < m_maxModes; ++p )
729 : {
730 0 : m_woofer += m_modevalDM( p, 0 ) * m_wModes.image( p );
731 : }
732 :
733 0 : for( int jj = 0; jj < m_woofer.cols(); ++jj )
734 : {
735 0 : for( int ii = 0; ii < m_woofer.rows(); ++ii )
736 : {
737 0 : float val = m_woofer( ii, jj );
738 0 : if( fabs( val ) > m_actLim )
739 : {
740 0 : if( val > 0 )
741 : {
742 0 : m_woofer( ii, jj ) = m_actLim;
743 : }
744 : else
745 : {
746 0 : m_woofer( ii, jj ) = -m_actLim;
747 : }
748 : }
749 : }
750 : }
751 :
752 0 : m_dmStream.md[0].write = 1;
753 :
754 0 : memcpy( m_dmStream.array.raw, m_woofer.data(), m_woofer.rows() * m_woofer.cols() * m_typeSize );
755 :
756 0 : m_dmStream.md[0].cnt0++;
757 :
758 0 : m_dmStream.md->write = 0;
759 0 : ImageStreamIO_sempost( &m_dmStream, -1 );
760 0 : }
761 :
762 0 : return 0;
763 : }
764 :
765 0 : int t2wOffloader::zero()
766 : {
767 0 : std::lock_guard<std::mutex> guard( m_shmimMutex );
768 :
769 0 : size_t n = 0;
770 0 : while( m_dmStream.md[0].write == 1 && n < 10000 ) // Check if processImage() is running
771 : {
772 0 : ++n;
773 0 : mx::sys::microSleep( 1 );
774 : }
775 :
776 0 : if( m_dmStream.md[0].write == 1 || n > 10000 - 1 )
777 : {
778 0 : log<software_warning>( { "timed out with write==1, processImage() might be stuck" } );
779 0 : return 0;
780 : }
781 :
782 0 : m_dmStream.md[0].write = 1;
783 :
784 0 : m_modevalDM.setWrite( true );
785 0 : m_modevalDM().setZero();
786 0 : m_modevalDM.post();
787 :
788 0 : m_woofer.setZero();
789 :
790 0 : memcpy( m_dmStream.array.raw, m_woofer.data(), m_woofer.rows() * m_woofer.cols() * m_typeSize );
791 :
792 0 : m_dmStream.md[0].cnt0++;
793 :
794 0 : m_dmStream.md->write = 0;
795 0 : ImageStreamIO_sempost( &m_dmStream, -1 );
796 :
797 0 : log<text_log>( "zeroed", logPrio::LOG_NOTICE );
798 :
799 0 : return 0;
800 0 : }
801 :
802 0 : int t2wOffloader::prepareModes()
803 : {
804 0 : mx::improc::eigenCube<float> tmodes;
805 :
806 0 : mx::fits::fitsFile<float> ff;
807 :
808 0 : ff.read( tmodes, m_tweeterModeFile );
809 :
810 0 : ff.read( m_tweeterMask, m_tweeterMaskFile );
811 :
812 0 : ff.read( m_twRespM, m_twRespMPath );
813 :
814 0 : eigenImage<float> wm;
815 0 : ff.read( wm, m_wooferMaskFile );
816 0 : m_wooferMask.create( std::format( "aol{}_dmmask", m_loopNumber ), wm );
817 :
818 0 : for( int p = 0; p < tmodes.planes(); ++p )
819 : {
820 0 : tmodes.image( p ) *= m_tweeterMask;
821 0 : float norm = ( tmodes.image( p ) ).square().sum();
822 0 : tmodes.image( p ) /= sqrt( norm );
823 : }
824 :
825 0 : m_tModesOrtho.resize( tmodes.rows(), tmodes.cols(), m_maxModes );
826 :
827 0 : for( int p = 0; p < m_tModesOrtho.planes(); ++p )
828 : {
829 0 : m_tModesOrtho.image( p ) = tmodes.image( p );
830 : }
831 :
832 0 : ff.write( "/tmp/tModesOrtho.fits", m_tModesOrtho );
833 :
834 0 : m_wModes.resize( 11, 11, m_tModesOrtho.planes() );
835 0 : mx::improc::eigenImage<realT> win, wout;
836 :
837 : // win.resize( 11, 11 );
838 0 : wout.resize( 11, 11 );
839 :
840 : // Calculate the woofer modes corresponding to the tweeter modes
841 0 : for( int p = 0; p < m_tModesOrtho.planes(); ++p )
842 : {
843 0 : win = m_tModesOrtho.image( p );
844 :
845 0 : Eigen::Map<Eigen::Matrix<float, -1, -1>>( wout.data(), wout.rows() * wout.cols(), 1 ) =
846 0 : m_twRespM.matrix() * Eigen::Map<Eigen::Matrix<float, -1, -1>>( win.data(), win.rows() * win.cols(), 1 );
847 :
848 0 : m_wModes.image( p ) = wout * m_wooferMask();
849 : }
850 :
851 0 : ff.write( "/tmp/wModes.fits", m_wModes );
852 :
853 0 : m_wModesStream = new IMAGE;
854 :
855 : uint32_t imsize[3];
856 0 : imsize[0] = m_wModes.rows();
857 0 : imsize[1] = m_wModes.cols();
858 0 : imsize[2] = m_wModes.planes();
859 :
860 0 : std::string imname = std::format( "aol{}_CMmodesDM", m_loopNumber );
861 0 : errno_t rv = ImageStreamIO_createIm_gpu( m_wModesStream,
862 : imname.c_str(),
863 : 3,
864 : imsize,
865 : _DATATYPE_FLOAT,
866 : -1,
867 : 1,
868 : IMAGE_NB_SEMAPHORE,
869 : 0,
870 : CIRCULAR_BUFFER | ZAXIS_TEMPORAL,
871 : 0 );
872 :
873 0 : if( rv != IMAGESTREAMIO_SUCCESS )
874 : {
875 0 : delete m_wModesStream;
876 0 : m_wModesStream = nullptr;
877 0 : return log<software_error, -1>( "failed to create " + imname );
878 : }
879 :
880 0 : memcpy( m_wModesStream->array.raw,
881 0 : m_wModes.data(),
882 0 : m_wModes.rows() * m_wModes.cols() * m_wModes.planes() * sizeof( float ) );
883 :
884 0 : return 0;
885 0 : }
886 :
887 0 : INDI_NEWCALLBACK_DEFN( t2wOffloader, m_indiP_gain )( const pcf::IndiProperty &ipRecv )
888 : {
889 0 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_gain, ipRecv );
890 :
891 : float target;
892 :
893 0 : if( indiTargetUpdate( m_indiP_gain, target, ipRecv, true ) < 0 )
894 : {
895 0 : log<software_error>( { "" } );
896 0 : return -1;
897 : }
898 :
899 0 : recordLoopGain( true );
900 :
901 0 : m_gain = target;
902 :
903 0 : recordLoopGain();
904 :
905 0 : updateIfChanged( m_indiP_gain, "current", m_gain );
906 0 : updateIfChanged( m_indiP_gain, "target", m_gain );
907 :
908 0 : log<text_log>( "set gain to " + std::to_string( m_gain ), logPrio::LOG_NOTICE );
909 :
910 0 : return 0;
911 : }
912 :
913 0 : INDI_NEWCALLBACK_DEFN( t2wOffloader, m_indiP_leak )( const pcf::IndiProperty &ipRecv )
914 : {
915 0 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_leak, ipRecv );
916 :
917 : float target;
918 :
919 0 : if( indiTargetUpdate( m_indiP_leak, target, ipRecv, true ) < 0 )
920 : {
921 0 : log<software_error>( { "" } );
922 0 : return -1;
923 : }
924 :
925 0 : recordLoopGain( true );
926 0 : m_leak = target;
927 0 : recordLoopGain();
928 :
929 0 : updateIfChanged( m_indiP_leak, "current", m_leak );
930 0 : updateIfChanged( m_indiP_leak, "target", m_leak );
931 :
932 0 : log<text_log>( "set leak to " + std::to_string( m_leak ), logPrio::LOG_NOTICE );
933 :
934 0 : return 0;
935 : }
936 :
937 0 : INDI_NEWCALLBACK_DEFN( t2wOffloader, m_indiP_actLim )( const pcf::IndiProperty &ipRecv )
938 : {
939 0 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_actLim, ipRecv );
940 :
941 : float target;
942 :
943 0 : if( indiTargetUpdate( m_indiP_actLim, target, ipRecv, true ) < 0 )
944 : {
945 0 : log<software_error>( { "" } );
946 0 : return -1;
947 : }
948 :
949 0 : recordLoopGain( true );
950 :
951 0 : m_actLim = target;
952 :
953 0 : recordLoopGain();
954 :
955 0 : updateIfChanged( m_indiP_actLim, "current", m_actLim );
956 0 : updateIfChanged( m_indiP_actLim, "target", m_actLim );
957 :
958 0 : log<text_log>( "set actuator limit to " + std::to_string( m_actLim ), logPrio::LOG_NOTICE );
959 :
960 0 : return 0;
961 : }
962 :
963 0 : INDI_NEWCALLBACK_DEFN( t2wOffloader, m_indiP_zero )( const pcf::IndiProperty &ipRecv )
964 : {
965 0 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_zero, ipRecv );
966 :
967 0 : if( ipRecv["request"].getSwitchState() == pcf::IndiElement::On )
968 : {
969 0 : return zero();
970 : }
971 0 : return 0;
972 : }
973 :
974 0 : INDI_NEWCALLBACK_DEFN( t2wOffloader, m_indiP_numModes )( const pcf::IndiProperty &ipRecv )
975 : {
976 0 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_numModes, ipRecv );
977 :
978 : float target;
979 :
980 0 : if( indiTargetUpdate( m_indiP_numModes, target, ipRecv, true ) < 0 )
981 : {
982 0 : log<software_error>( { "" } );
983 0 : return -1;
984 : }
985 :
986 0 : recordOffloading( true );
987 0 : m_numModes = target;
988 0 : recordOffloading();
989 :
990 0 : if( m_numModes > m_maxModes )
991 : {
992 0 : log<text_log>( std::format( "maximum number of offloadings modes is {}", m_maxModes ), logPrio::LOG_WARNING );
993 0 : m_numModes = m_maxModes;
994 : }
995 :
996 0 : updateIfChanged( m_indiP_numModes, "current", m_numModes );
997 0 : updateIfChanged( m_indiP_numModes, "target", m_numModes );
998 :
999 0 : log<text_log>( "set number of modes to " + std::to_string( m_numModes ), logPrio::LOG_NOTICE );
1000 :
1001 0 : return 0;
1002 : }
1003 :
1004 0 : INDI_NEWCALLBACK_DEFN( t2wOffloader, m_indiP_offloadToggle )( const pcf::IndiProperty &ipRecv )
1005 : {
1006 0 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_offloadToggle, ipRecv );
1007 :
1008 : // switch is toggled to on
1009 0 : if( ipRecv["toggle"].getSwitchState() == pcf::IndiElement::On )
1010 : {
1011 0 : if( !m_offloading ) // not offloading so change
1012 : {
1013 0 : m_woofer.setZero(); // always zero when offloading starts
1014 0 : log<text_log>( "zeroed", logPrio::LOG_NOTICE );
1015 :
1016 0 : recordLoopGain( true );
1017 0 : m_offloading = true;
1018 0 : recordLoopGain();
1019 :
1020 0 : log<text_log>( "started offloading", logPrio::LOG_NOTICE );
1021 0 : updateSwitchIfChanged( m_indiP_offloadToggle, "toggle", pcf::IndiElement::On, INDI_OK );
1022 : }
1023 0 : return 0;
1024 : }
1025 :
1026 : // switch is toggle to off
1027 0 : if( ipRecv["toggle"].getSwitchState() == pcf::IndiElement::Off )
1028 : {
1029 0 : if( m_offloading ) // offloading so change it
1030 : {
1031 0 : recordLoopGain( true );
1032 0 : m_offloading = false;
1033 0 : recordLoopGain();
1034 :
1035 0 : log<text_log>( "stopped offloading", logPrio::LOG_NOTICE );
1036 0 : updateSwitchIfChanged( m_indiP_offloadToggle, "toggle", pcf::IndiElement::Off, INDI_IDLE );
1037 : }
1038 0 : return 0;
1039 : }
1040 :
1041 0 : return 0;
1042 : }
1043 :
1044 0 : INDI_SETCALLBACK_DEFN( t2wOffloader, m_indiP_fpsSource )( const pcf::IndiProperty &ipRecv )
1045 : {
1046 0 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_fpsSource, ipRecv );
1047 :
1048 0 : if( ipRecv.find( "current" ) != true ) // this isn't valie
1049 : {
1050 0 : return 0;
1051 : }
1052 :
1053 0 : std::lock_guard<std::mutex> guard( m_indiMutex );
1054 :
1055 0 : realT fps = ipRecv["current"].get<float>();
1056 :
1057 0 : if( fps != m_fps )
1058 : {
1059 0 : m_fps = fps;
1060 0 : updateFPS();
1061 : }
1062 :
1063 0 : return 0;
1064 0 : }
1065 :
1066 0 : INDI_SETCALLBACK_DEFN( t2wOffloader, m_indiP_navgSource )( const pcf::IndiProperty &ipRecv )
1067 : {
1068 0 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_navgSource, ipRecv );
1069 :
1070 0 : if( ipRecv.find( "current" ) != true ) // this isn't valie
1071 : {
1072 0 : return 0;
1073 : }
1074 :
1075 0 : std::lock_guard<std::mutex> guard( m_indiMutex );
1076 :
1077 0 : realT navg = ipRecv["current"].get<float>();
1078 :
1079 0 : if( navg != m_navg )
1080 : {
1081 0 : m_navg = navg;
1082 0 : updateFPS();
1083 : }
1084 :
1085 0 : return 0;
1086 0 : }
1087 :
1088 0 : int t2wOffloader::checkRecordTimes()
1089 : {
1090 0 : return telemeterT::checkRecordTimes( telem_loopgain(), telem_offloading() );
1091 : }
1092 :
1093 0 : int t2wOffloader::recordTelem( const telem_loopgain * )
1094 : {
1095 0 : return recordLoopGain( true );
1096 : }
1097 :
1098 0 : int t2wOffloader::recordLoopGain( bool force )
1099 : {
1100 : static uint8_t state{ 0 };
1101 : static float gain{ -1000 };
1102 : static float leak{ 0 };
1103 : static float limit{ 0 };
1104 :
1105 0 : if( state != m_offloading || gain != m_gain || leak != m_leak || limit != m_actLim || force )
1106 : {
1107 0 : state = m_offloading;
1108 0 : gain = m_gain;
1109 0 : leak = m_leak;
1110 0 : limit = m_actLim;
1111 :
1112 0 : telem<telem_loopgain>( { state, m_gain, 1 - leak, limit } );
1113 : }
1114 :
1115 0 : return 0;
1116 : }
1117 :
1118 0 : int t2wOffloader::recordTelem( const telem_offloading * )
1119 : {
1120 0 : return recordOffloading( true );
1121 : }
1122 :
1123 0 : int t2wOffloader::recordOffloading( bool force )
1124 : {
1125 : static uint32_t num_modes{ 0 };
1126 : static uint32_t num_average{ 0 };
1127 0 : float fps{ 0 };
1128 :
1129 0 : if( num_modes != m_numModes || num_average != m_navg || fps != m_effFPS || force )
1130 : {
1131 0 : num_modes = m_numModes;
1132 0 : num_average = m_navg;
1133 0 : fps = m_effFPS;
1134 :
1135 0 : telem<telem_offloading>( { num_modes, num_average, fps } );
1136 : }
1137 :
1138 0 : return 0;
1139 : }
1140 :
1141 : } // namespace app
1142 : } // namespace MagAOX
1143 :
1144 : #endif // t2wOffloader_hpp
|
