Line data Source code
1 : /** \file zaberCtrl.hpp
2 : * \brief The MagAO-X Zaber Stage Controller header file
3 : * \author Jared R. Males (jaredmales@gmail.com)
4 : *
5 : * \ingroup zaberCtrl_files
6 : */
7 :
8 : #ifndef zaberCtrl_hpp
9 : #define zaberCtrl_hpp
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 zaberCtrl
15 : * \brief The MagAO-X application to control a single Zaber stage
16 : *
17 : * <a href="../handbook/operating/software/apps/zaberCtrl.html">Application Documentation</a>
18 : *
19 : * \ingroup apps
20 : *
21 : */
22 :
23 : /** \defgroup zaberCtrl_files
24 : * \ingroup zaberCtrl
25 : */
26 :
27 : namespace MagAOX
28 : {
29 : namespace app
30 : {
31 :
32 : /// The MagAO-X Zaber Stage Controller
33 : /** Interacts with the stage through a zaberLowLevel process via INDI.
34 : *
35 : * \ingroup zaberCtrl
36 : */
37 : class zaberCtrl : public MagAOXApp<>, public dev::stdMotionStage<zaberCtrl>, public dev::telemeter<zaberCtrl>
38 : {
39 :
40 : // Give the test harness access.
41 : friend class zaberCtrl_test;
42 :
43 : friend class dev::stdMotionStage<zaberCtrl>;
44 :
45 : friend class dev::telemeter<zaberCtrl>;
46 :
47 : protected:
48 : /** \name Configurable Parameters
49 : *@{
50 : */
51 :
52 : std::string m_lowLevelName{ "zaberLowLevel" };
53 :
54 : std::string m_stageName; ///< The name of this stage used for accessing via zaberLowLevel
55 :
56 : double m_countsPerMillimeter{ 10078.740157480315 }; ///< Counts per millimeter calibration of this stage.
57 :
58 : unsigned long m_maxRawPos{ 0 }; ///< Maximum counts available on this stage.
59 :
60 : ///@}
61 :
62 : double m_rawPos{ 0 }; ///< Current raw position in counts
63 : double m_tgtRawPos{ 0 }; ///< Target raw position in counts
64 : double m_stageTemp{ 0 }; ///< Temperature reported by the stage
65 :
66 : double m_maxPos{ 0 }; ///< Maximum position in mm available on this stage
67 : double m_pos{ 0 }; ///< Current position in mm
68 : double m_tgtPos{ 0 }; ///< Target position in mm.
69 : bool m_parked{ false }; ///< Whether the low-level Zaber stage reports a parked state.
70 :
71 : bool m_wason = true; ///< Whether the stage was in state power on before power off
72 :
73 : int m_homingState{ 0 }; ///< The homing state, tracks the stages of homing.
74 :
75 : time_t m_lastHomed{ 0 }; ///< Time stamp of the last time the stage was homed
76 :
77 : public:
78 : /// Default c'tor.
79 : zaberCtrl();
80 :
81 : /// D'tor, declared and defined for noexcept.
82 45 : ~zaberCtrl() noexcept
83 45 : {
84 45 : }
85 :
86 : /// Setup the configuration of this stage controller.
87 : virtual void setupConfig();
88 :
89 : /// Implementation of loadConfig logic, separated for testing.
90 : /** This is called by loadConfig().
91 : */
92 : int loadConfigImpl(
93 : mx::app::appConfigurator &_config /**< [in] an application configuration from which to load values*/ );
94 :
95 : /// Load the stage configuration
96 : /** Calls loadConfigImpl()
97 : */
98 : virtual void loadConfig();
99 :
100 : /// Startup function
101 : /**
102 : *
103 : */
104 : virtual int appStartup();
105 :
106 : /// Implementation of the FSM for zaberCtrl.
107 : /**
108 : * \returns 0 on no critical error
109 : * \returns -1 on an error requiring shutdown
110 : */
111 : virtual int appLogic();
112 :
113 : /// Shutdown the app.
114 : /**
115 : *
116 : */
117 : virtual int appShutdown();
118 :
119 : /// Start a high-level homing sequence.
120 : /**
121 : *
122 : * \returns 0 on success.
123 : * \returns -1 on error.
124 : */
125 : int startHoming();
126 :
127 : /// Stop the stage motion immediately.
128 : /**
129 : * \returns 0 on success.
130 : * \returns -1 on error.
131 : */
132 : int stop();
133 :
134 : double presetNumber();
135 :
136 : /// Move to a new position in mm.
137 : /** \todo this actually should move to a preset position by nearest integet.. A moveToMM should be used for the mm
138 : * command.
139 : *
140 : * \returns 0 on success.
141 : * \returns -1 on error.
142 : */
143 : int moveTo( const double &pos /**< [in] The new position in mm*/ );
144 :
145 : protected:
146 : // INDI properties for user interaction:
147 : pcf::IndiProperty m_indiP_pos;
148 : pcf::IndiProperty m_indiP_rawPos;
149 : pcf::IndiProperty m_indiP_maxPos;
150 : pcf::IndiProperty m_indiP_temp;
151 : pcf::IndiProperty m_indiP_parked;
152 : pcf::IndiProperty m_indiP_lastHomed;
153 :
154 : public:
155 0 : INDI_NEWCALLBACK_DECL( zaberCtrl, m_indiP_pos );
156 0 : INDI_NEWCALLBACK_DECL( zaberCtrl, m_indiP_rawPos );
157 :
158 : // INDI properties for interacting with Low-Level
159 : pcf::IndiProperty m_indiP_stageState;
160 : pcf::IndiProperty m_indiP_stageMaxRawPos;
161 : pcf::IndiProperty m_indiP_stageRawPos;
162 : pcf::IndiProperty m_indiP_stageTgtPos;
163 : pcf::IndiProperty m_indiP_stageTemp;
164 : pcf::IndiProperty m_indiP_stageParked;
165 : pcf::IndiProperty m_indiP_stageLastHomed;
166 :
167 : public:
168 0 : INDI_SETCALLBACK_DECL( zaberCtrl, m_indiP_stageState );
169 0 : INDI_SETCALLBACK_DECL( zaberCtrl, m_indiP_stageMaxRawPos );
170 0 : INDI_SETCALLBACK_DECL( zaberCtrl, m_indiP_stageRawPos );
171 0 : INDI_SETCALLBACK_DECL( zaberCtrl, m_indiP_stageTgtPos );
172 0 : INDI_SETCALLBACK_DECL( zaberCtrl, m_indiP_stageTemp );
173 0 : INDI_SETCALLBACK_DECL( zaberCtrl, m_indiP_stageParked );
174 0 : INDI_SETCALLBACK_DECL( zaberCtrl, m_indiP_stageLastHomed );
175 :
176 : /** \name Telemeter Interface
177 : *
178 : * @{
179 : */
180 : int checkRecordTimes();
181 :
182 : int recordTelem( const telem_stage * );
183 :
184 : int recordTelem( const telem_zaber * );
185 :
186 : int recordZaber( bool force = false );
187 :
188 : /// Update the stage telemetry values used while the low-level stage is powered off.
189 : /**
190 : * When a parked stage transitions to `POWEROFF`, preserve the logical
191 : * preset derived from the last reported position so telemetry consumers
192 : * such as `xrif2fits` can keep writing valid preset headers.
193 : *
194 : * \returns 0 on success
195 : */
196 : int syncPowerOffStageTelemetry();
197 :
198 : ///@}
199 : };
200 :
201 225 : zaberCtrl::zaberCtrl() : MagAOXApp( MAGAOX_CURRENT_SHA1, MAGAOX_REPO_MODIFIED )
202 : {
203 45 : m_fractionalPresets = false;
204 45 : m_defaultPositions = false;
205 :
206 45 : return;
207 0 : }
208 :
209 0 : void zaberCtrl::setupConfig()
210 : {
211 0 : config.add( "stage.lowLevelName",
212 : "",
213 : "stage.lowLevelName",
214 : argType::Required,
215 : "stage",
216 : "lowLevelName",
217 : false,
218 : "string",
219 : "The name of the low-level control process actually controlling this stage. "
220 : "Default is zaberLowLevel." );
221 :
222 0 : config.add( "stage.stageName",
223 : "",
224 : "stage.stageName",
225 : argType::Required,
226 : "stage",
227 : "stageName",
228 : false,
229 : "string",
230 : "the name of this stage in the low-level process INDI properties. "
231 : "Default is the configuration name." );
232 :
233 0 : config.add( "stage.countsPerMillimeter",
234 : "",
235 : "stage.countsPerMillimeter",
236 : argType::Required,
237 : "stage",
238 : "countsPerMillimeter",
239 : false,
240 : "float",
241 : "The counts per mm calibration of the stage. Default is 10078.74, "
242 : "which applies tot he X-LRQ-DE." );
243 :
244 0 : config.add( "stage.maxCounts",
245 : "",
246 : "stage.maxCounts",
247 : argType::Required,
248 : "stage",
249 : "maxCounts",
250 : false,
251 : "unsigned long",
252 : "The maximum counts possible for this stage." );
253 :
254 0 : dev::stdMotionStage<zaberCtrl>::setupConfig( config );
255 :
256 0 : dev::telemeter<zaberCtrl>::setupConfig( config );
257 0 : }
258 :
259 0 : int zaberCtrl::loadConfigImpl( mx::app::appConfigurator &_config )
260 : {
261 0 : _config( m_lowLevelName, "stage.lowLevelName" );
262 :
263 0 : m_stageName = m_configName;
264 0 : _config( m_stageName, "stage.stageName" );
265 :
266 0 : _config( m_countsPerMillimeter, "stage.countsPerMillimeter" );
267 :
268 0 : _config( m_maxRawPos, "stage.maxCounts" );
269 0 : m_maxPos = ( (double)m_maxRawPos ) / m_countsPerMillimeter;
270 :
271 0 : dev::stdMotionStage<zaberCtrl>::loadConfig( _config );
272 :
273 0 : dev::telemeter<zaberCtrl>::loadConfig( _config );
274 :
275 0 : return 0;
276 : }
277 :
278 0 : void zaberCtrl::loadConfig()
279 : {
280 0 : loadConfigImpl( config );
281 0 : }
282 :
283 0 : int zaberCtrl::appStartup()
284 : {
285 :
286 0 : createStandardIndiNumber<float>( m_indiP_pos, "position", 0.0, m_maxPos, 1 / m_countsPerMillimeter, "%.4f" );
287 0 : m_indiP_pos["current"].set( 0 );
288 0 : m_indiP_pos["target"].set( 0 );
289 0 : if( registerIndiPropertyNew( m_indiP_pos, INDI_NEWCALLBACK( m_indiP_pos ) ) < 0 )
290 : {
291 : #ifndef ZABERCTRL_TEST_NOLOG
292 0 : log<software_error>( { __FILE__, __LINE__ } );
293 : #endif
294 0 : return -1;
295 : }
296 :
297 0 : createStandardIndiNumber<unsigned long>( m_indiP_rawPos, "rawpos", 0.0, m_maxRawPos, 1, "%lu" );
298 0 : m_indiP_rawPos["current"].set( 0 );
299 0 : m_indiP_rawPos["target"].set( 0 );
300 0 : if( registerIndiPropertyNew( m_indiP_rawPos, INDI_NEWCALLBACK( m_indiP_rawPos ) ) < 0 )
301 : {
302 : #ifndef ZABERCTRL_TEST_NOLOG
303 0 : log<software_error>( { __FILE__, __LINE__ } );
304 : #endif
305 0 : return -1;
306 : }
307 :
308 0 : createROIndiNumber( m_indiP_maxPos, "maxpos", "Maximum Position" );
309 0 : indi::addNumberElement( m_indiP_maxPos, "value", m_maxPos, m_maxPos, 0.0, "%0.4f" );
310 0 : m_indiP_maxPos["value"].set<double>( m_maxPos );
311 0 : registerIndiPropertyReadOnly( m_indiP_maxPos );
312 :
313 0 : createROIndiNumber( m_indiP_temp, "temp", "Stage Temperature [C]" );
314 0 : indi::addNumberElement( m_indiP_temp, "current", -50, 100, 0, "%0.1f" );
315 0 : registerIndiPropertyReadOnly( m_indiP_temp );
316 :
317 0 : createROIndiNumber( m_indiP_parked, "parked", "Parked State" );
318 0 : indi::addNumberElement<int>( m_indiP_parked, "current", 0, 1, 1, "%d" );
319 0 : m_indiP_parked["current"].set<int>( 0 );
320 0 : registerIndiPropertyReadOnly( m_indiP_parked );
321 :
322 0 : createROIndiNumber( m_indiP_lastHomed, "last_homed", "Time of last home [sec]" );
323 0 : indi::addNumberElement<time_t>( m_indiP_lastHomed, "time_sec", 0, std::numeric_limits<time_t>::max(), 1, "%d" );
324 0 : registerIndiPropertyReadOnly( m_indiP_lastHomed );
325 :
326 : // Register for the low level status reports
327 0 : REG_INDI_SETPROP( m_indiP_stageState, m_lowLevelName, "curr_state" );
328 0 : REG_INDI_SETPROP( m_indiP_stageMaxRawPos, m_lowLevelName, std::string( "max_pos" ) );
329 0 : REG_INDI_SETPROP( m_indiP_stageRawPos, m_lowLevelName, std::string( "curr_pos" ) );
330 0 : REG_INDI_SETPROP( m_indiP_stageTgtPos, m_lowLevelName, std::string( "tgt_pos" ) );
331 0 : REG_INDI_SETPROP( m_indiP_stageTemp, m_lowLevelName, std::string( "temp" ) );
332 0 : REG_INDI_SETPROP( m_indiP_stageParked, m_lowLevelName, std::string( "parked" ) );
333 0 : REG_INDI_SETPROP( m_indiP_stageLastHomed, m_lowLevelName, std::string( "last_homed" ) );
334 :
335 0 : if( m_presetNames.size() != m_presetPositions.size() )
336 : {
337 0 : return log<text_log, -1>( "must set a position for each preset", logPrio::LOG_CRITICAL );
338 : }
339 :
340 0 : m_presetNames.insert( m_presetNames.begin(), "none" );
341 0 : m_presetPositions.insert( m_presetPositions.begin(), -1 );
342 :
343 0 : dev::stdMotionStage<zaberCtrl>::appStartup();
344 :
345 0 : if( dev::telemeter<zaberCtrl>::appStartup() < 0 )
346 : {
347 0 : return log<software_error, -1>( { __FILE__, __LINE__ } );
348 : }
349 :
350 0 : return 0;
351 : }
352 :
353 0 : int zaberCtrl::appLogic()
354 : {
355 0 : if( state() == stateCodes::INITIALIZED )
356 : {
357 0 : state( stateCodes::NOTCONNECTED );
358 : }
359 :
360 0 : if( state() == stateCodes::POWEROFF )
361 : {
362 0 : if( m_wason )
363 : {
364 : // Here do poweroff update
365 0 : if( stdMotionStage<zaberCtrl>::onPowerOff() < 0 )
366 : {
367 0 : log<software_error>( { __FILE__, __LINE__ } );
368 : }
369 :
370 0 : m_wason = false;
371 : }
372 : else
373 : {
374 : // Here do poweroff update
375 0 : if( stdMotionStage<zaberCtrl>::whilePowerOff() < 0 )
376 : {
377 0 : log<software_error>( { __FILE__, __LINE__ } );
378 : }
379 : }
380 :
381 0 : if( dev::stdMotionStage<zaberCtrl>::updateINDI() < 0 )
382 : {
383 0 : log<software_error>( { __FILE__, __LINE__ } );
384 : }
385 :
386 : // record telem if it's been longer than 10 sec:
387 0 : if( telemeter<zaberCtrl>::appLogic() < 0 )
388 : {
389 0 : log<software_error>( { __FILE__, __LINE__ } );
390 : }
391 :
392 0 : return 0;
393 : }
394 :
395 0 : if( state() == stateCodes::NOTCONNECTED )
396 : {
397 0 : recordStage();
398 :
399 : // record telem if it's been longer than 10 sec:
400 0 : if( telemeter<zaberCtrl>::appLogic() < 0 )
401 : {
402 0 : log<software_error>( { __FILE__, __LINE__ } );
403 : }
404 :
405 0 : return 0;
406 : }
407 :
408 0 : if( state() == stateCodes::POWERON )
409 : {
410 0 : m_wason = true;
411 0 : recordStage();
412 0 : return 0;
413 : }
414 :
415 0 : if( state() == stateCodes::NOTHOMED )
416 : {
417 0 : if( m_powerOnHome )
418 : {
419 0 : startHoming();
420 0 : sleep( 1 ); // give it time to start
421 : }
422 :
423 0 : return 0;
424 : }
425 :
426 0 : if( state() == stateCodes::HOMING )
427 : {
428 0 : if( m_homingState == 2 )
429 : {
430 0 : if( m_homePreset >= 0 )
431 : {
432 0 : m_preset_target = m_presetPositions[m_homePreset];
433 0 : updateIfChanged( m_indiP_preset, "target", m_preset_target, INDI_BUSY );
434 :
435 : // sleep(2); //pause to give time for controller to update itself and throw a warning
436 :
437 0 : moveTo( m_preset_target );
438 : }
439 :
440 0 : m_homingState = 0;
441 : }
442 : }
443 : // Otherwise we don't do anything differently
444 0 : std::lock_guard<std::mutex> guard( m_indiMutex );
445 :
446 : static int last_moving = -10;
447 :
448 0 : bool changed = false;
449 0 : if( last_moving != m_moving )
450 : {
451 0 : changed = true;
452 0 : last_moving = m_moving;
453 : }
454 :
455 0 : if( changed )
456 : {
457 0 : if( m_moving )
458 : {
459 0 : m_indiP_pos.setState( INDI_BUSY );
460 0 : m_indiP_rawPos.setState( INDI_BUSY );
461 : }
462 : else
463 : {
464 0 : m_indiP_pos.setState( INDI_IDLE );
465 0 : m_indiP_pos["target"] = m_pos;
466 0 : m_indiP_rawPos.setState( INDI_IDLE );
467 0 : m_indiP_rawPos["target"] = m_rawPos;
468 : }
469 :
470 0 : m_indiDriver->sendSetProperty( m_indiP_pos );
471 0 : m_indiDriver->sendSetProperty( m_indiP_rawPos );
472 : }
473 :
474 0 : if( m_moving && m_movingState < 1 )
475 : {
476 0 : updateIfChanged( m_indiP_pos, "current", m_pos, INDI_BUSY );
477 0 : updateIfChanged( m_indiP_rawPos, "current", m_rawPos, INDI_BUSY );
478 : }
479 : else
480 : {
481 0 : updateIfChanged( m_indiP_pos, "current", m_pos, INDI_IDLE );
482 0 : updateIfChanged( m_indiP_rawPos, "current", m_rawPos, INDI_IDLE );
483 : }
484 :
485 0 : int n = presetNumber();
486 0 : if( n == -1 )
487 : {
488 0 : m_preset = 0;
489 0 : m_preset_target = 0;
490 : }
491 : else
492 : {
493 0 : m_preset = n + 1;
494 0 : m_preset_target = n + 1;
495 : }
496 :
497 0 : recordStage();
498 0 : recordZaber();
499 :
500 0 : dev::stdMotionStage<zaberCtrl>::updateINDI();
501 :
502 0 : if( telemeter<zaberCtrl>::appLogic() < 0 )
503 : {
504 0 : log<software_error>( { __FILE__, __LINE__ } );
505 0 : return 0;
506 : }
507 :
508 0 : return 0;
509 0 : }
510 :
511 2 : int zaberCtrl::syncPowerOffStageTelemetry()
512 : {
513 2 : if( !m_parked )
514 : {
515 1 : m_preset = 0;
516 1 : m_preset_target = 0;
517 1 : return 0;
518 : }
519 :
520 1 : int n = presetNumber();
521 1 : if( n == 0 )
522 : {
523 0 : m_preset = 0;
524 0 : m_preset_target = 0;
525 : }
526 : else
527 : {
528 1 : m_preset = n;
529 1 : m_preset_target = n;
530 : }
531 :
532 1 : return 0;
533 : }
534 :
535 0 : int zaberCtrl::appShutdown()
536 : {
537 0 : return 0;
538 : }
539 :
540 0 : int zaberCtrl::startHoming()
541 : {
542 0 : updateSwitchIfChanged( m_indiP_home, "request", pcf::IndiElement::Off, INDI_IDLE );
543 :
544 0 : pcf::IndiProperty indiP_stageHome = pcf::IndiProperty( pcf::IndiProperty::Switch );
545 0 : indiP_stageHome.setDevice( m_lowLevelName );
546 0 : indiP_stageHome.setName( "req_home" );
547 0 : indiP_stageHome.setPerm( pcf::IndiProperty::ReadWrite );
548 0 : indiP_stageHome.setState( pcf::IndiProperty::Idle );
549 0 : indiP_stageHome.add( pcf::IndiElement( m_stageName ) );
550 0 : indiP_stageHome[m_stageName].setSwitchState( pcf::IndiElement::On );
551 :
552 0 : m_moving = 2;
553 0 : m_homingState = 1;
554 :
555 0 : if( sendNewProperty( indiP_stageHome ) < 0 )
556 : {
557 0 : return log<software_error, -1>( { __FILE__, __LINE__ } );
558 : }
559 :
560 0 : return 0;
561 0 : }
562 :
563 0 : int zaberCtrl::stop()
564 : {
565 0 : updateSwitchIfChanged( m_indiP_stop, "request", pcf::IndiElement::Off, INDI_IDLE );
566 :
567 0 : pcf::IndiProperty indiP_stageHalt = pcf::IndiProperty( pcf::IndiProperty::Switch );
568 0 : indiP_stageHalt.setDevice( m_lowLevelName );
569 0 : indiP_stageHalt.setName( "req_halt" );
570 0 : indiP_stageHalt.setPerm( pcf::IndiProperty::ReadWrite );
571 0 : indiP_stageHalt.setState( pcf::IndiProperty::Idle );
572 0 : indiP_stageHalt.add( pcf::IndiElement( m_stageName ) );
573 0 : indiP_stageHalt[m_stageName].setSwitchState( pcf::IndiElement::On );
574 :
575 0 : if( sendNewProperty( indiP_stageHalt ) < 0 )
576 : {
577 0 : return log<software_error, -1>( { __FILE__, __LINE__ } );
578 : }
579 :
580 0 : return 0;
581 0 : }
582 :
583 10 : double zaberCtrl::presetNumber()
584 : {
585 20 : for( size_t n = 1; n < m_presetPositions.size(); ++n )
586 : {
587 20 : if( fabs( m_pos - m_presetPositions[n] ) < 1. / m_countsPerMillimeter )
588 : {
589 10 : return n;
590 : }
591 : }
592 :
593 0 : return 0;
594 : }
595 :
596 0 : int zaberCtrl::moveTo( const double &target )
597 : {
598 0 : if( target < 0 )
599 : {
600 0 : return 0;
601 : }
602 :
603 0 : m_tgtRawPos = ( target * m_countsPerMillimeter + 0.5 );
604 :
605 0 : pcf::IndiProperty indiP_stageTgtPos = pcf::IndiProperty( pcf::IndiProperty::Number );
606 0 : indiP_stageTgtPos.setDevice( m_lowLevelName );
607 0 : indiP_stageTgtPos.setName( "tgt_pos" );
608 0 : indiP_stageTgtPos.setPerm( pcf::IndiProperty::ReadWrite );
609 0 : indiP_stageTgtPos.setState( pcf::IndiProperty::Idle );
610 0 : indiP_stageTgtPos.add( pcf::IndiElement( m_stageName ) );
611 0 : indiP_stageTgtPos[m_stageName].set( m_tgtRawPos );
612 :
613 0 : m_moving = 1;
614 0 : dev::stdMotionStage<zaberCtrl>::recordStage( true );
615 0 : recordZaber( true );
616 :
617 0 : if( sendNewProperty( indiP_stageTgtPos ) < 0 )
618 : {
619 0 : return log<software_error, -1>( { __FILE__, __LINE__ } );
620 : }
621 :
622 0 : return 0;
623 0 : }
624 :
625 3 : INDI_NEWCALLBACK_DEFN( zaberCtrl, m_indiP_pos )( const pcf::IndiProperty &ipRecv )
626 : {
627 3 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_pos, ipRecv );
628 :
629 : double target = -1;
630 : if( ipRecv.find( "target" ) ) // Just not our stage.
631 : {
632 : target = ipRecv["target"].get<double>();
633 : }
634 :
635 : if( target < 0 )
636 : {
637 : return 0;
638 : }
639 :
640 : if( m_moving != 0 )
641 : {
642 : log<text_log>( "abs move to " + std::to_string( target ) + " rejected due to already moving" );
643 : return 0;
644 : }
645 :
646 : log<text_log>( "moving stage to " + std::to_string( target ) );
647 :
648 : std::lock_guard<std::mutex> guard( m_indiMutex );
649 :
650 : m_tgtPos = target;
651 : clearPresetNameTracking();
652 : m_movingState = 0;
653 :
654 : moveTo( m_tgtPos );
655 : updateIfChanged( m_indiP_pos, "target", m_tgtPos, INDI_BUSY );
656 : updateIfChanged( m_indiP_rawPos, "target", m_tgtRawPos, INDI_BUSY );
657 : return 0;
658 : }
659 :
660 3 : INDI_NEWCALLBACK_DEFN( zaberCtrl, m_indiP_rawPos )( const pcf::IndiProperty &ipRecv )
661 : {
662 3 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_rawPos, ipRecv );
663 :
664 : double target = -1;
665 : if( ipRecv.find( "target" ) ) // Just not our stage.
666 : {
667 : target = ipRecv["target"].get<double>();
668 : }
669 :
670 : if( target < 0 )
671 : {
672 : if( ipRecv.find( "current" ) ) // Just not our stage.
673 : {
674 : target = ipRecv["current"].get<double>();
675 : }
676 :
677 : if( target < 0 )
678 : {
679 : return log<text_log, -1>( "no valid target position provided", logPrio::LOG_ERROR );
680 : }
681 : }
682 :
683 : if( m_moving != 0 )
684 : {
685 : log<text_log>( "rel move rejected due to already moving" );
686 : return 0;
687 : }
688 :
689 : log<text_log>( "moving stage by " + std::to_string( target ) );
690 :
691 : std::lock_guard<std::mutex> guard( m_indiMutex );
692 : clearPresetNameTracking();
693 : m_movingState = 0;
694 :
695 : pcf::IndiProperty indiP_stageTgtPos = pcf::IndiProperty( pcf::IndiProperty::Text );
696 : indiP_stageTgtPos.setDevice( m_lowLevelName );
697 : indiP_stageTgtPos.setName( "tgt_pos" );
698 : indiP_stageTgtPos.setPerm( pcf::IndiProperty::ReadWrite );
699 : indiP_stageTgtPos.setState( pcf::IndiProperty::Idle );
700 : indiP_stageTgtPos.add( pcf::IndiElement( m_stageName ) );
701 : indiP_stageTgtPos[m_stageName].set( target );
702 :
703 : dev::stdMotionStage<zaberCtrl>::recordStage( true );
704 :
705 : if( sendNewProperty( indiP_stageTgtPos ) < 0 )
706 : {
707 : return log<software_error, -1>( { __FILE__, __LINE__ } );
708 : }
709 :
710 : m_tgtRawPos = target;
711 : m_tgtPos = m_tgtRawPos / m_countsPerMillimeter;
712 : updateIfChanged( m_indiP_pos, "target", m_tgtPos, INDI_BUSY );
713 : updateIfChanged( m_indiP_rawPos, "target", m_tgtRawPos, INDI_BUSY );
714 : return 0;
715 : }
716 :
717 5 : INDI_SETCALLBACK_DEFN( zaberCtrl, m_indiP_stageState )( const pcf::IndiProperty &ipRecv )
718 : {
719 5 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_stageState, ipRecv );
720 :
721 : if( ipRecv.find( m_stageName ) != true ) // Just not our stage.
722 : {
723 : return 0;
724 : }
725 :
726 : m_indiP_stageState = ipRecv;
727 :
728 : std::string sstr = ipRecv[m_stageName].get<std::string>();
729 :
730 : std::lock_guard<std::mutex> guard( m_indiMutex );
731 :
732 : if( sstr == "POWEROFF" )
733 : {
734 : state( stateCodes::POWEROFF );
735 : m_moving = -2;
736 : syncPowerOffStageTelemetry();
737 : dev::stdMotionStage<zaberCtrl>::updateINDI();
738 : dev::stdMotionStage<zaberCtrl>::recordStage( true );
739 : }
740 : else if( sstr == "POWERON" )
741 : {
742 : state( stateCodes::POWERON );
743 : m_moving = -1;
744 : }
745 : else if( sstr == "NODEVICE" )
746 : {
747 : state( stateCodes::NODEVICE );
748 : m_moving = -2;
749 : }
750 : else if( sstr == "NOTCONNECTED" )
751 : {
752 : state( stateCodes::NOTCONNECTED );
753 : m_moving = -2;
754 : }
755 : else if( sstr == "CONNECTED" )
756 : {
757 : state( stateCodes::CONNECTED );
758 : m_moving = -2;
759 : }
760 : else if( sstr == "NOTHOMED" )
761 : {
762 : state( stateCodes::NOTHOMED );
763 : m_moving = -1;
764 : }
765 : else if( sstr == "HOMING" )
766 : {
767 : state( stateCodes::HOMING );
768 : m_homingState = 1;
769 : m_moving = 2;
770 : }
771 : else if( sstr == "READY" )
772 : {
773 : if( m_homingState == 0 || m_homePreset < 0 )
774 : {
775 : m_homingState = 0;
776 : state( stateCodes::READY );
777 : }
778 : else
779 : {
780 : m_homingState = 2;
781 : }
782 : m_moving = 0;
783 : }
784 : else if( sstr == "OPERATING" )
785 : {
786 : state( stateCodes::OPERATING );
787 : m_moving = 1;
788 : }
789 : else if( sstr == "SHUTDOWN" )
790 : {
791 : state( stateCodes::NOTCONNECTED );
792 : m_moving = -2;
793 : }
794 : else if( sstr == "FAILURE" )
795 : {
796 : state( stateCodes::FAILURE );
797 : m_moving = -2;
798 : }
799 : else if( sstr == "ERROR" )
800 : {
801 : state( stateCodes::ERROR );
802 : m_moving = -2;
803 : }
804 : else
805 : {
806 : std::cerr << "else: " << sstr << "\n";
807 : state( stateCodes::UNINITIALIZED );
808 : m_moving = -2;
809 : }
810 : return 0;
811 : }
812 :
813 3 : INDI_SETCALLBACK_DEFN( zaberCtrl, m_indiP_stageMaxRawPos )( const pcf::IndiProperty &ipRecv )
814 : {
815 3 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_stageMaxRawPos, ipRecv );
816 :
817 : if( ipRecv.find( m_stageName ) != true ) // Just not our stage.
818 : {
819 : return 0;
820 : }
821 :
822 : std::lock_guard<std::mutex> guard( m_indiMutex );
823 :
824 : long maxRawPos = ipRecv[m_stageName].get<long>();
825 :
826 : if( maxRawPos > 10000000000000 || maxRawPos < 0 )
827 : return 0; // Indicates a bad value
828 :
829 : if( (unsigned long)maxRawPos != m_maxRawPos &&
830 : ( state() == stateCodes::CONNECTED || state() == stateCodes::HOMING || state() == stateCodes::READY ||
831 : state() == stateCodes::OPERATING ) )
832 : {
833 : log<text_log>( std::format( "max position mismatch between config-ed value ({}) and stage value ({})",
834 : m_maxRawPos,
835 : maxRawPos ),
836 : logPrio::LOG_WARNING );
837 : }
838 :
839 : return 0;
840 : }
841 :
842 3 : INDI_SETCALLBACK_DEFN( zaberCtrl, m_indiP_stageRawPos )( const pcf::IndiProperty &ipRecv )
843 : {
844 3 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_stageRawPos, ipRecv );
845 :
846 : if( ipRecv.find( m_stageName ) != true ) // Just not our stage.
847 : {
848 : return 0;
849 : }
850 :
851 : std::lock_guard<std::mutex> guard( m_indiMutex );
852 :
853 : m_rawPos = ipRecv[m_stageName].get<double>();
854 :
855 : m_pos = m_rawPos / m_countsPerMillimeter;
856 :
857 : if( m_moving )
858 : {
859 : updateIfChanged( m_indiP_rawPos, "current", m_rawPos, INDI_BUSY );
860 : updateIfChanged( m_indiP_pos, "current", m_pos, INDI_BUSY );
861 : }
862 : else
863 : {
864 : updateIfChanged( m_indiP_rawPos, "current", m_rawPos, INDI_IDLE );
865 : updateIfChanged( m_indiP_pos, "current", m_pos, INDI_IDLE );
866 : }
867 :
868 : dev::stdMotionStage<zaberCtrl>::recordStage();
869 :
870 : return 0;
871 : }
872 :
873 3 : INDI_SETCALLBACK_DEFN( zaberCtrl, m_indiP_stageTgtPos )( const pcf::IndiProperty &ipRecv )
874 : {
875 3 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_stageTgtPos, ipRecv );
876 :
877 : if( ipRecv.find( m_stageName ) != true ) // Just not our stage.
878 : {
879 : return 0;
880 : }
881 :
882 : // Test for empty property to see if target reached
883 : std::string test = ipRecv[m_stageName].get<std::string>();
884 :
885 : if( test == "" )
886 : {
887 : return 0;
888 : }
889 :
890 : std::lock_guard<std::mutex> guard( m_indiMutex );
891 :
892 : m_tgtRawPos = ipRecv[m_stageName].get<double>();
893 : m_tgtPos = m_tgtRawPos / m_countsPerMillimeter;
894 :
895 : if( m_moving )
896 : {
897 : updateIfChanged( m_indiP_rawPos, "target", m_tgtRawPos, INDI_BUSY );
898 : updateIfChanged( m_indiP_pos, "target", m_tgtPos, INDI_BUSY );
899 : }
900 : else
901 : {
902 : updateIfChanged( m_indiP_rawPos, "target", m_tgtRawPos, INDI_IDLE );
903 : updateIfChanged( m_indiP_pos, "target", m_tgtPos, INDI_IDLE );
904 : }
905 :
906 : return 0;
907 : }
908 :
909 3 : INDI_SETCALLBACK_DEFN( zaberCtrl, m_indiP_stageTemp )( const pcf::IndiProperty &ipRecv )
910 : {
911 3 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_stageTemp, ipRecv );
912 :
913 : if( ipRecv.find( m_stageName ) != true ) // Just not our stage.
914 : {
915 : return 0;
916 : }
917 :
918 : std::lock_guard<std::mutex> guard( m_indiMutex );
919 :
920 : m_stageTemp = ipRecv[m_stageName].get<double>();
921 :
922 : updateIfChanged( m_indiP_temp, "current", m_stageTemp );
923 :
924 : return 0;
925 : }
926 :
927 3 : INDI_SETCALLBACK_DEFN( zaberCtrl, m_indiP_stageParked )( const pcf::IndiProperty &ipRecv )
928 : {
929 3 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_stageParked, ipRecv );
930 :
931 : if( ipRecv.find( m_stageName ) != true ) // Just not our stage.
932 : {
933 : return 0;
934 : }
935 :
936 : std::lock_guard<std::mutex> guard( m_indiMutex );
937 :
938 : m_parked = ( ipRecv[m_stageName].get<int>() != 0 );
939 :
940 : if( state() == stateCodes::POWEROFF )
941 : {
942 : syncPowerOffStageTelemetry();
943 : dev::stdMotionStage<zaberCtrl>::updateINDI();
944 : dev::stdMotionStage<zaberCtrl>::recordStage( true );
945 : }
946 :
947 : updateIfChanged( m_indiP_parked, "current", static_cast<int>( m_parked ) );
948 :
949 : return 0;
950 : }
951 :
952 0 : INDI_SETCALLBACK_DEFN( zaberCtrl, m_indiP_stageLastHomed )( const pcf::IndiProperty &ipRecv )
953 : {
954 0 : INDI_VALIDATE_CALLBACK_PROPS( m_indiP_stageLastHomed, ipRecv );
955 :
956 : if( ipRecv.find( m_stageName ) != true ) // Just not our stage.
957 : {
958 : return 0;
959 : }
960 :
961 : std::lock_guard<std::mutex> guard( m_indiMutex );
962 :
963 : m_lastHomed = ipRecv[m_stageName].get<double>();
964 :
965 : updateIfChanged( m_indiP_lastHomed, "time_sec", m_lastHomed );
966 :
967 : return 0;
968 : }
969 :
970 0 : int zaberCtrl::checkRecordTimes()
971 : {
972 0 : return telemeter<zaberCtrl>::checkRecordTimes( telem_stage(), telem_zaber() );
973 : }
974 :
975 0 : int zaberCtrl::recordTelem( const telem_stage * )
976 : {
977 0 : return stdMotionStage<zaberCtrl>::recordStage( true );
978 : }
979 :
980 0 : int zaberCtrl::recordTelem( const telem_zaber * )
981 : {
982 0 : return recordZaber( true );
983 : }
984 :
985 0 : int zaberCtrl::recordZaber( bool force )
986 : {
987 0 : static double last_pos = m_pos - 1000;
988 0 : static double last_rawPos = m_rawPos;
989 0 : static double last_temp = m_stageTemp;
990 :
991 0 : if( m_pos != last_pos || m_rawPos != last_rawPos || m_stageTemp != last_temp || force )
992 : {
993 0 : telem<telem_zaber>( { (float)m_pos, (float)m_rawPos, (float)m_stageTemp } );
994 :
995 0 : last_pos = m_pos;
996 0 : last_rawPos = m_rawPos;
997 0 : last_temp = m_stageTemp;
998 : }
999 :
1000 0 : return 0;
1001 : }
1002 :
1003 : } // namespace app
1004 : } // namespace MagAOX
1005 :
1006 : #endif // zaberCtrl_hpp
|
