Debian Codegraph @

ExosIIMountControl.hpp

raw
1 /*
2 * ExosIIMountControl.hpp
3 *
4 * Copyright 2020 Kevin Krüger <[email protected]>
5 *
6 * This program is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 *
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with this program; if not, write to the Free Software
18 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
19 * MA 02110-1301, USA.
20 *
21 *
22 */
23
24 #pragma once
25
26 #include <cstdint>
27 #include <vector>
28 #include <string>
29 #include <limits>
30 #include <mutex>
31 #include <condition_variable>
32 #include <thread>
33 #include "config.h"
34
35 #include "StateMachine.hpp"
36 #include "CriticalData.hpp"
37 #include "SerialCommand.hpp"
38 #include "SerialCommandTransceiver.hpp"
39 #include "INotifyPointingCoordinatesReceived.hpp"
40
41 //The manual states a tracking speed for 0.004°/s everything above is
42 //considered slewing.
43 #define TRACK_SLEW_THRESHOLD (0.0045)
44
45 #define EXPR_TO_STRING(x) #x
46
47 namespace TelescopeMountControl
48 {
49 //enum representing the telescope mount state
50 enum TelescopeMountState
51 {
52 //initial state, no serial connection established
53 Disconnected = 0,
54 //initial state if the serial connection was established,
55 //the mount did not report any pointing coordinates yet.
56 //if an error occurs, the telescope also will be in this state.
57 Unknown = 1,
58 //The driver received at least one response from the mount,
59 //indicating the communication is working, and the mount accepts commands.
60 Connected = 2,
61 //If the user issues the park command the telescope likely needs to slew to the parking position,
62 //this is determined by the differentials of the positions information send by the controller.
63 ParkingIssued = 3,
64 //If the status report messages arrive and the telescope is not moving, this is the assumed state.
65 //the telescope is assumed in park/initial position according to manual
66 //or if the "park" command is issued.
67 Parked = 4,
68 //The controller autonomously desides the motion speeds, and does not report any "state".
69 //This state is assumed if the motion speeds exceed a certain threshold.
70 Slewing = 5,
71 //This state is assumed if the telescope is moves below the slewing threshold.
72 //its also the default when a "goto" is issued, since the telescope controller automatically tracks the issued coordinates.
73 Tracking = 6,
74 //when tracking an object, move to a direction.
75 MoveWhileTracking = 7,
76 //This state is reached when issuing the stop command while slewing or tracking.
77 Idle = 8,
78 //The error state of the telescope any none defined transition will reset to this state.
79 FailSafe = 9,
80 };
81
82 //Enum reqresenting the virtuals signal the telescope issues.
83 enum TelescopeSignals
84 {
85 //connect to the telescope.
86 //need to be exclusive from states so there is no hazzling with communtativity of the xor operator of hashing functions.
87 Connect = TelescopeMountState::FailSafe + 1,
88 //disconnect from the telescope.
89 Disconnect,
90 //request the geolocation initially.
91 RequestedGeoLocationReceived,
92 //when the geolocation is requested this signal gets issued if the controller reports the pointing coordinates.
93 InitialPointingCoordinatesReceived,
94 //stop any motion of the telescope.
95 Stop,
96 //goto a position and track the position when reached.
97 GoTo,
98 //park the telescope.
99 Park,
100 //issued when the parking position is reached.
101 ParkingPositionReached,
102 //motion above tracking threshold.
103 Slew,
104 //motion below tracking threshold.
105 Track,
106 //motion to tracking target, when reached this is signaled.
107 TrackingTargetReached,
108 //move a certain direction while tracking.
109 StartMotion,
110 //stop moving in a certain direction while tracking.
111 StopMotion,
112 //used as a token to represent an initialized yet invalid signal.
113 INVALID,
114 };
115
116 //type definition of the state machine type for convinience.
117 typedef StateMachine<TelescopeMountState, TelescopeSignals, IStateNotification<TelescopeMountState, TelescopeSignals>>
118 MountStateMachine;
119
120 //store the motion state while tracking.
121 struct MotionState
122 {
123 //move in what direction
124 SerialDeviceControl::SerialCommandID MotionDirection;
125 //how many messages per second
126 uint16_t CommandsPerSecond;
127 };
128
129 //These types have to inherit/implement:
130 //-The ISerialInterface.hpp as Interface type.
131 template<class InterfaceType>
132 class ExosIIMountControl :
133 public SerialDeviceControl::SerialCommandTransceiver<InterfaceType, TelescopeMountControl::ExosIIMountControl<InterfaceType>>,
134 public SerialDeviceControl::INotifyPointingCoordinatesReceived,
135 public IStateNotification<TelescopeMountState, TelescopeSignals>
136 {
137 public:
138 //create a exos controller using a reference of a particular serial implementation.
139 ExosIIMountControl(InterfaceType &interfaceImplementation) :
140 SerialDeviceControl::SerialCommandTransceiver<InterfaceType, TelescopeMountControl::ExosIIMountControl<InterfaceType>>
141 (interfaceImplementation, *this),
142 mIsMotionControlThreadRunning(false),
143 mIsMotionControlRunning(false),
144 mMountStateMachine(*this, TelescopeMountState::Disconnected, TelescopeMountState::FailSafe)
145 {
146 SerialDeviceControl::EquatorialCoordinates initialCoordinates;
147 initialCoordinates.RightAscension = std::numeric_limits<float>::quiet_NaN();
148 initialCoordinates.Declination = std::numeric_limits<float>::quiet_NaN();
149
150 mCurrentPointingCoordinates.Set(initialCoordinates);
151 mSiteLocationCoordinates.Set(initialCoordinates);
152
153 MotionState initialState;
154 initialState.MotionDirection = SerialDeviceControl::SerialCommandID::NULL_COMMAND_ID;
155 initialState.CommandsPerSecond = 0;
156
157 mMotionState.Set(initialState);
158
159 //initialize statemachine:
160 mMountStateMachine.AddFinalState(TelescopeMountState::Disconnected);
161
162 //build transition table:
163 mMountStateMachine.AddTransition(TelescopeMountState::Disconnected, TelescopeSignals::Disconnect,
164 TelescopeMountState::Disconnected);
165 mMountStateMachine.AddTransition(TelescopeMountState::Disconnected, TelescopeSignals::Connect,
166 TelescopeMountState::Unknown);
167
168 mMountStateMachine.AddTransition(TelescopeMountState::Unknown, TelescopeSignals::Disconnect,
169 TelescopeMountState::Disconnected);
170 mMountStateMachine.AddTransition(TelescopeMountState::Unknown, TelescopeSignals::InitialPointingCoordinatesReceived,
171 TelescopeMountState::Connected);
172 mMountStateMachine.AddTransition(TelescopeMountState::Unknown, TelescopeSignals::RequestedGeoLocationReceived,
173 TelescopeMountState::Connected);
174
175 mMountStateMachine.AddTransition(TelescopeMountState::Connected, TelescopeSignals::Connect, TelescopeMountState::Connected);
176 mMountStateMachine.AddTransition(TelescopeMountState::Connected, TelescopeSignals::RequestedGeoLocationReceived,
177 TelescopeMountState::Parked);
178 mMountStateMachine.AddTransition(TelescopeMountState::Connected, TelescopeSignals::InitialPointingCoordinatesReceived,
179 TelescopeMountState::Parked);
180 mMountStateMachine.AddTransition(TelescopeMountState::Connected, TelescopeSignals::Track, TelescopeMountState::Tracking);
181 mMountStateMachine.AddTransition(TelescopeMountState::Connected, TelescopeSignals::Slew, TelescopeMountState::Slewing);
182 mMountStateMachine.AddTransition(TelescopeMountState::Connected, TelescopeSignals::Disconnect,
183 TelescopeMountState::Disconnected);
184
185 mMountStateMachine.AddTransition(TelescopeMountState::Parked, TelescopeSignals::Park, TelescopeMountState::Parked);
186 mMountStateMachine.AddTransition(TelescopeMountState::Parked, TelescopeSignals::Disconnect,
187 TelescopeMountState::Disconnected);
188 mMountStateMachine.AddTransition(TelescopeMountState::Parked, TelescopeSignals::Stop, TelescopeMountState::Parked);
189 mMountStateMachine.AddTransition(TelescopeMountState::Parked, TelescopeSignals::GoTo, TelescopeMountState::Slewing);
190
191 mMountStateMachine.AddTransition(TelescopeMountState::Idle, TelescopeSignals::Stop, TelescopeMountState::Idle);
192 mMountStateMachine.AddTransition(TelescopeMountState::Idle, TelescopeSignals::GoTo, TelescopeMountState::Slewing);
193 mMountStateMachine.AddTransition(TelescopeMountState::Idle, TelescopeSignals::Park, TelescopeMountState::ParkingIssued);
194 mMountStateMachine.AddTransition(TelescopeMountState::Idle, TelescopeSignals::Disconnect,
195 TelescopeMountState::Disconnected);
196
197 mMountStateMachine.AddTransition(TelescopeMountState::ParkingIssued, TelescopeSignals::Park,
198 TelescopeMountState::ParkingIssued);
199 mMountStateMachine.AddTransition(TelescopeMountState::ParkingIssued, TelescopeSignals::Slew,
200 TelescopeMountState::ParkingIssued);
201 mMountStateMachine.AddTransition(TelescopeMountState::ParkingIssued, TelescopeSignals::Track,
202 TelescopeMountState::ParkingIssued);
203 mMountStateMachine.AddTransition(TelescopeMountState::ParkingIssued, TelescopeSignals::ParkingPositionReached,
204 TelescopeMountState::Parked);
205 mMountStateMachine.AddTransition(TelescopeMountState::ParkingIssued, TelescopeSignals::Stop, TelescopeMountState::Idle);
206 mMountStateMachine.AddTransition(TelescopeMountState::ParkingIssued, TelescopeSignals::Disconnect,
207 TelescopeMountState::Disconnected);
208
209 mMountStateMachine.AddTransition(TelescopeMountState::Slewing, TelescopeSignals::Stop, TelescopeMountState::Idle);
210 mMountStateMachine.AddTransition(TelescopeMountState::Slewing, TelescopeSignals::GoTo, TelescopeMountState::Slewing);
211 mMountStateMachine.AddTransition(TelescopeMountState::Slewing, TelescopeSignals::Track, TelescopeMountState::Tracking);
212 mMountStateMachine.AddTransition(TelescopeMountState::Slewing, TelescopeSignals::Slew, TelescopeMountState::Slewing);
213 mMountStateMachine.AddTransition(TelescopeMountState::Slewing, TelescopeSignals::Park, TelescopeMountState::ParkingIssued);
214 mMountStateMachine.AddTransition(TelescopeMountState::Slewing, TelescopeSignals::Disconnect,
215 TelescopeMountState::Disconnected);
216
217 mMountStateMachine.AddTransition(TelescopeMountState::Tracking, TelescopeSignals::Track, TelescopeMountState::Tracking);
218 mMountStateMachine.AddTransition(TelescopeMountState::Tracking, TelescopeSignals::Slew, TelescopeMountState::Slewing);
219 mMountStateMachine.AddTransition(TelescopeMountState::Tracking, TelescopeSignals::GoTo, TelescopeMountState::Slewing);
220 mMountStateMachine.AddTransition(TelescopeMountState::Tracking, TelescopeSignals::Stop, TelescopeMountState::Idle);
221 mMountStateMachine.AddTransition(TelescopeMountState::Tracking, TelescopeSignals::StartMotion,
222 TelescopeMountState::MoveWhileTracking);
223 mMountStateMachine.AddTransition(TelescopeMountState::Tracking, TelescopeSignals::Park, TelescopeMountState::ParkingIssued);
224 mMountStateMachine.AddTransition(TelescopeMountState::Tracking, TelescopeSignals::Disconnect,
225 TelescopeMountState::Disconnected);
226
227 mMountStateMachine.AddTransition(TelescopeMountState::MoveWhileTracking, TelescopeSignals::StopMotion,
228 TelescopeMountState::Tracking);
229 mMountStateMachine.AddTransition(TelescopeMountState::MoveWhileTracking, TelescopeSignals::StartMotion,
230 TelescopeMountState::MoveWhileTracking);
231 mMountStateMachine.AddTransition(TelescopeMountState::MoveWhileTracking, TelescopeSignals::Stop, TelescopeMountState::Idle);
232 mMountStateMachine.AddTransition(TelescopeMountState::MoveWhileTracking, TelescopeSignals::StopMotion,
233 TelescopeMountState::Tracking);
234 mMountStateMachine.AddTransition(TelescopeMountState::MoveWhileTracking, TelescopeSignals::GoTo,
235 TelescopeMountState::Slewing);
236 mMountStateMachine.AddTransition(TelescopeMountState::MoveWhileTracking, TelescopeSignals::Track,
237 TelescopeMountState::MoveWhileTracking);
238 mMountStateMachine.AddTransition(TelescopeMountState::MoveWhileTracking, TelescopeSignals::Slew,
239 TelescopeMountState::MoveWhileTracking);
240 mMountStateMachine.AddTransition(TelescopeMountState::MoveWhileTracking, TelescopeSignals::Park,
241 TelescopeMountState::ParkingIssued);
242 mMountStateMachine.AddTransition(TelescopeMountState::MoveWhileTracking, TelescopeSignals::Disconnect,
243 TelescopeMountState::Disconnected);
244
245 mMountStateMachine.Reset();
246 }
247
248 virtual ~ExosIIMountControl()
249 {
250 SerialDeviceControl::SerialCommandTransceiver<InterfaceType, TelescopeMountControl::ExosIIMountControl<InterfaceType>>::Stop();
251 }
252
253 //open the serial connection and start the serial reporting.
254 virtual bool Start()
255 {
256 mMountStateMachine.Reset();
257
258 mMountStateMachine.DoTransition(TelescopeSignals::Connect);
259
260 SerialDeviceControl::SerialCommandTransceiver<InterfaceType, TelescopeMountControl::ExosIIMountControl<InterfaceType>>::Start();
261
262 mMotionCommandThread = std::thread(&ExosIIMountControl<InterfaceType>::MotionControlThreadFunction, this);
263
264 return true;
265 }
266
267 //Stop the serial reporting and close the serial port.
268 virtual bool Stop()
269 {
270 if(mIsMotionControlThreadRunning.Get())
271 {
272 mIsMotionControlThreadRunning.Set(false);
273 StopMotionToDirection();
274 mMotionCommandThread.join();
275 }
276
277 bool rc = DisconnectSerial();
278
279 rc |= SerialDeviceControl::SerialCommandTransceiver<InterfaceType, TelescopeMountControl::ExosIIMountControl<InterfaceType>>::Stop();
280
281 return rc;
282 }
283
284 bool StartMotionToDirection(
285 SerialDeviceControl::SerialCommandID direction,
286 uint16_t commandsPerSecond
287 )
288 {
289 //this only works while tracking a target
290 {
291 std::lock_guard<std::mutex> notifyLock(mMotionCommandControlMutex);
292
293 MotionState initialState;
294 initialState.MotionDirection = direction;
295 initialState.CommandsPerSecond = commandsPerSecond;
296
297 mMotionState.Set(initialState);
298
299 mIsMotionControlRunning.Set(true);
300 }
301
302 mMotionControlCondition.notify_all();
303
304 return mMountStateMachine.DoTransition(TelescopeSignals::StartMotion);
305 }
306
307 bool StopMotionToDirection()
308 {
309 //this changes back to tracking
310 MotionState stopState;
311 stopState.MotionDirection = SerialDeviceControl::SerialCommandID::NULL_COMMAND_ID;
312 stopState.CommandsPerSecond = 0;
313
314 mMotionState.Set(stopState);
315
316 mIsMotionControlRunning.Set(false);
317
318 mMotionControlCondition.notify_all();
319
320 if(mMountStateMachine.CurrentState() != TelescopeMountState::MoveWhileTracking)
321 {
322 std::cerr << "motion already disabled." << std::endl;
323 return true;
324 }
325 else
326 {
327 return mMountStateMachine.DoTransition(TelescopeSignals::StopMotion);
328 }
329 }
330
331 //Disconnect from the mount.
332 bool DisconnectSerial()
333 {
334 std::vector<uint8_t> messageBuffer;
335 if(SerialDeviceControl::SerialCommand::GetDisconnectCommandMessage(messageBuffer))
336 {
337 bool rc = SerialDeviceControl::SerialCommandTransceiver<InterfaceType, TelescopeMountControl::ExosIIMountControl<InterfaceType>>::SendMessageBuffer(
338 &messageBuffer[0], 0, messageBuffer.size());
339
340 return rc && mMountStateMachine.DoTransition(TelescopeSignals::Disconnect);
341 }
342 else
343 {
344 std::cerr << "DisconnectSerial: Failed!" << std::endl;
345 return false;
346 }
347 }
348
349 //stop any motion of the telescope.
350 bool StopMotion()
351 {
352 std::vector<uint8_t> messageBuffer;
353 if(SerialDeviceControl::SerialCommand::GetStopMotionCommandMessage(messageBuffer))
354 {
355 bool rc = SerialDeviceControl::SerialCommandTransceiver<InterfaceType, TelescopeMountControl::ExosIIMountControl<InterfaceType>>::SendMessageBuffer(
356 &messageBuffer[0], 0, messageBuffer.size());
357
358 return rc && mMountStateMachine.DoTransition(TelescopeSignals::Stop);
359 }
360 else
361 {
362 std::cerr << "StopMotion: Failed!" << std::endl;
363 return false;
364 }
365 }
366
367 //Order to telescope to go to the parking state.
368 bool ParkPosition()
369 {
370
371 std::vector<uint8_t> messageBuffer;
372 if(SerialDeviceControl::SerialCommand::GetParkCommandMessage(messageBuffer))
373 {
374 bool rc = SerialDeviceControl::SerialCommandTransceiver<InterfaceType, TelescopeMountControl::ExosIIMountControl<InterfaceType>>::SendMessageBuffer(
375 &messageBuffer[0], 0, messageBuffer.size());
376
377
378 return rc && mMountStateMachine.DoTransition(TelescopeSignals::Park);
379 }
380 else
381 {
382 std::cerr << "ParkPosition: Failed!" << std::endl;
383 return false;
384 }
385 }
386
387 //GoTo and track the sky position represented by the equatorial coordinates.
388 bool GoTo(
389 float rightAscension,
390 float declination
391 )
392 {
393 std::vector<uint8_t> messageBuffer;
394 if(SerialDeviceControl::SerialCommand::GetGotoCommandMessage(messageBuffer, rightAscension, declination))
395 {
396 bool rc = SerialDeviceControl::SerialCommandTransceiver<InterfaceType, TelescopeMountControl::ExosIIMountControl<InterfaceType>>::SendMessageBuffer(
397 &messageBuffer[0], 0, messageBuffer.size());
398
399 return rc && mMountStateMachine.DoTransition(TelescopeSignals::GoTo);
400 }
401 else
402 {
403 //TODO: error message
404 std::cerr << "GoTo: Failed!" << std::endl;
405 return false;
406 }
407 }
408
409 //GoTo and track the sky position represented by the equatorial coordinates.
410 bool Sync(
411 float rightAscension,
412 float declination
413 )
414 {
415 std::vector<uint8_t> messageBuffer;
416 if(SerialDeviceControl::SerialCommand::GetSyncCommandMessage(messageBuffer, rightAscension, declination))
417 {
418 std::cerr << "Sent Sync command!" << std::endl;
419 return SerialDeviceControl::SerialCommandTransceiver<InterfaceType, TelescopeMountControl::ExosIIMountControl<InterfaceType>>::SendMessageBuffer(
420 &messageBuffer[0], 0, messageBuffer.size());
421 //return rc && mMountStateMachine.DoTransition(TelescopeSignals::GoTo);
422 }
423 else
424 {
425 //TODO: error message
426 std::cerr << "Sync: Failed!" << std::endl;
427 return false;
428 }
429 }
430
431 //Set the location of the telesope, using decimal latitude and longitude parameters.
432 //This does not change to state of the telescope.
433 bool SetSiteLocation(
434 float latitude,
435 float longitude
436 )
437 {
438 std::vector<uint8_t> messageBuffer;
439 if(SerialDeviceControl::SerialCommand::GetSetSiteLocationCommandMessage(messageBuffer, latitude, longitude))
440 {
441 return SerialDeviceControl::SerialCommandTransceiver<InterfaceType, TelescopeMountControl::ExosIIMountControl<InterfaceType>>::SendMessageBuffer(
442 &messageBuffer[0], 0, messageBuffer.size());
443 }
444 else
445 {
446 std::cerr << "SetSiteLocation: Failed!" << std::endl;
447 return false;
448 }
449 }
450
451 //Set the location of the telesope, using decimal latitude and longitude parameters.
452 //This does not change to state of the telescope.
453 bool RequestSiteLocation()
454 {
455 std::vector<uint8_t> messageBuffer;
456 if(SerialDeviceControl::SerialCommand::GetGetSiteLocationCommandMessage(messageBuffer))
457 {
458 //std::cout << "Message sent!" << std::endl;
459 return SerialDeviceControl::SerialCommandTransceiver<InterfaceType, TelescopeMountControl::ExosIIMountControl<InterfaceType>>::SendMessageBuffer(
460 &messageBuffer[0], 0, messageBuffer.size());
461 }
462 else
463 {
464 //TODO: error message
465 std::cerr << "RequestSiteLocation: Failed!" << std::endl;
466 return false;
467 }
468 }
469
470 //issue the set time command, using date and time parameters. This does not change the state of the telescope.
471 bool SetDateTime(
472 uint16_t year,
473 uint8_t month,
474 uint8_t day,
475 uint8_t hour,
476 uint8_t minute,
477 uint8_t second,
478 int8_t utc_offset
479 )
480 {
481 std::vector<uint8_t> messageBuffer;
482 if(SerialDeviceControl::SerialCommand::GetSetDateTimeCommandMessage(messageBuffer, year, month, day, hour, minute, second, utc_offset))
483 {
484 return SerialDeviceControl::SerialCommandTransceiver<InterfaceType, TelescopeMountControl::ExosIIMountControl<InterfaceType>>::SendMessageBuffer(
485 &messageBuffer[0], 0, messageBuffer.size());
486 }
487 else
488 {
489 //TODO:error message.
490 std::cerr << "SetDateTime: Failed!" << std::endl;
491 return false;
492 }
493 }
494
495 template<SerialDeviceControl::SerialCommandID Direction>
496 bool GuideDirection()
497 {
498 std::vector<uint8_t> messageBuffer;
499
500 if(SerialDeviceControl::SerialCommand::GetMoveWhileTrackingCommandMessage(messageBuffer,Direction))
501 {
502 return SerialDeviceControl::SerialCommandTransceiver<InterfaceType,TelescopeMountControl::ExosIIMountControl<InterfaceType>>::SendMessageBuffer(&messageBuffer[0],0,messageBuffer.size());
503 }
504 else
505 {
506 //TODO:error message.
507 return false;
508 }
509 }
510
511 bool GuideNorth()
512 {
513 return GuideDirection<SerialDeviceControl::SerialCommandID::MOVE_NORTH_COMMAND_ID>();
514 }
515
516 bool GuideSouth()
517 {
518 return GuideDirection<SerialDeviceControl::SerialCommandID::MOVE_SOUTH_COMMAND_ID>();
519 }
520
521 bool GuideEast()
522 {
523 return GuideDirection<SerialDeviceControl::SerialCommandID::MOVE_EAST_COMMAND_ID>();
524 }
525
526 bool GuideWest()
527 {
528 return GuideDirection<SerialDeviceControl::SerialCommandID::MOVE_WEST_COMMAND_ID>();
529 }
530
531 template<SerialDeviceControl::SerialCommandID Direction>
532 bool MoveDirection()
533 {
534 std::vector<uint8_t> messageBuffer;
535 if(SerialDeviceControl::SerialCommand::GetMoveWhileTrackingCommandMessage(messageBuffer, Direction))
536 {
537 bool rc = SerialDeviceControl::SerialCommandTransceiver<InterfaceType, TelescopeMountControl::ExosIIMountControl<InterfaceType>>::SendMessageBuffer(
538 &messageBuffer[0], 0, messageBuffer.size());
539 return rc && mMountStateMachine.DoTransition(TelescopeSignals::StartMotion);
540 }
541 else
542 {
543 //TODO:error message.
544 return false;
545 }
546 }
547
548 bool MoveNorth()
549 {
550 return MoveDirection<SerialDeviceControl::SerialCommandID::MOVE_NORTH_COMMAND_ID>();
551 }
552
553 bool MoveSouth()
554 {
555 return MoveDirection<SerialDeviceControl::SerialCommandID::MOVE_SOUTH_COMMAND_ID>();
556 }
557
558 bool MoveEast()
559 {
560 return MoveDirection<SerialDeviceControl::SerialCommandID::MOVE_EAST_COMMAND_ID>();
561 }
562
563 bool MoveWest()
564 {
565 return MoveDirection<SerialDeviceControl::SerialCommandID::MOVE_WEST_COMMAND_ID>();
566 }
567
568
569 //Called each time a pair of coordinates was received from the serial interface.
570 virtual void OnPointingCoordinatesReceived(
571 float right_ascension,
572 float declination
573 )
574 {
575 //std::cerr << "Received data : RA: " << right_ascension << " DEC:" << declination << std::endl;
576
577 SerialDeviceControl::EquatorialCoordinates lastCoordinates = GetPointingCoordinates();
578
579 SerialDeviceControl::EquatorialCoordinates coordinatesReceived;
580 coordinatesReceived.RightAscension = right_ascension;
581 coordinatesReceived.Declination = declination;
582
583 bool coordinatesNotNan = !std::isnan(right_ascension) && !std::isnan(declination);
584
585 mCurrentPointingCoordinates.Set(coordinatesReceived);
586
587 SerialDeviceControl::EquatorialCoordinates delta = SerialDeviceControl::EquatorialCoordinates::Delta(lastCoordinates,
588 coordinatesReceived);
589 float absDelta = SerialDeviceControl::EquatorialCoordinates::Absolute(delta);
590
591 TelescopeMountState currentState = mMountStateMachine.CurrentState();
592 TelescopeSignals signal = TelescopeSignals::INVALID;
593
594 switch(currentState)
595 {
596 case TelescopeMountState::Unknown:
597 if(coordinatesNotNan)
598 {
599 signal = TelescopeSignals::InitialPointingCoordinatesReceived;
600 }
601 break;
602
603 case TelescopeMountState::Connected:
604 if(std::isnan(absDelta))
605 {
606 break;
607 }
608
609 if(!std::isnan(absDelta) && (absDelta > 0.0f))
610 {
611 //see if the telescope is moving initially -> previous/externally triggered motion.
612 if(absDelta > TRACK_SLEW_THRESHOLD)
613 {
614 signal = TelescopeSignals::Slew;
615 }
616 else
617 {
618 signal = TelescopeSignals::Track;
619 }
620 }
621 else
622 {
623 //assume parked otherwise.
624 signal = TelescopeSignals::InitialPointingCoordinatesReceived;
625 }
626 break;
627
628 case TelescopeMountState::ParkingIssued:
629 if(absDelta > 0.0f)
630 {
631 if(absDelta > TRACK_SLEW_THRESHOLD)
632 {
633 signal = TelescopeSignals::Slew;
634 }
635 else
636 {
637 signal = TelescopeSignals::Track;
638 }
639 }
640 else
641 {
642 signal = TelescopeSignals::ParkingPositionReached;
643 }
644 break;
645
646 case TelescopeMountState::Idle:
647
648 break;
649
650 case TelescopeMountState::Parked:
651
652 break;
653
654 case TelescopeMountState::Tracking:
655 if(absDelta > 0.0f)
656 {
657 //may be externally triggered motion
658 if(absDelta > TRACK_SLEW_THRESHOLD)
659 {
660 signal = TelescopeSignals::Slew;
661 }
662 else
663 {
664 signal = TelescopeSignals::Track;
665 }
666 }
667 break;
668
669 case TelescopeMountState::Slewing:
670 if(absDelta > 0.0f)
671 {
672 //may be externally triggered motion
673 if(absDelta > TRACK_SLEW_THRESHOLD)
674 {
675 signal = TelescopeSignals::Slew;
676 }
677 else
678 {
679 signal = TelescopeSignals::Track;
680 }
681 }
682
683 default:
684
685 break;
686 }
687
688 if(signal != TelescopeSignals::INVALID)
689 {
690 mMountStateMachine.DoTransition(signal);
691 }
692 }
693
694 //Called each time a pair of geo coordinates was received from
695 //the serial inferface. This happends only by active request
696 //(GET_SITE_LOCATION_COMMAND_ID)
697 virtual void OnSiteLocationCoordinatesReceived(
698 float latitude,
699 float longitude
700 )
701 {
702 std::cerr << "Received data : LAT: " << latitude << " LON:" << longitude << std::endl;
703
704 SerialDeviceControl::EquatorialCoordinates coordinatesReceived;
705 coordinatesReceived.RightAscension = latitude;
706 coordinatesReceived.Declination = longitude;
707
708 mSiteLocationCoordinates.Set(coordinatesReceived);
709
710 mMountStateMachine.DoTransition(TelescopeSignals::RequestedGeoLocationReceived);
711 }
712
713 virtual void OnTransitionChanged(
714 TelescopeMountState fromState,
715 TelescopeSignals signal,
716 TelescopeMountState toState
717 )
718 {
719 if(fromState != toState)
720 {
721 std::cerr << "Transition : (" << StateToString(fromState) << "," << SignalToString(signal) << ") -> " << StateToString(
722 toState) << std::endl;
723 }
724 }
725
726 virtual void OnErrorStateReached(
727 TelescopeMountState fromState,
728 TelescopeSignals signal
729 )
730 {
731 std::cerr << "Reached Error/Fail Safe State: most likly an undefined transition occurred!" << std::endl;
732 std::cerr << "Transition : (" << StateToString(fromState) << "," << SignalToString(signal) << ") -> ??? tripped this error!"
733 << std::endl;
734 }
735
736 //return the current telescope state.
737 TelescopeMountState GetTelescopeState()
738 {
739 return mMountStateMachine.CurrentState();
740 }
741
742 //return the current pointing coordinates.
743 SerialDeviceControl::EquatorialCoordinates GetPointingCoordinates()
744 {
745 return mCurrentPointingCoordinates.Get();
746 }
747
748 //return the current pointing coordinates.
749 SerialDeviceControl::EquatorialCoordinates GetSiteLocation()
750 {
751 return mSiteLocationCoordinates.Get();
752 }
753
754 private:
755 //mutex protected container for the current coordinates the telescope is pointing at.
756 SerialDeviceControl::CriticalData<SerialDeviceControl::EquatorialCoordinates> mCurrentPointingCoordinates;
757
758 //mutex protected container for the current site location set in the telescope.
759 SerialDeviceControl::CriticalData<SerialDeviceControl::EquatorialCoordinates> mSiteLocationCoordinates;
760
761 //mutex protected container for the current telescope state.
762 //SerialDeviceControl::CriticalData<TelescopeMountState> mTelescopeState;
763
764 //mutex protected state variable of the motion thread.
765 SerialDeviceControl::CriticalData<bool> mIsMotionControlThreadRunning;
766
767 //mutex protected state variable of the motion thread to indicate if a motion is started.
768 SerialDeviceControl::CriticalData<bool> mIsMotionControlRunning;
769
770 //holds the state of motion, direction and rate.
771 SerialDeviceControl::CriticalData<MotionState> mMotionState;
772
773 //motion control thread structure, to periodically sent direction commands.
774 std::thread mMotionCommandThread;
775
776 //mutex used for signaling the thread to start motion command sending,
777 //this is also used to halt the thread when the motion should be stopped.
778 std::mutex mMotionCommandControlMutex;
779
780 //Condition variable to signal start and stop of motion command sending.
781 std::condition_variable mMotionControlCondition;
782
783 //state machine of the the telescope hardware
784 MountStateMachine mMountStateMachine;
785
786 //Thread function for the motion thread.
787 void MotionControlThreadFunction()
788 {
789 bool isThreadRunning = mIsMotionControlThreadRunning.Get();
790 bool isMotionRunning;// = mIsMotionControlRunning.Get();
791
792 if(!isThreadRunning)
793 {
794 mIsMotionControlThreadRunning.Set(true);
795
796 std::cerr << "Motion Control Thread started!" << std::endl;
797
798 do
799 {
800 do
801 {
802 std::unique_lock<std::mutex> motionLock(mMotionCommandControlMutex);
803 isMotionRunning = mIsMotionControlRunning.Get();
804
805 if(!isMotionRunning)
806 {
807 //initially no motion commands are send, so wait until a motion in either direction is started by the start call.
808 mMotionControlCondition.wait(motionLock);
809
810 isMotionRunning = mIsMotionControlRunning.Get();
811 if(!isMotionRunning)
812 {
813 break;
814 }
815 }
816
817 MotionState motionState = mMotionState.Get();
818
819 //check if motion state is valid.
820 if
821 (
822 motionState.MotionDirection <= SerialDeviceControl::SerialCommandID::NULL_COMMAND_ID ||
823 motionState.MotionDirection >= SerialDeviceControl::SerialCommandID::STOP_MOTION_COMMAND_ID ||
824 motionState.CommandsPerSecond == 0
825 )
826 {
827 //motion is tripped but no values are provided -> disable motion and wait again.
828 mIsMotionControlRunning.Set(false);
829
830 motionState.MotionDirection = SerialDeviceControl::SerialCommandID::NULL_COMMAND_ID;
831 motionState.CommandsPerSecond = 0;
832
833 mMotionState.Set(motionState);
834 break;
835 }
836
837 int waitTime = 1000 / motionState.CommandsPerSecond;
838
839 //send command to move to direction.
840 switch(motionState.MotionDirection)
841 {
842 case SerialDeviceControl::SerialCommandID::MOVE_EAST_COMMAND_ID:
843 MoveEast();
844 break;
845
846 case SerialDeviceControl::SerialCommandID::MOVE_WEST_COMMAND_ID:
847 MoveWest();
848 break;
849
850 case SerialDeviceControl::SerialCommandID::MOVE_NORTH_COMMAND_ID:
851 MoveNorth();
852 break;
853
854 case SerialDeviceControl::SerialCommandID::MOVE_SOUTH_COMMAND_ID:
855 MoveSouth();
856 break;
857
858 default:
859 break;
860 }
861
862 //wait before next loop.
863 std::this_thread::sleep_until(std::chrono::system_clock::now() + std::chrono::milliseconds(waitTime));
864
865 isMotionRunning = mIsMotionControlRunning.Get();
866 }
867 while(isMotionRunning);
868
869 isThreadRunning = mIsMotionControlThreadRunning.Get();
870 }
871 while(isThreadRunning);
872
873 std::cerr << "Motion Control Thread stopped!" << std::endl;
874 }
875 }
876
877 public:
878 static std::string SignalToString(TelescopeSignals signal)
879 {
880 switch(signal)
881 {
882 case TelescopeSignals::Connect:
883 return EXPR_TO_STRING(TelescopeSignals::Connect);
884
885 case TelescopeSignals::Disconnect:
886 return EXPR_TO_STRING(TelescopeSignals::Disconnect);
887
888 case TelescopeSignals::GoTo:
889 return EXPR_TO_STRING(TelescopeSignals::GoTo);
890
891 case TelescopeSignals::InitialPointingCoordinatesReceived:
892 return EXPR_TO_STRING(TelescopeSignals::InitialPointingCoordinatesReceived);
893
894 case TelescopeSignals::ParkingPositionReached:
895 return EXPR_TO_STRING(TelescopeSignals::ParkingPositionReached);
896
897 case TelescopeSignals::RequestedGeoLocationReceived:
898 return EXPR_TO_STRING(TelescopeSignals::RequestedGeoLocationReceived);
899
900 case TelescopeSignals::Slew:
901 return EXPR_TO_STRING(TelescopeSignals::Slew);
902
903 case TelescopeSignals::Track:
904 return EXPR_TO_STRING(TelescopeSignals::Track);
905
906 case TelescopeSignals::TrackingTargetReached:
907 return EXPR_TO_STRING(TelescopeSignals::TrackingTargetReached);
908
909 case TelescopeSignals::StartMotion:
910 return EXPR_TO_STRING(TelescopeSignals::StartMotion);
911
912 case TelescopeSignals::StopMotion:
913 return EXPR_TO_STRING(TelescopeSignals::StopMotion);
914
915 case TelescopeSignals::Park:
916 return EXPR_TO_STRING(TelescopeSignals::Park);
917
918 case TelescopeSignals::Stop:
919 return EXPR_TO_STRING(TelescopeSignals::Stop);
920
921 default:
922 return "Invalid Signal!";
923
924 }
925 }
926
927 static std::string StateToString(TelescopeMountState state)
928 {
929 switch(state)
930 {
931 case TelescopeMountState::Disconnected:
932 return EXPR_TO_STRING(TelescopeMountState::Disconnected);
933
934 case TelescopeMountState::Connected:
935 return EXPR_TO_STRING(TelescopeMountState::Connected);
936
937 case TelescopeMountState::Parked:
938 return EXPR_TO_STRING(TelescopeMountState::Parked);
939
940 case TelescopeMountState::Idle:
941 return EXPR_TO_STRING(TelescopeMountState::Idle);
942
943 case TelescopeMountState::Unknown:
944 return EXPR_TO_STRING(TelescopeMountState::Unknown);
945
946 case TelescopeMountState::ParkingIssued:
947 return EXPR_TO_STRING(TelescopeMountState::ParkingIssued);
948
949 case TelescopeMountState::Tracking:
950 return EXPR_TO_STRING(TelescopeMountState::Tracking);
951
952 case TelescopeMountState::Slewing:
953 return EXPR_TO_STRING(TelescopeMountState::Slewing);
954
955 case TelescopeMountState::MoveWhileTracking:
956 return EXPR_TO_STRING(TelescopeMountState::MoveWhileTracking);
957
958 case TelescopeMountState::FailSafe:
959 return EXPR_TO_STRING(TelescopeMountState::FailSafe);
960
961 default:
962 return "Invalid State";
963 }
964 }
965 };
966 }