Line data Source code
1 : /** \file alignLoop.hpp
2 : * \brief The MagAO-X XXXXXX header file
3 : *
4 : * \ingroup alignLoop_files
5 : */
6 :
7 : #ifndef alignLoop_hpp
8 : #define alignLoop_hpp
9 :
10 :
11 : #include "../../libMagAOX/libMagAOX.hpp" //Note this is included on command line to trigger pch
12 : #include "../../magaox_git_version.h"
13 :
14 : /** \defgroup alignLoop
15 : * \brief The XXXXXX application to do YYYYYYY
16 : *
17 : * <a href="../handbook/operating/software/apps/XXXXXX.html">Application Documentation</a>
18 : *
19 : * \ingroup apps
20 : *
21 : */
22 :
23 : /** \defgroup alignLoop_files
24 : * \ingroup alignLoop
25 : */
26 :
27 : namespace MagAOX
28 : {
29 : namespace app
30 : {
31 :
32 : /// The MagAO-X xxxxxxxx
33 : /**
34 : * \ingroup alignLoop
35 : */
36 : class alignLoop : public MagAOXApp<true>, public dev::shmimMonitor<alignLoop>
37 : {
38 : //Give the test harness access.
39 : friend class alignLoop_test;
40 :
41 : friend class dev::shmimMonitor<alignLoop>;
42 :
43 : public:
44 : //The base shmimMonitor type
45 : typedef dev::shmimMonitor<alignLoop> shmimMonitorT;
46 :
47 : protected:
48 :
49 : /** \name Configurable Parameters
50 : *@{
51 : */
52 :
53 : std::vector<std::string> m_ctrlDevices;
54 : std::vector<std::string> m_ctrlProperties;
55 : std::vector<std::string> m_ctrlCurrents;
56 : std::vector<std::string> m_ctrlTargets;
57 :
58 : std::vector<float> m_currents;
59 :
60 : std::vector<pcf::IndiProperty> m_indiP_ctrl;
61 :
62 : std::string m_intMatFile;
63 :
64 : std::vector<float> m_defaultGains;
65 :
66 : std::string m_upstreamDevice;
67 : std::string m_upstreamProperty {"loop_state"};
68 : bool m_upstreamFollowClosed {false};
69 :
70 :
71 : ///@}
72 :
73 : mx::improc::eigenImage<float> m_intMat;
74 :
75 : mx::improc::eigenImage<float> m_measurements;
76 : float m_delta0 {0};
77 : float m_delta1 {0};
78 :
79 : mx::improc::eigenImage<float> m_commands;
80 :
81 : float m_ggain {0};
82 :
83 : std::vector<float> m_gains;
84 :
85 : bool m_ctrlEnabled {false};
86 :
87 : public:
88 : /// Default c'tor.
89 : alignLoop();
90 :
91 : /// D'tor, declared and defined for noexcept.
92 0 : ~alignLoop() noexcept
93 0 : {}
94 :
95 : virtual void setupConfig();
96 :
97 : /// Implementation of loadConfig logic, separated for testing.
98 : /** This is called by loadConfig().
99 : */
100 : int loadConfigImpl( mx::app::appConfigurator & _config /**< [in] an application configuration from which to load values*/);
101 :
102 : virtual void loadConfig();
103 :
104 : /// Startup function
105 : /**
106 : *
107 : */
108 : virtual int appStartup();
109 :
110 : /// Implementation of the FSM for alignLoop.
111 : /**
112 : * \returns 0 on no critical error
113 : * \returns -1 on an error requiring shutdown
114 : */
115 : virtual int appLogic();
116 :
117 : /// Shutdown the app.
118 : /**
119 : *
120 : */
121 : virtual int appShutdown();
122 :
123 : int toggleLoop( bool onoff );
124 :
125 : // shmimMonitor interface:
126 : int allocate( const dev::shmimT &);
127 :
128 : int processImage( void* curr_src,
129 : const dev::shmimT &
130 : );
131 :
132 : int sendCommands(std::vector<float> & commands);
133 :
134 : //INDI
135 :
136 : pcf::IndiProperty m_indiP_deltas;
137 :
138 : pcf::IndiProperty m_indiP_ggain;
139 : pcf::IndiProperty m_indiP_ctrlEnabled;
140 :
141 0 : INDI_NEWCALLBACK_DECL(alignLoop, m_indiP_ggain);
142 0 : INDI_NEWCALLBACK_DECL(alignLoop, m_indiP_ctrlEnabled);
143 :
144 0 : static int st_setCallBack_ctrl( void * app, const pcf::IndiProperty &ipRecv)
145 : {
146 0 : return static_cast<alignLoop *>(app)->setCallBack_ctrl(ipRecv);
147 : }
148 :
149 : int setCallBack_ctrl(const pcf::IndiProperty &ipRecv);
150 :
151 : int m_upstreamState {0};
152 : pcf::IndiProperty m_indiP_upstream; ///< Property used to report the loop state
153 :
154 0 : INDI_SETCALLBACK_DECL(alignLoop, m_indiP_upstream);
155 : };
156 :
157 0 : alignLoop::alignLoop() : MagAOXApp(MAGAOX_CURRENT_SHA1, MAGAOX_REPO_MODIFIED)
158 : {
159 :
160 0 : return;
161 0 : }
162 :
163 0 : void alignLoop::setupConfig()
164 : {
165 0 : shmimMonitorT::setupConfig(config);
166 :
167 0 : config.add("ctrl.devices", "", "ctrl.devices", argType::Required, "ctrl", "devices", false, "string", "Device names of the controller(s) (one per element).");
168 0 : config.add("ctrl.properties", "", "ctrl.properties", argType::Required, "ctrl", "properties", false, "string", "Properties of the ctrl devices to which to give the commands. One per element");
169 0 : config.add("ctrl.currents", "", "ctrl.currents", argType::Required, "ctrl", "currents", false, "vector<string>", "current elements of the properties on which base the commands.");
170 0 : config.add("ctrl.targets", "", "ctrl.targets", argType::Required, "ctrl", "targets", false, "vector<string>", "target elements of the properties to which to send the commands.");
171 :
172 0 : config.add("loop.gain", "", "loop.gain", argType::Required, "loop", "gain", false, "float", "default global loop gain.");
173 0 : config.add("loop.intMat", "", "loop.intMat", argType::Required, "loop", "intMat", false, "string", "file name of the interaction matrix.");
174 0 : config.add("loop.gains", "", "loop.gains", argType::Required, "loop", "gains", false, "vector<float>", "default loop gains. If single number, it is applied to all axes.");
175 0 : config.add("loop.upstream", "", "loop.upstream", argType::Required, "loop", "upstream", false, "string", "Upstream loop device name. This loop will open, and optionally close, with the upstream loop. Default none.");
176 0 : config.add("loop.upstreamProperty", "", "loop.upstreamProperty", argType::Required, "loop", "upstreamProperty", false, "string", "Property of upstream loop device to follow. Must be a toggle. Default is loop_state.");
177 :
178 0 : }
179 :
180 0 : int alignLoop::loadConfigImpl( mx::app::appConfigurator & _config )
181 : {
182 0 : shmimMonitorT::loadConfig(_config);
183 :
184 0 : _config(m_ctrlDevices, "ctrl.devices");
185 0 : _config(m_ctrlProperties, "ctrl.properties");
186 0 : _config(m_ctrlCurrents, "ctrl.currents");
187 0 : _config(m_ctrlTargets, "ctrl.targets");
188 0 : _config(m_ggain, "loop.gain");
189 0 : _config(m_intMatFile, "loop.intMat");
190 0 : _config(m_defaultGains, "loop.gains");
191 0 : _config(m_upstreamDevice, "loop.upstream");
192 0 : _config(m_upstreamProperty, "loop.upstreamProperty");
193 0 : return 0;
194 : }
195 :
196 0 : void alignLoop::loadConfig()
197 : {
198 0 : loadConfigImpl(config);
199 0 : }
200 :
201 0 : int alignLoop::appStartup()
202 : {
203 0 : if(shmimMonitorT::appStartup() < 0)
204 : {
205 0 : return log<software_error,-1>({__FILE__, __LINE__});
206 : }
207 :
208 0 : if(m_ctrlTargets.size() != m_ctrlDevices.size())
209 : {
210 0 : return log<software_error, -1>({__FILE__, __LINE__, "ctrl.Targets and ctrl.devices are not the same size"});
211 : }
212 :
213 0 : if(m_ctrlTargets.size() != m_ctrlProperties.size())
214 : {
215 0 : return log<software_error, -1>({__FILE__, __LINE__, "ctrl.Targets and ctrl.properties are not the same size"});
216 : }
217 :
218 0 : if(m_ctrlTargets.size() != m_ctrlCurrents.size())
219 : {
220 0 : return log<software_error, -1>({__FILE__, __LINE__, "ctrl.Currents and ctrl.properties are not the same size"});
221 : }
222 :
223 0 : if(m_ctrlTargets.size() != m_defaultGains.size())
224 : {
225 0 : if(m_defaultGains.size()==1)
226 : {
227 0 : float g = m_defaultGains[0];
228 0 : m_defaultGains.resize(m_ctrlTargets.size(), g);
229 0 : log<text_log>("Setting loop.gains gains to be same size as ctrl.Targets", logPrio::LOG_NOTICE);
230 : }
231 : else
232 : {
233 0 : return log<software_error, -1>({__FILE__, __LINE__, "ctrl.Targets and loop.gains are not the same size"});
234 : }
235 : }
236 :
237 0 : CREATE_REG_INDI_RO_NUMBER( m_indiP_deltas, "deltas", "Deltas", "Deltas");
238 0 : m_indiP_deltas.add(pcf::IndiElement("delta0"));
239 0 : m_indiP_deltas["delta0"] = 0;
240 0 : m_indiP_deltas.add(pcf::IndiElement("delta1"));
241 0 : m_indiP_deltas["delta1"] = 0;
242 :
243 0 : m_gains.resize(m_defaultGains.size());
244 0 : for(size_t n=0; n < m_defaultGains.size(); ++n) m_gains[n] = m_defaultGains[n];
245 :
246 0 : createStandardIndiNumber<unsigned>( m_indiP_ggain, "loop_gain", 0, 1, 0, "%0.2f");
247 0 : m_indiP_ggain["current"] = m_ggain;
248 0 : m_indiP_ggain["target"] = m_ggain;
249 0 : if( registerIndiPropertyNew( m_indiP_ggain, INDI_NEWCALLBACK(m_indiP_ggain)) < 0)
250 : {
251 0 : log<software_error>({__FILE__,__LINE__});
252 0 : return -1;
253 : }
254 :
255 0 : createStandardIndiToggleSw( m_indiP_ctrlEnabled, "loop_state");
256 0 : if( registerIndiPropertyNew( m_indiP_ctrlEnabled, INDI_NEWCALLBACK(m_indiP_ctrlEnabled)) < 0)
257 : {
258 0 : log<software_error>({__FILE__,__LINE__});
259 0 : return -1;
260 : }
261 :
262 0 : m_currents.resize(m_ctrlDevices.size(), -1e15);
263 :
264 0 : m_indiP_ctrl.resize(m_ctrlDevices.size());
265 :
266 0 : for(size_t n=0; n< m_ctrlDevices.size();++n)
267 : {
268 0 : registerIndiPropertySet( m_indiP_ctrl[n], m_ctrlDevices[n], m_ctrlProperties[n], &st_setCallBack_ctrl);
269 : }
270 :
271 0 : m_commands.resize(m_ctrlTargets.size(), m_ctrlTargets.size()) ;
272 0 : m_commands.setZero();
273 :
274 0 : m_intMat.resize(m_ctrlTargets.size(), m_ctrlTargets.size()) ;
275 0 : m_intMat.setZero();
276 0 : m_intMat(1,0) = 0.926;
277 0 : m_intMat(1,1) = -0.370;
278 0 : m_intMat(0,0) = 0.185;
279 0 : m_intMat(0,1) = 0.926;
280 :
281 : /*
282 : std::string intMatPath = m_calibDir + "/" + m_intMatFile;
283 :
284 : mx::fits::fitsFile<float> ff;
285 : try
286 : {
287 : ff.read(m_intMat, intMatPath);
288 : }
289 : catch(...)
290 : {
291 : return log<software_error, -1>({__FILE__, __LINE__, "error reading loop.intMatFile. Does it exist?"});
292 : }
293 :
294 : if(m_intMat.rows() != m_ctrlTargets.size())
295 : {
296 : return log<software_error, -1>({__FILE__, __LINE__, "interaction matrix wrong size: rows do not match sensor Targets"});
297 : }
298 :
299 : if(m_intMat.cols() != m_correctorTargets.size())
300 : {
301 : return log<software_error, -1>({__FILE__, __LINE__, "interaction matrix wrong size: cols do not match corrector Targets"});
302 : }
303 : */
304 :
305 : //Get the loop state for managing offloading
306 0 : if(m_upstreamDevice != "")
307 : {
308 0 : REG_INDI_SETPROP(m_indiP_upstream, m_upstreamDevice, m_upstreamProperty);
309 : }
310 :
311 0 : return 0;
312 : }
313 :
314 0 : int alignLoop::appLogic()
315 : {
316 0 : if( shmimMonitorT::appLogic() < 0)
317 : {
318 0 : return log<software_error,-1>({__FILE__,__LINE__});
319 : }
320 :
321 0 : state(stateCodes::OPERATING);
322 :
323 0 : return 0;
324 : }
325 :
326 0 : int alignLoop::appShutdown()
327 : {
328 0 : shmimMonitorT::appShutdown();
329 :
330 0 : return 0;
331 : }
332 :
333 0 : int alignLoop::toggleLoop(bool onoff)
334 : {
335 0 : if(!m_ctrlEnabled && onoff) //not enabled so change
336 : {
337 0 : m_ctrlEnabled = true;
338 0 : log<loop_closed>();
339 0 : updateSwitchIfChanged(m_indiP_ctrlEnabled, "toggle", pcf::IndiElement::On, INDI_OK);
340 0 : return 0;
341 : }
342 :
343 0 : if(m_ctrlEnabled && !onoff)
344 : {
345 0 : m_ctrlEnabled = false;
346 0 : log<loop_open>();
347 0 : updateSwitchIfChanged(m_indiP_ctrlEnabled, "toggle", pcf::IndiElement::Off, INDI_IDLE);
348 :
349 0 : return 0;
350 : }
351 :
352 0 : return 0;
353 : }
354 :
355 : inline
356 0 : int alignLoop::allocate(const dev::shmimT & dummy)
357 : {
358 : static_cast<void>(dummy);
359 :
360 0 : std::lock_guard<std::mutex> guard(m_indiMutex);
361 :
362 0 : std::cerr << shmimMonitorT::m_width << shmimMonitorT::m_height << "\n";
363 0 : m_measurements.resize(shmimMonitorT::m_width, shmimMonitorT::m_height);
364 :
365 0 : return 0;
366 0 : }
367 :
368 : inline
369 0 : int alignLoop::processImage( void* curr_src,
370 : const dev::shmimT & dummy
371 : )
372 : {
373 : static_cast<void>(dummy);
374 :
375 0 : for(unsigned nn=0; nn < shmimMonitorT::m_width*shmimMonitorT::m_height; ++nn)
376 : {
377 0 : m_measurements.data()[nn] = (static_cast<float*>(curr_src)) [nn];
378 : }
379 :
380 :
381 0 : m_delta0 = m_measurements(0,0);
382 0 : m_delta1 = m_measurements(1,0);
383 :
384 0 : std::cout << "measurements: ";
385 0 : for(int cc = 0; cc < m_measurements.rows(); ++cc)
386 : {
387 0 : std::cout << m_measurements(cc,0) << " ";
388 : }
389 0 : std::cout << "\n";
390 :
391 0 : if(m_currents[0] < -1e14) return 0;
392 :
393 0 : m_commands.matrix() = m_intMat.matrix() * m_measurements.matrix();
394 :
395 :
396 :
397 0 : std::cout << "delta commands: ";
398 0 : for(int cc = 0; cc < m_measurements.rows(); ++cc)
399 : {
400 0 : std::cout << -m_commands(cc,0) << " ";
401 : }
402 0 : std::cout << "\n";
403 :
404 0 : std::vector<float> commands;
405 0 : commands.resize(m_measurements.rows());
406 :
407 0 : std::cout << "commands: ";
408 0 : for(int cc = 0; cc < m_measurements.rows(); ++cc)
409 : {
410 0 : commands[cc] = m_currents[cc] - m_ggain*m_gains[cc]*m_commands(cc,0);
411 0 : std::cout << commands[cc] << " ";
412 : }
413 0 : std::cout << "\n";
414 :
415 : //And send commands.
416 : int rv;
417 0 : if(m_ctrlEnabled)
418 : {
419 0 : rv = sendCommands(commands);
420 : }
421 : else
422 : {
423 0 : rv = 0;
424 : }
425 :
426 0 : updateIfChanged(m_indiP_deltas, std::vector<std::string>({"delta0", "delta1"}), std::vector<float>({m_delta0, m_delta1}));
427 0 : return rv;
428 0 : }
429 :
430 : inline
431 0 : int alignLoop::sendCommands(std::vector<float> & commands)
432 : {
433 0 : for(size_t n=0; n < m_ctrlDevices.size(); ++n)
434 : {
435 0 : pcf::IndiProperty ip(pcf::IndiProperty::Number);
436 :
437 0 : ip.setDevice(m_ctrlDevices[n]);
438 0 : ip.setName(m_ctrlProperties[n]);
439 0 : ip.add(pcf::IndiElement(m_ctrlTargets[n]));
440 0 : ip[m_ctrlTargets[n]] = commands[n];
441 :
442 0 : sendNewProperty(ip);
443 0 : }
444 :
445 0 : return 0;
446 : }
447 :
448 0 : INDI_NEWCALLBACK_DEFN(alignLoop, m_indiP_ggain)(const pcf::IndiProperty &ipRecv)
449 : {
450 0 : if(ipRecv.getName() != m_indiP_ggain.getName())
451 : {
452 0 : log<software_error>({__FILE__, __LINE__, "invalid indi property received"});
453 0 : return -1;
454 : }
455 :
456 : float target;
457 :
458 0 : if( indiTargetUpdate( m_indiP_ggain, target, ipRecv, true) < 0)
459 : {
460 0 : log<software_error>({__FILE__,__LINE__});
461 0 : return -1;
462 : }
463 :
464 0 : m_ggain = target;
465 :
466 0 : updateIfChanged(m_indiP_ggain, "current", m_ggain);
467 0 : updateIfChanged(m_indiP_ggain, "target", m_ggain);
468 :
469 0 : log<text_log>("set global gain to " + std::to_string(m_ggain), logPrio::LOG_NOTICE);
470 :
471 0 : return 0;
472 : }
473 :
474 0 : INDI_NEWCALLBACK_DEFN(alignLoop, m_indiP_ctrlEnabled)(const pcf::IndiProperty &ipRecv)
475 : {
476 0 : if(ipRecv.getName() != m_indiP_ctrlEnabled.getName())
477 : {
478 0 : log<software_error>({__FILE__, __LINE__, "invalid indi property received"});
479 0 : return -1;
480 : }
481 :
482 : //switch is toggled to on
483 0 : if( ipRecv["toggle"].getSwitchState() == pcf::IndiElement::On)
484 : {
485 0 : return toggleLoop(true);
486 : }
487 :
488 : //switch is toggle to off
489 0 : if( ipRecv["toggle"].getSwitchState() == pcf::IndiElement::Off)
490 : {
491 0 : return toggleLoop(false);
492 : }
493 :
494 0 : return 0;
495 : }
496 :
497 : inline
498 0 : int alignLoop::setCallBack_ctrl(const pcf::IndiProperty &ipRecv)
499 : {
500 0 : for(size_t n = 0; n < m_ctrlDevices.size(); ++n)
501 : {
502 0 : if( ipRecv.getDevice() == m_ctrlDevices[n])
503 : {
504 0 : if(ipRecv.getName() == m_ctrlProperties[n] && ipRecv.find(m_ctrlCurrents[n]))
505 : {
506 0 : m_currents[n] = ipRecv[m_ctrlCurrents[n]].get<float>();
507 : }
508 : }
509 : }
510 :
511 0 : return 0;
512 : }
513 :
514 0 : INDI_SETCALLBACK_DEFN(alignLoop, m_indiP_upstream)(const pcf::IndiProperty &ipRecv)
515 : {
516 0 : if(ipRecv.getName() != m_indiP_upstream.getName())
517 : {
518 0 : return log<software_error>({__FILE__,__LINE__, "wrong INDI property received"});
519 : }
520 :
521 0 : if(!ipRecv.find("toggle")) return 0;
522 :
523 0 : if(ipRecv["toggle"].getSwitchState() == pcf::IndiElement::On && m_upstreamFollowClosed)
524 : {
525 0 : std::cerr << "upstream on\n";
526 0 : return toggleLoop(true);
527 : }
528 0 : else if(ipRecv["toggle"].getSwitchState() == pcf::IndiElement::Off)
529 : {
530 0 : std::cerr << "upstream off\n";
531 0 : return toggleLoop(false);
532 : }
533 :
534 0 : return 0;
535 : }
536 :
537 :
538 : } //namespace app
539 : } //namespace MagAOX
540 :
541 : #endif //alignLoop_hpp
|
