Line data Source code
1 : /** \file picoMotorCtrl.hpp
2 : * \brief The MagAO-X Pico Motor Controller header file
3 : *
4 : * \ingroup picoMotorCtrl_files
5 : */
6 :
7 : #ifndef picoMotorCtrl_hpp
8 : #define picoMotorCtrl_hpp
9 :
10 : #include <map>
11 :
12 : #include "../../libMagAOX/libMagAOX.hpp" //Note this is included on command line to trigger pch
13 : #include "../../magaox_git_version.h"
14 :
15 : /** \defgroup picoMotorCtrl
16 : * \brief The Pico Motor Controller application.
17 : *
18 : * Controls a multi-channel Newport pico motor controller. Each motor gets its own thread.
19 : *
20 : * <a href="../handbook/operating/software/apps/picoMotorCtrl.html">Application Documentation</a>
21 : *
22 : * \ingroup apps
23 : *
24 : */
25 :
26 : /** \defgroup picoMotorCtrl_files
27 : * \ingroup picoMotorCtrl
28 : */
29 :
30 :
31 :
32 : namespace MagAOX
33 : {
34 : namespace app
35 : {
36 :
37 0 : int splitResponse( int & address,
38 : std::string & response,
39 : const std::string & fullResponse
40 : )
41 : {
42 0 : size_t carrot = fullResponse.find('>');
43 :
44 0 : if(carrot == std::string::npos)
45 : {
46 0 : address = 1;
47 0 : response = fullResponse;
48 0 : return 0;
49 : }
50 :
51 0 : if(carrot == 0)
52 : {
53 0 : address = 0;
54 0 : response = "";
55 0 : return -1;
56 : }
57 :
58 0 : if(carrot == fullResponse.size()-1)
59 : {
60 0 : address = 0;
61 0 : response = "";
62 0 : return -2;
63 : }
64 :
65 : try
66 : {
67 0 : address = std::stoi( fullResponse.substr(0,carrot));
68 0 : response = fullResponse.substr(carrot+1);
69 : }
70 0 : catch(...)
71 : {
72 0 : address = 0;
73 0 : response = "";
74 0 : return -3;
75 0 : }
76 :
77 0 : return 0;
78 :
79 : }
80 :
81 : /** MagAO-X application to control a multi-channel Newport Picomotor Controller.
82 : *
83 : * \todo need to recognize signals in tty polls and not return errors, etc.
84 : * \todo need to implement an onDisconnect() to update values to unknown indicators.
85 : * \todo need a frequency-dependent max amp facility.
86 : * \todo convert to ioDevice
87 : * \todo need telnet device, with optional username/password.
88 : *
89 : */
90 : class picoMotorCtrl : public MagAOXApp<>, public dev::ioDevice, public dev::telemeter<picoMotorCtrl>
91 : {
92 :
93 : friend class dev::telemeter<picoMotorCtrl>;
94 :
95 : typedef dev::telemeter<picoMotorCtrl> telemeterT;
96 :
97 : typedef long posT;
98 :
99 : struct motorChannel
100 : {
101 : picoMotorCtrl * m_parent {nullptr}; ///< A pointer to this for thread starting.
102 :
103 : std::string m_name; ///< The name of this channel, from the config section
104 :
105 : int m_address {1}; ///< The controller address, default is 1
106 :
107 : int m_channel {-1}; ///< The number of this channel, where the motor is plugged in
108 :
109 : int m_type {3}; ///< The motor type of this channel, default is 3
110 :
111 : std::vector<std::string> m_presetNames;
112 : std::vector<posT> m_presetPositions;
113 :
114 : posT m_currCounts {0}; ///< The current counts, the cumulative position
115 :
116 : bool m_doMove {false}; ///< Flag indicating that a move is requested.
117 : bool m_moving {false}; ///< Flag to indicate that we are actually moving
118 :
119 : pcf::IndiProperty m_property;
120 : pcf::IndiProperty m_indiP_presetName;
121 :
122 : std::thread * m_thread {nullptr}; /**< Thread for managing this channel. A pointer to allow
123 : copying, but must be deleted in d'tor of parent.*/
124 :
125 : bool m_threadInit {true}; ///< Thread initialization flag.
126 :
127 : pid_t m_threadID {0}; ///< The ID of the thread.
128 :
129 : pcf::IndiProperty m_threadProp; ///< The property to hold the thread details.
130 :
131 : explicit motorChannel( picoMotorCtrl * p /**< [in] The parent point to set */) : m_parent(p)
132 : {
133 : m_thread = new std::thread;
134 : }
135 :
136 0 : motorChannel( picoMotorCtrl * p, ///< [in] The parent point to set
137 : const std::string & n, ///< [in] The name of this channel
138 : int add, ///< [in] The controller address
139 : int ch, ///< [in] The number of this channel
140 : int type ///< [in] The motor type of this channel
141 0 : ) : m_parent(p), m_name(n), m_address(add), m_channel(ch), m_type(type)
142 : {
143 0 : m_thread = new std::thread;
144 0 : }
145 :
146 0 : motorChannel(const motorChannel &mc)
147 0 : {
148 0 : m_parent = mc.m_parent;
149 0 : m_name = mc.m_name;
150 0 : m_address = mc.m_address;
151 0 : m_channel = mc.m_channel;
152 0 : m_type = mc.m_type;
153 0 : m_presetNames = mc.m_presetNames;
154 0 : m_presetPositions = mc.m_presetPositions;
155 0 : m_currCounts = mc.m_currCounts;
156 0 : m_doMove = mc.m_doMove;
157 0 : m_moving = mc.m_moving;
158 0 : m_property = mc.m_property;
159 0 : m_indiP_presetName = mc.m_indiP_presetName;
160 0 : m_thread = mc.m_thread;
161 0 : m_threadInit = mc.m_threadInit;
162 0 : m_threadID =mc.m_threadID;
163 0 : m_threadProp = mc.m_threadProp;
164 0 : }
165 : };
166 :
167 : typedef std::map<std::string, motorChannel> channelMapT;
168 :
169 : /** \name Configurable Parameters
170 : * @{
171 : */
172 :
173 : std::string m_deviceAddr; ///< The device address
174 : std::string m_devicePort {"23"}; ///< The device port
175 :
176 : int m_nChannels {4}; ///< The number of motor channels total on the hardware. Number of attached motors inferred from config.
177 :
178 : ///@}
179 :
180 : std::vector<int> m_addresses; ///< The unique controller addresses.
181 :
182 : channelMapT m_channels; ///< Map of motor names to channel.
183 :
184 : tty::telnetConn m_telnetConn; ///< The telnet connection manager
185 :
186 : ///Mutex for locking telnet communications.
187 : std::mutex m_telnetMutex;
188 :
189 : public:
190 :
191 : /// Default c'tor.
192 : picoMotorCtrl();
193 :
194 : /// D'tor, declared and defined for noexcept.
195 : ~picoMotorCtrl() noexcept;
196 :
197 : /// Setup the configuration system (called by MagAOXApp::setup())
198 : virtual void setupConfig();
199 :
200 : /// Implementation of loadConfig logic, separated for testing.
201 : /** This is called by loadConfig().
202 : */
203 : int loadConfigImpl( mx::app::appConfigurator & _config /**< [in] an application configuration from which to load values*/);
204 :
205 : /// load the configuration system results (called by MagAOXApp::setup())
206 : virtual void loadConfig();
207 :
208 : /// Startup functions
209 : /** Setsup the INDI vars.
210 : *
211 : */
212 : virtual int appStartup();
213 :
214 : /// Implementation of the FSM
215 : /**
216 : * \returns 0 on no critical error
217 : * \returns -1 on an error requiring shutdown
218 : */
219 : virtual int appLogic();
220 :
221 : /// Implementation of the on-power-off FSM logic
222 : virtual int onPowerOff();
223 :
224 : /// Implementation of the while-powered-off FSM
225 : virtual int whilePowerOff();
226 :
227 : /// Do any needed shutdown tasks.
228 : virtual int appShutdown();
229 :
230 : /// Read the current channel counts from disk at startup
231 : /** Reads the counts from the file with the specified name in this apps sys directory.
232 : * Returns the file contents as a posT.
233 : */
234 : posT readChannelCounts(const std::string & chName);
235 :
236 : int writeChannelCounts( const std::string & chName,
237 : posT counts
238 : );
239 :
240 : /// Channel thread starter function
241 : static void channelThreadStart( motorChannel * mc /**< [in] the channel to start controlling */);
242 :
243 : /// Channel thread execution function
244 : /** Runs until m_shutdown is true.
245 : */
246 : void channelThreadExec( motorChannel * mc );
247 :
248 : /** \name INDI
249 : * @{
250 : */
251 : protected:
252 :
253 : //declare our properties
254 : std::vector<pcf::IndiProperty> m_indiP_counts;
255 :
256 :
257 : public:
258 : /// The static callback function to be registered for relative position requests
259 : /** Dispatches to the handler, which then signals the relavent thread.
260 : *
261 : * \returns 0 on success.
262 : * \returns -1 on error.
263 : */
264 : static int st_newCallBack_picopos( void * app, ///< [in] a pointer to this, will be static_cast-ed to this
265 : const pcf::IndiProperty &ipRecv ///< [in] the INDI property sent with the the new property request.
266 : );
267 :
268 : /// The handler function for relative position requests, called by the static callback
269 : /** Signals the relavent thread.
270 : *
271 : * \returns 0 on success.
272 : * \returns -1 on error.
273 : */
274 : int newCallBack_picopos( const pcf::IndiProperty &ipRecv /**< [in] the INDI property sent with the the new property request.*/);
275 :
276 : /// The static callback function to be registered for position presets
277 : /** Dispatches to the handler, which then signals the relavent thread.
278 : *
279 : * \returns 0 on success.
280 : * \returns -1 on error.
281 : */
282 : static int st_newCallBack_presetName( void * app, ///< [in] a pointer to this, will be static_cast-ed to this
283 : const pcf::IndiProperty &ipRecv ///< [in] the INDI property sent with the the new property request.
284 : );
285 :
286 : /// The handler function for position presets, called by the static callback
287 : /** Signals the relavent thread.
288 : *
289 : * \returns 0 on success.
290 : * \returns -1 on error.
291 : */
292 : int newCallBack_presetName( const pcf::IndiProperty &ipRecv /**< [in] the INDI property sent with the the new property request.*/);
293 : ///@}
294 :
295 : /** \name Telemeter Interface
296 : *
297 : * @{
298 : */
299 : int checkRecordTimes();
300 :
301 : int recordTelem( const telem_pico * );
302 :
303 : int recordPico( bool force = false );
304 : ///@}
305 :
306 : };
307 :
308 0 : picoMotorCtrl::picoMotorCtrl() : MagAOXApp(MAGAOX_CURRENT_SHA1, MAGAOX_REPO_MODIFIED)
309 : {
310 0 : m_powerMgtEnabled = true;
311 0 : m_telnetConn.m_prompt = "\r\n";
312 0 : return;
313 0 : }
314 :
315 0 : picoMotorCtrl::~picoMotorCtrl() noexcept
316 : {
317 : //Wait for each channel thread to exit, then delete it.
318 0 : for(channelMapT::iterator it = m_channels.begin(); it != m_channels.end(); ++ it)
319 : {
320 0 : if(it->second.m_thread != nullptr)
321 : {
322 0 : if(it->second.m_thread->joinable()) it->second.m_thread->join();
323 0 : delete it->second.m_thread;
324 : }
325 : }
326 0 : }
327 :
328 :
329 0 : void picoMotorCtrl::setupConfig()
330 : {
331 0 : config.add("device.address", "", "device.address", argType::Required, "device", "address", false, "string", "The controller IP address.");
332 0 : config.add("device.nChannels", "", "device.nChannels", argType::Required, "device", "nChannels", false, "int", "Number of motoro channels. Default is 4.");
333 :
334 0 : dev::ioDevice::setupConfig(config);
335 :
336 0 : TELEMETER_SETUP_CONFIG( config );
337 :
338 0 : }
339 :
340 : #define PICOMOTORCTRL_E_NOMOTORS (-5)
341 : #define PICOMOTORCTRL_E_BADCHANNEL (-6)
342 : #define PICOMOTORCTRL_E_DUPMOTOR (-7)
343 : #define PICOMOTORCTRL_E_INDIREG (-20)
344 :
345 0 : int picoMotorCtrl::loadConfigImpl( mx::app::appConfigurator & _config )
346 : {
347 : //Standard config parsing
348 0 : _config(m_deviceAddr, "device.address");
349 0 : _config(m_nChannels, "device.nChannels");
350 :
351 :
352 : // Parse the unused config options to look for motors
353 0 : std::vector<std::string> sections;
354 :
355 0 : _config.unusedSections(sections);
356 :
357 0 : if( sections.size() == 0 )
358 : {
359 0 : log<text_log>("No motors found in config.", logPrio::LOG_CRITICAL);
360 :
361 0 : return PICOMOTORCTRL_E_NOMOTORS;
362 : }
363 :
364 : //Now see if any unused sections have a channel keyword
365 0 : for(size_t i=0;i<sections.size(); ++i)
366 : {
367 0 : int channel = -1;
368 0 : _config.configUnused(channel, mx::app::iniFile::makeKey(sections[i], "channel" ) );
369 0 : if( channel == -1 )
370 : {
371 : //not a channel
372 0 : continue;
373 : }
374 :
375 0 : if(channel < 1 || channel > m_nChannels)
376 : {
377 0 : log<text_log>("Bad channel specificiation: " + sections[i] + " channel: " + std::to_string(channel), logPrio::LOG_CRITICAL);
378 :
379 0 : return PICOMOTORCTRL_E_BADCHANNEL;
380 : }
381 :
382 0 : int address = 1;
383 0 : _config.configUnused(address, mx::app::iniFile::makeKey(sections[i], "address" ) );
384 :
385 0 : if(address < 1)
386 : {
387 0 : log<text_log>("Bad channel specificiation: " + sections[i] + " address: " + std::to_string(address), logPrio::LOG_CRITICAL);
388 :
389 0 : return PICOMOTORCTRL_E_BADCHANNEL;
390 : }
391 :
392 0 : int type = 3;
393 0 : _config.configUnused(type, mx::app::iniFile::makeKey(sections[i], "type" ) );
394 :
395 0 : if(type < 1)
396 : {
397 0 : log<text_log>("Bad motor type specificiation: " + sections[i] + " type: " + std::to_string(type), logPrio::LOG_CRITICAL);
398 :
399 0 : return PICOMOTORCTRL_E_BADCHANNEL;
400 : }
401 :
402 : //Ok, valid channel. Insert into map and check for duplicates.
403 0 : std::pair<channelMapT::iterator, bool> insert = m_channels.insert(std::pair<std::string, motorChannel>(sections[i], motorChannel(this,sections[i], address, channel, type)));
404 :
405 0 : if(insert.second == false)
406 : {
407 0 : log<text_log>("Duplicate motor specificiation: " + sections[i] + " " + std::to_string(channel), logPrio::LOG_CRITICAL);
408 0 : return PICOMOTORCTRL_E_DUPMOTOR;
409 : }
410 : else
411 : {
412 0 : _config.configUnused(insert.first->second.m_presetNames, mx::app::iniFile::makeKey(sections[i], "names" ));
413 0 : _config.configUnused(insert.first->second.m_presetPositions, mx::app::iniFile::makeKey(sections[i], "positions" ));
414 : }
415 :
416 : ///\todo extend to include address
417 0 : log<pico_channel>({sections[i], (uint8_t) channel});
418 :
419 0 : bool found = false;
420 0 : for(size_t n = 0; n < m_addresses.size(); ++n)
421 : {
422 0 : if(address == m_addresses[n])
423 : {
424 0 : found = true;
425 0 : break;
426 : }
427 : }
428 :
429 0 : if(!found)
430 : {
431 0 : m_addresses.push_back(address);
432 : }
433 : }
434 :
435 0 : TELEMETER_LOAD_CONFIG( config );
436 :
437 0 : return 0;
438 0 : }
439 :
440 0 : void picoMotorCtrl::loadConfig()
441 : {
442 0 : if( loadConfigImpl(config) < 0)
443 : {
444 0 : log<text_log>("Error during config", logPrio::LOG_CRITICAL);
445 0 : m_shutdown = true;
446 : }
447 :
448 0 : if(dev::ioDevice::loadConfig(config) < 0)
449 : {
450 0 : log<text_log>("Error during ioDevice config", logPrio::LOG_CRITICAL);
451 0 : m_shutdown = true;
452 : }
453 :
454 :
455 :
456 0 : }
457 :
458 0 : int picoMotorCtrl::appStartup()
459 : {
460 : ///\todo read state from disk to get current counts.
461 :
462 0 : for(channelMapT::iterator it = m_channels.begin(); it != m_channels.end(); ++ it)
463 : {
464 0 : it->second.m_currCounts = readChannelCounts(it->second.m_name);
465 :
466 :
467 0 : createStandardIndiNumber( it->second.m_property, it->first+"_pos", std::numeric_limits<posT>::lowest(), std::numeric_limits<posT>::max(), static_cast<posT>(1), "%d", "Position", it->first);
468 0 : it->second.m_property["current"].set(it->second.m_currCounts);
469 0 : it->second.m_property["target"].set(it->second.m_currCounts);
470 0 : it->second.m_property.setState(INDI_IDLE);
471 :
472 0 : if( registerIndiPropertyNew( it->second.m_property, st_newCallBack_picopos) < 0)
473 : {
474 : #ifndef PICOMOTORCTRL_TEST_NOLOG
475 0 : log<software_error>({__FILE__,__LINE__});
476 : #endif
477 0 : return PICOMOTORCTRL_E_INDIREG;
478 : }
479 :
480 0 : if(it->second.m_presetNames.size() > 0)
481 : {
482 0 : if(createStandardIndiSelectionSw( it->second.m_indiP_presetName, it->first, it->second.m_presetNames) < 0)
483 : {
484 0 : log<software_critical>({__FILE__, __LINE__});
485 0 : return -1;
486 : }
487 0 : if( registerIndiPropertyNew( it->second.m_indiP_presetName, st_newCallBack_presetName) < 0)
488 : {
489 0 : log<software_error>({__FILE__,__LINE__});
490 0 : return -1;
491 : }
492 : }
493 :
494 : //Here we start each channel thread, with 0 R/T prio.
495 0 : threadStart( *it->second.m_thread, it->second.m_threadInit, it->second.m_threadID, it->second.m_threadProp, 0, "", it->second.m_name, &it->second, channelThreadStart);
496 : }
497 :
498 : //Install empty signal handler for USR1, which is used to interrupt sleeps in the channel threads.
499 : struct sigaction act;
500 : sigset_t set;
501 :
502 0 : act.sa_sigaction = &sigUsr1Handler;
503 0 : act.sa_flags = SA_SIGINFO;
504 0 : sigemptyset(&set);
505 0 : act.sa_mask = set;
506 :
507 0 : errno = 0;
508 0 : if( sigaction(SIGUSR1, &act, 0) < 0 )
509 : {
510 0 : std::string logss = "Setting handler for SIGUSR1 failed. Errno says: ";
511 0 : logss += strerror(errno);
512 :
513 0 : log<software_error>({__FILE__, __LINE__, errno, 0, logss});
514 :
515 0 : return -1;
516 0 : }
517 :
518 0 : TELEMETER_APP_STARTUP;
519 :
520 0 : return 0;
521 : }
522 :
523 0 : int picoMotorCtrl::appLogic()
524 : {
525 0 : if( state() == stateCodes::POWERON)
526 : {
527 0 : if(!powerOnWaitElapsed()) return 0;
528 :
529 0 : state(stateCodes::NOTCONNECTED);
530 : }
531 :
532 0 : if(state() == stateCodes::NOTCONNECTED || state() == stateCodes::ERROR)
533 : {
534 0 : int rv = m_telnetConn.connect(m_deviceAddr, m_devicePort);
535 :
536 0 : if(rv == 0)
537 : {
538 0 : state(stateCodes::CONNECTED);
539 0 : m_telnetConn.noLogin();
540 : }
541 : else
542 : {
543 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
544 :
545 0 : if(!stateLogged())
546 : {
547 0 : log<text_log>("Failed to connect on " + m_deviceAddr + ":" + m_devicePort);
548 : }
549 :
550 0 : return 0;
551 : }
552 :
553 : }
554 :
555 0 : if(state() == stateCodes::CONNECTED)
556 : {
557 :
558 0 : std::unique_lock<std::mutex> lock(m_telnetMutex);
559 :
560 : //First check the address 1 controller
561 0 : int rv = m_telnetConn.write("*IDN?\r\n", m_writeTimeout);
562 0 : if(rv != TTY_E_NOERROR)
563 : {
564 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
565 0 : log<software_error>({__FILE__, __LINE__, tty::ttyErrorString(rv)});
566 0 : state(stateCodes::ERROR);
567 0 : return 0;
568 : }
569 :
570 0 : rv = m_telnetConn.read("\r\n", m_readTimeout, true);
571 0 : if(rv != TTY_E_NOERROR)
572 : {
573 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
574 0 : log<software_error>({__FILE__, __LINE__, tty::ttyErrorString(rv)});
575 0 : state(stateCodes::ERROR);
576 0 : return 0;
577 : }
578 :
579 0 : if(m_telnetConn.m_strRead.find("New_Focus") != std::string::npos)
580 : {
581 0 : log<text_log>("Connected to " + m_telnetConn.m_strRead + " at address 1");
582 : }
583 : else
584 : {
585 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
586 0 : log<software_error>({__FILE__, __LINE__, "wrong response to IDN query at address 1"});
587 0 : state(stateCodes::ERROR);
588 0 : return 0;
589 : }
590 :
591 : //Now do a controller scan
592 0 : rv = m_telnetConn.write("SC1\r\n", m_writeTimeout); //Will adjust addresses.
593 0 : if(rv != TTY_E_NOERROR)
594 : {
595 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
596 0 : log<software_error>({__FILE__, __LINE__, tty::ttyErrorString(rv)});
597 0 : state(stateCodes::ERROR);
598 0 : return 0;
599 : }
600 :
601 : //And now do a motor scan
602 0 : rv = m_telnetConn.write("MC\r\n", m_writeTimeout);
603 0 : if(rv != TTY_E_NOERROR)
604 : {
605 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
606 0 : log<software_error>({__FILE__, __LINE__, tty::ttyErrorString(rv)});
607 0 : state(stateCodes::ERROR);
608 0 : return 0;
609 : }
610 :
611 0 : sleep(2); //Give time for controller scan to finish
612 :
613 0 : for(size_t n = 0; n < m_addresses.size(); ++n)
614 : {
615 0 : if(m_addresses[n] == 1) continue; //already done.
616 :
617 0 : std::string addprefix = std::to_string(m_addresses[n]) + ">";
618 :
619 0 : int rv = m_telnetConn.write(addprefix + "*IDN?\r\n", m_writeTimeout);
620 0 : if(rv != TTY_E_NOERROR)
621 : {
622 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
623 0 : log<software_error>({__FILE__, __LINE__, tty::ttyErrorString(rv)});
624 0 : state(stateCodes::ERROR);
625 0 : return 0;
626 : }
627 :
628 0 : rv = m_telnetConn.read("\r\n", m_readTimeout, true);
629 0 : if(rv != TTY_E_NOERROR)
630 : {
631 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
632 0 : log<software_error>({__FILE__, __LINE__, tty::ttyErrorString(rv)});
633 0 : state(stateCodes::ERROR);
634 0 : return 0;
635 : }
636 :
637 : int add;
638 0 : std::string resp;
639 :
640 0 : rv = splitResponse(add, resp, m_telnetConn.m_strRead);
641 0 : if(rv != 0)
642 : {
643 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
644 0 : log<software_error>({__FILE__, __LINE__, "splitResponse returned " + std::to_string(rv)});
645 0 : state(stateCodes::ERROR);
646 0 : return 0;
647 : }
648 :
649 0 : if(add != m_addresses[n])
650 : {
651 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
652 0 : log<software_error>({__FILE__, __LINE__, "address did not match in response"});
653 0 : state(stateCodes::ERROR);
654 0 : return 0;
655 : }
656 :
657 0 : if(resp.find("New_Focus") != std::string::npos)
658 : {
659 0 : log<text_log>("Connected to " + resp + " at address " + std::to_string(m_addresses[n]));
660 : }
661 : else
662 : {
663 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
664 0 : log<software_error>({__FILE__, __LINE__, "wrong response to IDN query at address " + std::to_string(m_addresses[n])});
665 0 : state(stateCodes::ERROR);
666 0 : return 0;
667 : }
668 :
669 : //Now do a motor scan
670 0 : rv = m_telnetConn.write(addprefix + "MC\r\n", m_writeTimeout);
671 0 : if(rv != TTY_E_NOERROR)
672 : {
673 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
674 0 : log<software_error>({__FILE__, __LINE__, tty::ttyErrorString(rv)});
675 0 : state(stateCodes::ERROR);
676 0 : return 0;
677 : }
678 0 : }
679 :
680 0 : sleep(2); //This is to give time for motor scans to finish
681 :
682 : //Now check for each motor attached
683 0 : for(auto it=m_channels.begin(); it!=m_channels.end();++it)
684 : {
685 0 : std::string query = std::to_string(it->second.m_address) + ">" + std::to_string(it->second.m_channel) + "QM?";
686 :
687 0 : rv = m_telnetConn.write(query + "\r\n", m_writeTimeout);
688 0 : if(rv != TTY_E_NOERROR)
689 : {
690 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
691 0 : log<software_error>({__FILE__, __LINE__, tty::ttyErrorString(rv)});
692 0 : state(stateCodes::ERROR);
693 0 : return 0;
694 : }
695 :
696 0 : rv = m_telnetConn.read("\r\n", m_readTimeout, true);
697 0 : if(rv != TTY_E_NOERROR)
698 : {
699 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
700 0 : log<software_error>({__FILE__, __LINE__, tty::ttyErrorString(rv)});
701 0 : state(stateCodes::ERROR);
702 0 : return 0;
703 : }
704 :
705 : int add;
706 0 : std::string resp;
707 :
708 0 : rv = splitResponse(add, resp, m_telnetConn.m_strRead);
709 0 : if(rv != 0)
710 : {
711 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
712 0 : log<software_error>({__FILE__, __LINE__, "splitResponse returned " + std::to_string(rv)});
713 0 : state(stateCodes::ERROR);
714 0 : return 0;
715 : }
716 :
717 0 : if(add != it->second.m_address)
718 : {
719 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
720 0 : log<software_error>({__FILE__, __LINE__, "address did not match in response"});
721 0 : state(stateCodes::ERROR);
722 0 : return 0;
723 : }
724 :
725 0 : int moType = std::stoi(resp);
726 0 : if(moType == 0)
727 : {
728 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
729 0 : log<text_log>("No motor connected on channel " + std::to_string(it->second.m_address) + "." + std::to_string(it->second.m_channel) + " [" + it->second.m_name + "]", logPrio::LOG_CRITICAL);
730 0 : state(stateCodes::FAILURE);
731 0 : return -1;
732 : }
733 0 : else if (moType != it->second.m_type)
734 : {
735 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
736 0 : std::string msg = "Wrong motor type connected on channel " + std::to_string(it->second.m_address) + ".";
737 0 : msg += std::to_string(it->second.m_channel) + " [" + it->second.m_name + "] ";
738 0 : msg += "expected " + std::to_string(it->second.m_type) + " ";
739 0 : msg += "got " + std::to_string(moType);
740 :
741 0 : log<text_log>(msg, logPrio::LOG_CRITICAL);
742 0 : state(stateCodes::FAILURE);
743 0 : return -1;
744 0 : }
745 0 : }
746 :
747 0 : state(stateCodes::READY);
748 :
749 0 : return 0;
750 0 : }
751 :
752 0 : if(state() == stateCodes::READY || state() == stateCodes::OPERATING)
753 : {
754 : //check connection
755 : {
756 0 : std::unique_lock<std::mutex> lock(m_telnetMutex);
757 :
758 0 : int rv = m_telnetConn.write("*IDN?\r\n", m_writeTimeout);
759 0 : if(rv != TTY_E_NOERROR)
760 : {
761 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
762 0 : log<software_error>({__FILE__, __LINE__, tty::ttyErrorString(rv)});
763 0 : state(stateCodes::ERROR);
764 0 : return 0;
765 : }
766 :
767 0 : rv = m_telnetConn.read("\r\n", m_readTimeout, true);
768 0 : if(rv != TTY_E_NOERROR)
769 : {
770 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
771 0 : log<software_error>({__FILE__, __LINE__, tty::ttyErrorString(rv)});
772 0 : state(stateCodes::ERROR);
773 0 : return 0;
774 : }
775 :
776 0 : if(m_telnetConn.m_strRead.find("New_Focus") == std::string::npos)
777 : {
778 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
779 :
780 0 : log<software_error>({__FILE__, __LINE__, "wrong response to IDN query"});
781 0 : state(stateCodes::ERROR);
782 0 : return 0;
783 : }
784 0 : }
785 :
786 : //Now check state of motors
787 0 : bool anymoving = false;
788 :
789 : //This is where we'd check for moving
790 0 : for(channelMapT::iterator it = m_channels.begin(); it != m_channels.end(); ++ it)
791 : {
792 0 : std::unique_lock<std::mutex> lock(m_telnetMutex);
793 :
794 0 : std::string query = std::to_string(it->second.m_address) + ">" + std::to_string(it->second.m_channel) + "MD?";
795 :
796 0 : int rv = m_telnetConn.write(query + "\r\n", m_writeTimeout);
797 0 : if(rv != TTY_E_NOERROR)
798 : {
799 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
800 0 : log<software_error>({__FILE__, __LINE__, tty::ttyErrorString(rv)});
801 0 : state(stateCodes::ERROR);
802 0 : return 0;
803 : }
804 :
805 0 : rv = m_telnetConn.read("\r\n", m_readTimeout, true);
806 0 : if(rv != TTY_E_NOERROR)
807 : {
808 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
809 0 : log<software_error>({__FILE__, __LINE__, tty::ttyErrorString(rv)});
810 0 : state(stateCodes::ERROR);
811 0 : return 0;
812 : }
813 :
814 : int add;
815 0 : std::string resp;
816 :
817 0 : rv = splitResponse(add, resp, m_telnetConn.m_strRead);
818 0 : if(rv != 0)
819 : {
820 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
821 0 : log<software_error>({__FILE__, __LINE__, "splitResponse returned " + std::to_string(rv)});
822 0 : state(stateCodes::ERROR);
823 0 : return 0;
824 : }
825 :
826 0 : if(add != it->second.m_address)
827 : {
828 0 : if(powerState() != 1 || powerStateTarget() != 1) return 0;
829 0 : log<software_error>({__FILE__, __LINE__, "address did not match in response"});
830 0 : state(stateCodes::ERROR);
831 0 : return 0;
832 : }
833 :
834 : //The check for moving here. With power off detection
835 0 : if(std::stoi(resp) == 0)
836 : {
837 0 : anymoving = true;
838 0 : it->second.m_moving = true;
839 : }
840 : else
841 : {
842 0 : it->second.m_moving = false;
843 : }
844 :
845 0 : if(it->second.m_moving == false && it->second.m_doMove == true)
846 : {
847 0 : it->second.m_currCounts = it->second.m_property["target"].get<long>();
848 0 : log<text_log>("moved " + it->second.m_name + " to " + std::to_string(it->second.m_currCounts) + " counts");
849 0 : it->second.m_doMove = false;
850 0 : recordPico(true);
851 : }
852 0 : }
853 :
854 0 : if(anymoving == false) state(stateCodes::READY);
855 0 : else state(stateCodes::OPERATING);
856 :
857 0 : for(channelMapT::iterator it = m_channels.begin(); it != m_channels.end(); ++ it)
858 : {
859 0 : std::unique_lock<std::mutex> lock(m_indiMutex);
860 0 : if(it->second.m_moving) updateIfChanged(it->second.m_property, "current", it->second.m_currCounts, INDI_BUSY);
861 0 : else updateIfChanged(it->second.m_property, "current", it->second.m_currCounts, INDI_IDLE);
862 :
863 0 : for(size_t n=0; n < it->second.m_presetNames.size(); ++n)
864 : {
865 0 : bool changed = false;
866 0 : if( it->second.m_currCounts == it->second.m_presetPositions[n])
867 : {
868 0 : if(it->second.m_indiP_presetName[it->second.m_presetNames[n]] == pcf::IndiElement::Off) changed = true;
869 0 : it->second.m_indiP_presetName[it->second.m_presetNames[n]] = pcf::IndiElement::On;
870 : }
871 : else
872 : {
873 0 : if(it->second.m_indiP_presetName[it->second.m_presetNames[n]] == pcf::IndiElement::On) changed = true;
874 0 : it->second.m_indiP_presetName[it->second.m_presetNames[n]] = pcf::IndiElement::Off;
875 : }
876 :
877 0 : if(changed) m_indiDriver->sendSetProperty(it->second.m_indiP_presetName);
878 : }
879 :
880 0 : if(writeChannelCounts(it->second.m_name, it->second.m_currCounts) < 0)
881 : {
882 0 : log<software_error>({__FILE__, __LINE__});
883 : }
884 0 : }
885 :
886 0 : TELEMETER_APP_LOGIC;
887 :
888 0 : return 0;
889 : }
890 :
891 :
892 0 : return 0;
893 : }
894 :
895 0 : int picoMotorCtrl::onPowerOff()
896 : {
897 0 : return 0;
898 : }
899 :
900 0 : int picoMotorCtrl::whilePowerOff()
901 : {
902 0 : return 0;
903 : }
904 :
905 0 : int picoMotorCtrl::appShutdown()
906 : {
907 : //Shutdown and join the threads
908 0 : for(channelMapT::iterator it = m_channels.begin(); it != m_channels.end(); ++ it)
909 : {
910 0 : if(it->second.m_thread->joinable())
911 : {
912 0 : pthread_kill(it->second.m_thread->native_handle(), SIGUSR1);
913 : try
914 : {
915 0 : it->second.m_thread->join(); //this will throw if it was already joined
916 : }
917 0 : catch(...)
918 : {
919 0 : }
920 : }
921 : }
922 :
923 0 : TELEMETER_APP_SHUTDOWN;
924 :
925 0 : return 0;
926 : }
927 :
928 0 : picoMotorCtrl::posT picoMotorCtrl::readChannelCounts(const std::string & chName)
929 : {
930 0 : std::string statusDir = m_sysPath;
931 0 : statusDir += "/";
932 0 : statusDir += m_configName;
933 :
934 0 : std::string fileName = statusDir + "/" + chName;
935 :
936 0 : std::ifstream posIn;
937 0 : posIn.open( fileName );
938 :
939 0 : if(!posIn.good())
940 : {
941 0 : log<text_log>("no position file for " + chName + " found. initializing to 0.");
942 0 : return 0;
943 : }
944 :
945 : long pos;
946 0 : posIn >> pos;
947 :
948 0 : posIn.close();
949 :
950 0 : log<text_log>("initializing " + chName + " to " + std::to_string(pos));
951 :
952 0 : return pos;
953 0 : }
954 :
955 0 : int picoMotorCtrl::writeChannelCounts( const std::string & chName,
956 : posT counts
957 : )
958 : {
959 0 : std::string statusDir = m_sysPath;
960 0 : statusDir += "/";
961 0 : statusDir += m_configName;
962 :
963 0 : std::string fileName = statusDir + "/" + chName;
964 :
965 0 : elevatedPrivileges ep(this);
966 :
967 0 : std::ofstream posOut;
968 0 : posOut.open( fileName );
969 :
970 0 : if(!posOut.good())
971 : {
972 0 : log<text_log>("could not open counts file for " + chName + " -- can not store position.", logPrio::LOG_ERROR);
973 0 : return -1;
974 : }
975 :
976 0 : posOut << counts;
977 :
978 0 : posOut.close();
979 :
980 0 : return 0;
981 0 : }
982 :
983 0 : void picoMotorCtrl::channelThreadStart( motorChannel * mc )
984 : {
985 0 : mc->m_parent->channelThreadExec(mc);
986 0 : }
987 :
988 0 : void picoMotorCtrl::channelThreadExec( motorChannel * mc)
989 : {
990 : //Get the thread PID immediately so the caller can return.
991 0 : mc->m_threadID = syscall(SYS_gettid);
992 :
993 : //Wait for initialization to complete.
994 0 : while( mc->m_threadInit == true && m_shutdown == 0)
995 : {
996 0 : sleep(1);
997 : }
998 :
999 : //Now begin checking for state change request.
1000 0 : while(!m_shutdown)
1001 : {
1002 : //If told to move and not moving, start a move
1003 0 : if(mc->m_doMove && !mc->m_moving && (state() == stateCodes::READY || state() == stateCodes::OPERATING))
1004 : {
1005 0 : long dr = mc->m_property["target"].get<long>() - mc->m_currCounts;
1006 :
1007 0 : recordPico(true);
1008 0 : std::unique_lock<std::mutex> lock(m_telnetMutex);
1009 0 : state(stateCodes::OPERATING);
1010 0 : mc->m_moving = true;
1011 0 : log<text_log>("moving " + mc->m_name + " by " + std::to_string(dr) + " counts");
1012 :
1013 0 : std::string comm = std::to_string(mc->m_address) + ">" + std::to_string(mc->m_channel) + "PR" + std::to_string(dr);
1014 :
1015 0 : int rv = m_telnetConn.write(comm + "\r\n", m_writeTimeout);
1016 0 : if(rv != TTY_E_NOERROR)
1017 : {
1018 0 : if(powerState() != 1 || powerStateTarget() != 1) //about to get POWEROFF
1019 : {
1020 0 : sleep(1);
1021 0 : continue;
1022 : }
1023 0 : log<software_error>({__FILE__, __LINE__, tty::ttyErrorString(rv)});
1024 0 : state(stateCodes::ERROR);
1025 : }
1026 0 : }
1027 0 : else if( !(state() == stateCodes::READY || state() == stateCodes::OPERATING))
1028 : {
1029 0 : mc->m_doMove = false; //In case a move is requested when not able to move
1030 : }
1031 :
1032 0 : sleep(1);
1033 : }
1034 :
1035 :
1036 0 : }
1037 :
1038 :
1039 0 : int picoMotorCtrl::st_newCallBack_picopos( void * app,
1040 : const pcf::IndiProperty &ipRecv
1041 : )
1042 : {
1043 0 : return static_cast<picoMotorCtrl*>(app)->newCallBack_picopos(ipRecv);
1044 : }
1045 :
1046 0 : int picoMotorCtrl::newCallBack_picopos( const pcf::IndiProperty &ipRecv )
1047 : {
1048 :
1049 : //Search for the channel
1050 0 : std::string propName = ipRecv.getName();
1051 0 : size_t nend = propName.rfind("_pos");
1052 :
1053 0 : if(nend == std::string::npos)
1054 : {
1055 0 : log<software_error>({__FILE__, __LINE__, "Channel without _pos received"});
1056 0 : return -1;
1057 : }
1058 :
1059 0 : std::string chName = propName.substr(0, nend);
1060 0 : channelMapT::iterator it = m_channels.find(chName);
1061 :
1062 0 : if(it == m_channels.end())
1063 : {
1064 0 : log<software_error>({__FILE__, __LINE__, "Unknown channel name received"});
1065 0 : return -1;
1066 : }
1067 :
1068 0 : if(it->second.m_doMove == true)
1069 : {
1070 0 : log<text_log>("channel " + it->second.m_name + " is already moving", logPrio::LOG_WARNING);
1071 0 : return 0;
1072 : }
1073 :
1074 : //Set the target element, and the doMove flag, and then signal the thread.
1075 : {//scope for mutex
1076 0 : std::unique_lock<std::mutex> lock(m_indiMutex);
1077 :
1078 : long counts; //not actually used
1079 0 : if(indiTargetUpdate( it->second.m_property, counts, ipRecv, true) < 0)
1080 : {
1081 0 : return log<software_error,-1>({__FILE__,__LINE__});
1082 : }
1083 0 : }
1084 :
1085 0 : it->second.m_doMove= true;
1086 :
1087 0 : pthread_kill(it->second.m_thread->native_handle(), SIGUSR1);
1088 :
1089 0 : return 0;
1090 0 : }
1091 :
1092 0 : int picoMotorCtrl::st_newCallBack_presetName( void * app,
1093 : const pcf::IndiProperty &ipRecv
1094 : )
1095 : {
1096 0 : return static_cast<picoMotorCtrl*>(app)->newCallBack_presetName (ipRecv);
1097 : }
1098 :
1099 0 : int picoMotorCtrl::newCallBack_presetName( const pcf::IndiProperty &ipRecv )
1100 : {
1101 0 : channelMapT::iterator it = m_channels.find(ipRecv.getName());
1102 :
1103 0 : if(it == m_channels.end())
1104 : {
1105 0 : log<software_error>({__FILE__, __LINE__, "Unknown channel name received"});
1106 0 : return -1;
1107 : }
1108 :
1109 0 : if(it->second.m_doMove == true)
1110 : {
1111 0 : log<text_log>("channel " + it->second.m_name + " is already moving", logPrio::LOG_WARNING);
1112 0 : return 0;
1113 : }
1114 :
1115 0 : long counts = -1e10;
1116 :
1117 : size_t i;
1118 0 : for(i=0; i< it->second.m_presetNames.size(); ++i)
1119 : {
1120 0 : if(!ipRecv.find(it->second.m_presetNames[i])) continue;
1121 :
1122 0 : if(ipRecv[it->second.m_presetNames[i]].getSwitchState() == pcf::IndiElement::On)
1123 : {
1124 0 : if(counts != -1e10)
1125 : {
1126 0 : log<text_log>("More than one preset selected", logPrio::LOG_ERROR);
1127 0 : return -1;
1128 : }
1129 :
1130 0 : counts = it->second.m_presetPositions[i];
1131 0 : std::cerr << "selected: " << it->second.m_presetNames[i] << " " << counts << "\n";
1132 : }
1133 : }
1134 :
1135 : //Set the target element, and the doMove flag, and then signal the thread.
1136 : {//scope for mutex
1137 0 : std::unique_lock<std::mutex> lock(m_indiMutex);
1138 :
1139 0 : it->second.m_property["target"].set(counts);
1140 0 : }
1141 :
1142 0 : it->second.m_doMove= true;
1143 :
1144 0 : pthread_kill(it->second.m_thread->native_handle(), SIGUSR1);
1145 :
1146 0 : return 0;
1147 : }
1148 :
1149 0 : int picoMotorCtrl::checkRecordTimes()
1150 : {
1151 0 : return telemeterT::checkRecordTimes(telem_pico());
1152 : }
1153 :
1154 0 : int picoMotorCtrl::recordTelem( const telem_pico * )
1155 : {
1156 0 : return recordPico(true);
1157 : }
1158 :
1159 0 : int picoMotorCtrl::recordPico( bool force )
1160 : {
1161 0 : static std::vector<int64_t> lastpos(m_nChannels, std::numeric_limits<long>::max());
1162 :
1163 0 : bool changed = false;
1164 0 : for(channelMapT::iterator it = m_channels.begin(); it != m_channels.end(); ++ it)
1165 : {
1166 0 : if(it->second.m_currCounts != lastpos[it->second.m_channel-1]) changed = true;
1167 : }
1168 :
1169 0 : if( changed || force )
1170 : {
1171 0 : for(channelMapT::iterator it = m_channels.begin(); it != m_channels.end(); ++ it)
1172 : {
1173 0 : lastpos[it->second.m_channel-1] = it->second.m_currCounts;
1174 : }
1175 :
1176 0 : telem<telem_pico>(lastpos);
1177 : }
1178 :
1179 0 : return 0;
1180 : }
1181 :
1182 : } //namespace app
1183 : } //namespace MagAOX
1184 :
1185 : #endif //picoMotorCtrl_hpp
|
