diff --git a/CHANGELOG.md b/CHANGELOG.md index df21abc5a7df57d1a713be1dc97c99cb48d60448..9fd35d937c3d12206edd01ad230813e17e835572 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -103,6 +103,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/ This command must be placed in the .bck file in order to work properly. issue #619 - Active surface components are now capable of changing look-up tables on the fly via the `asSetLUT` command issue #806 Added support for the C-band receiver at the SRT + project #2 - Completed integration of new SRT Minor Servos ## Fixed ## Changed diff --git a/Common/Clients/MinorServoBossTextClient/include/MinorServoBossTextClient.h b/Common/Clients/MinorServoBossTextClient/include/MinorServoBossTextClient.h index 97cdc5198961f5144f3876a2cd65c11a51d3fa2a..3656b750c2a2dacce10a7ace74d813bd66d1cdfe 100644 --- a/Common/Clients/MinorServoBossTextClient/include/MinorServoBossTextClient.h +++ b/Common/Clients/MinorServoBossTextClient/include/MinorServoBossTextClient.h @@ -78,10 +78,12 @@ /* define name and component interface */ /* ********************************** */ #define COMPONENT_NAME "MINORSERVO/Boss" /* the name of the instantiation of the component */ -#define COMPONENT_INTERFACE_TPYE "IDL:alma/MinorServo/MinorServoBoss:1.0" /* the type of the interface */ #define COMPONENT_IDL_MODULE MinorServo /* the IDL module that contains the component interface */ #define COMPONENT_IDL_INTERFACE MinorServoBoss /* the IDL interface of the component */ #define COMPONENT_SMARTPOINTER MinorServoBoss_var /* the component type */ +#ifndef COMPONENT_INTERFACE_TPYE + #define COMPONENT_INTERFACE_TPYE "IDL:alma/MinorServo/MinorServoBoss:1.0" +#endif /* ********************************** */ /* define user input command style */ diff --git a/Common/Clients/MinorServoBossTextClient/src/Makefile b/Common/Clients/MinorServoBossTextClient/src/Makefile index 39d35e7433e0ac1db9d6211778854005be9558e6..246fd626eafe2750811b132be8135eeea0e53c79 100644 --- a/Common/Clients/MinorServoBossTextClient/src/Makefile +++ b/Common/Clients/MinorServoBossTextClient/src/Makefile @@ -69,6 +69,9 @@ _tui_MinorServoBossTextClient_LIBS = MinorServoBossStubs IRALibrary Managme TextWindowLibrary ClientErrors ComponentErrors ManagementErrors MinorServoErrors AntennaDefinitionsStubs \ MinorServoDefinitionsStubs +ifeq ($(STATION),SRT) + MinorServoBossTextClient_CFLAGS = -DCOMPONENT_INTERFACE_TPYE="\"IDL:alma/MinorServo/SRTMinorServoBoss:1.0\"" +endif SCRIPTS = minorservoBossTui diff --git a/Common/Clients/MinorServoBossTextClient/src/MinorServoBossTextClient.cpp b/Common/Clients/MinorServoBossTextClient/src/MinorServoBossTextClient.cpp index 9f490e5badec555e129e0bf516cba0d4331b2f8a..c90158e95c2b861bf4f301c7580a2fc3e8695787 100644 --- a/Common/Clients/MinorServoBossTextClient/src/MinorServoBossTextClient.cpp +++ b/Common/Clients/MinorServoBossTextClient/src/MinorServoBossTextClient.cpp @@ -289,7 +289,7 @@ int main(int argc, char *argv[]) { status_box->setStatusLook(Management::MNG_OK,CStyle(BLACK_GREEN,CStyle::BOLD)); status_box->setStatusLook(Management::MNG_WARNING,CStyle(BLACK_YELLOW,CStyle::BOLD)); status_box->setStatusLook(Management::MNG_FAILURE,CStyle(BLACK_RED,CStyle::BOLD)); - _TW_SET_COMPONENT(motionInfo_field,18,3,23,1,CColorPair::WHITE_BLACK,CStyle::BOLD,output_label); + _TW_SET_COMPONENT(motionInfo_field,18,3,30,1,CColorPair::WHITE_BLACK,CStyle::BOLD,output_label); /* ****************************************************************** */ _TW_SET_COMPONENT(userInput,0,WINDOW_HEIGHT-6,WINDOW_WIDTH-1,1,USER_INPUT_COLOR_PAIR,USER_INPUT_STYLE,NULL); diff --git a/SRT/CDB/MACI/Components/MINORSERVO/Boss/Boss.xml b/SRT/CDB/MACI/Components/MINORSERVO/Boss/Boss.xml index 2705230c3a2f779384bd66d681fd3f9fa9161d56..b7a0bfd4102cdc65503946ad7446f43602ea2196 100644 --- a/SRT/CDB/MACI/Components/MINORSERVO/Boss/Boss.xml +++ b/SRT/CDB/MACI/Components/MINORSERVO/Boss/Boss.xml @@ -6,10 +6,9 @@ xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" Name="Boss" - Code="MinorServoBossImpl" + Code="SRTMinorServoBossImpl" ImplLang="cpp" - Type="IDL:alma/MinorServo/MinorServoBoss:1.0" - Container="MinorServoBossContainer" - Default="true" - + Type="IDL:alma/MinorServo/SRTMinorServoBoss:1.0" + Container="MinorServoContainer" + Default="true" /> diff --git a/SRT/CDB/MACI/Components/MINORSERVO/GFR/GFR.xml b/SRT/CDB/MACI/Components/MINORSERVO/GFR/GFR.xml index ed2c44f77034a4d76cac0bacce6b735f562874aa..23da6db86964dbb40d2a49684fb7a0b11280d257 100644 --- a/SRT/CDB/MACI/Components/MINORSERVO/GFR/GFR.xml +++ b/SRT/CDB/MACI/Components/MINORSERVO/GFR/GFR.xml @@ -6,10 +6,9 @@ xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" Name="GFR" - Code="WPServoImpl" + Code="SRTGenericMinorServoImpl" ImplLang="cpp" - Type="IDL:alma/MinorServo/WPServo:1.0" + Type="IDL:alma/MinorServo/SRTGenericMinorServo:1.0" Container="MinorServoContainer" - Default="true" - + Default="true" /> diff --git a/SRT/CDB/MACI/Components/MINORSERVO/M3R/M3R.xml b/SRT/CDB/MACI/Components/MINORSERVO/M3R/M3R.xml index 8a01ba209e1e753a055233df1075f687a4f907b7..94efe94c9728624cfaaab98a2f33e6adadde1420 100644 --- a/SRT/CDB/MACI/Components/MINORSERVO/M3R/M3R.xml +++ b/SRT/CDB/MACI/Components/MINORSERVO/M3R/M3R.xml @@ -6,10 +6,9 @@ xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" Name="M3R" - Code="WPServoImpl" + Code="SRTGenericMinorServoImpl" ImplLang="cpp" - Type="IDL:alma/MinorServo/WPServo:1.0" + Type="IDL:alma/MinorServo/SRTGenericMinorServo:1.0" Container="MinorServoContainer" Default="true" - /> diff --git a/SRT/CDB/MACI/Components/MINORSERVO/PFP/PFP.xml b/SRT/CDB/MACI/Components/MINORSERVO/PFP/PFP.xml index d75a83d755efa2d9ef22a12bfdf60f12822268f5..a5801b66da5c3bdcc192282f036f16269a5c689c 100644 --- a/SRT/CDB/MACI/Components/MINORSERVO/PFP/PFP.xml +++ b/SRT/CDB/MACI/Components/MINORSERVO/PFP/PFP.xml @@ -6,9 +6,9 @@ xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" Name="PFP" - Code="WPServoImpl" + Code="SRTProgramTrackMinorServoImpl" ImplLang="cpp" - Type="IDL:alma/MinorServo/WPServo:1.0" + Type="IDL:alma/MinorServo/SRTProgramTrackMinorServo:1.0" Container="MinorServoContainer" Default="true" diff --git a/SRT/CDB/MACI/Components/MINORSERVO/SRP/SRP.xml b/SRT/CDB/MACI/Components/MINORSERVO/SRP/SRP.xml index 4534c3d829738e843555dc04ed020e31c9743fdc..b957480d671b04fb21d14738f5a90d079e1dab46 100644 --- a/SRT/CDB/MACI/Components/MINORSERVO/SRP/SRP.xml +++ b/SRT/CDB/MACI/Components/MINORSERVO/SRP/SRP.xml @@ -5,11 +5,11 @@ xmlns:cdb="urn:schemas-cosylab-com:CDB:1.0" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" - Name="SRP" - Code="WPServoImpl" + Name="SRP" + Code="SRTProgramTrackMinorServoImpl" ImplLang="cpp" - Type="IDL:alma/MinorServo/WPServo:1.0" + Type="IDL:alma/MinorServo/SRTProgramTrackMinorServo:1.0" Container="MinorServoContainer" - Default="true" + Default="true" /> diff --git a/SRT/CDB/MACI/Containers/MinorServoBossContainer/MinorServoBossContainer.xml b/SRT/CDB/MACI/Containers/MinorServoBossContainer/MinorServoBossContainer.xml deleted file mode 100644 index 64fcda240156c3af04b69288aecc2ec87d71420b..0000000000000000000000000000000000000000 --- a/SRT/CDB/MACI/Containers/MinorServoBossContainer/MinorServoBossContainer.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/SRT/CDB/alma/DataBlock/MinorServo/GFR/LookupTables/LookupTables.xml b/SRT/CDB/alma/DataBlock/MinorServo/GFR/LookupTables/LookupTables.xml new file mode 100644 index 0000000000000000000000000000000000000000..00e681dfa7356bc407de6884408d293551f70f76 --- /dev/null +++ b/SRT/CDB/alma/DataBlock/MinorServo/GFR/LookupTables/LookupTables.xml @@ -0,0 +1,32 @@ + + + + + + ROTATION -88.70659 + ROTATION -88.70659 + ROTATION -159.8899 + ROTATION -159.8899 + ROTATION 90.971610 + ROTATION 90.971610 + ROTATION 162.771 + ROTATION 162.771 + ROTATION 55.373967 + ROTATION 55.373967 + ROTATION -51.821170 + ROTATION -51.821170 + + + ROTATION 0 + ROTATION 0 + ROTATION 0 + ROTATION 0 + + diff --git a/SRT/CDB/alma/DataBlock/MinorServo/GFR/Properties/Properties.xml b/SRT/CDB/alma/DataBlock/MinorServo/GFR/Properties/Properties.xml new file mode 100644 index 0000000000000000000000000000000000000000..d68395104cb04be48c9716da3edc66c7e7141a03 --- /dev/null +++ b/SRT/CDB/alma/DataBlock/MinorServo/GFR/Properties/Properties.xml @@ -0,0 +1,24 @@ + + + + + + CLOCKWISE_ENABLED + COUNTERCLOCKWISE_ENABLED + + CLOCKWISE + COUNTERCLOCKWISE + + ROTATION + + degree + + OFFSET + diff --git a/SRT/CDB/alma/DataBlock/MinorServo/M3R/LookupTables/LookupTables.xml b/SRT/CDB/alma/DataBlock/MinorServo/M3R/LookupTables/LookupTables.xml new file mode 100644 index 0000000000000000000000000000000000000000..fccec0b0fbb14be2c72f208fb4b41091176b6fea --- /dev/null +++ b/SRT/CDB/alma/DataBlock/MinorServo/M3R/LookupTables/LookupTables.xml @@ -0,0 +1,18 @@ + + + + + + ROTATION 0 + ROTATION 0 + ROTATION 0 + ROTATION 0 + + diff --git a/SRT/CDB/alma/DataBlock/MinorServo/M3R/Properties/Properties.xml b/SRT/CDB/alma/DataBlock/MinorServo/M3R/Properties/Properties.xml new file mode 100644 index 0000000000000000000000000000000000000000..d68395104cb04be48c9716da3edc66c7e7141a03 --- /dev/null +++ b/SRT/CDB/alma/DataBlock/MinorServo/M3R/Properties/Properties.xml @@ -0,0 +1,24 @@ + + + + + + CLOCKWISE_ENABLED + COUNTERCLOCKWISE_ENABLED + + CLOCKWISE + COUNTERCLOCKWISE + + ROTATION + + degree + + OFFSET + diff --git a/SRT/CDB/alma/DataBlock/MinorServo/PFP/LookupTables/LookupTables.xml b/SRT/CDB/alma/DataBlock/MinorServo/PFP/LookupTables/LookupTables.xml new file mode 100644 index 0000000000000000000000000000000000000000..086b3785bfad3d4cf429fc8d5b6703514c07c6f6 --- /dev/null +++ b/SRT/CDB/alma/DataBlock/MinorServo/PFP/LookupTables/LookupTables.xml @@ -0,0 +1,33 @@ + + + + + + TX 0 + TZ 0 + RTHETA 0 + + TX 0 + TZ 0 + RTHETA 0 + + TX 0 + TZ 0 + RTHETA 0 + + TX 0 + TZ 0 + RTHETA 0 + + TX 0 + TZ 0 + RTHETA 0 + + diff --git a/SRT/CDB/alma/DataBlock/MinorServo/PFP/Properties/Properties.xml b/SRT/CDB/alma/DataBlock/MinorServo/PFP/Properties/Properties.xml new file mode 100644 index 0000000000000000000000000000000000000000..688491e3051fc40ae7a0b13e14b0ff78ca5f6ec9 --- /dev/null +++ b/SRT/CDB/alma/DataBlock/MinorServo/PFP/Properties/Properties.xml @@ -0,0 +1,36 @@ + + + + + + X_ENABLED + Z_MASTER_ENABLED + Z_SLAVE_ENABLED + THETA_MASTER_ENABLED + THETA_SLAVE_ENABLED + + ELONG_X + ELONG_Z_MASTER + ELONG_Z_SLAVE + ELONG_THETA_MASTER + ELONG_THETA_SLAVE + + TX + TZ + RTHETA + + mm + mm + degree + + OFFSET_TX + OFFSET_TZ + OFFSET_RTHETA + diff --git a/SRT/CDB/alma/DataBlock/MinorServo/SRP/LookupTables/LookupTables.xml b/SRT/CDB/alma/DataBlock/MinorServo/SRP/LookupTables/LookupTables.xml new file mode 100644 index 0000000000000000000000000000000000000000..6869a5c77c731224691bcc2d122940bceb4a6e70 --- /dev/null +++ b/SRT/CDB/alma/DataBlock/MinorServo/SRP/LookupTables/LookupTables.xml @@ -0,0 +1,125 @@ + + + + + + TX-1.5 + TY28.256852219272844,-0.002707044952,-0.009870218853,0.000031617958 + TZ9.530113849340003,-0.169826241752,0.000419997047,0.000003985237 + RX-0.011392527142374848,-0.0001426193499425,0.000039508844799,-0.000000131010010 + RY-0.03611111111111111 + RZ0 + + TX-1.5 + TY29.556666666666498,0.263472663139432,-0.018206701940039,0.000072373113855 + TZ-15.6669651675,1.9293068324,-0.0628990613,0.0007771141,-0.0000032940 + RX-0.0055555555555569409, 0.00014822163433269445, 0.000027586713698, -0.000000077732053 + RY-0.036111111111111108 + RZ0 + + TX-1.5 + TY28.256852219272844,-0.002707044952,-0.009870218853,0.000031617958 + TZ9.530113849340003,-0.169826241752,0.000419997047,0.000003985237 + RX-0.011392527142374848,-0.0001426193499425,0.000039508844799,-0.000000131010010 + RY-0.03611111111111111 + RZ0 + + TX-1.5 + TY29.556666666666498,0.263472663139432,-0.018206701940039,0.000072373113855 + TZ-15.6669651675,1.9293068324,-0.0628990613,0.0007771141,-0.0000032940 + RX-0.0055555555555569409, 0.00014822163433269445, 0.000027586713698, -0.000000077732053 + RY-0.036111111111111108 + RZ0 + + TX0 + TY0 + TZ0 + RX0 + RY0 + RZ0 + + TX0 + TY0 + TZ0 + RX0 + RY0 + RZ0 + + TX0 + TY0 + TZ0 + RX0 + RY0 + RZ0 + + TX0 + TY0 + TZ0 + RX0 + RY0 + RZ0 + + TX0 + TY0 + TZ0 + RX0 + RY0 + RZ0 + + TX0 + TY0 + TZ0 + RX0 + RY0 + RZ0 + + TX38 + TY31 + TZ-12.788622422210521 + RX-0.005254963341757046 + RY-0.03611111111111111 + RZ0 + + TX38 + TY31 + TZ-12.788622422210521 + RX-0.005254963341757046 + RY-0.03611111111111111 + RZ0 + + TX-1.5 + TY28.256852219272844,-0.002707044952,-0.009870218853,0.000031617958 + TZ9.530113849340003,-0.169826241752,0.000419997047,0.000003985237 + RX-0.011392527142374848,-0.0001426193499425,0.000039508844799,-0.000000131010010 + RY-0.03611111111111111 + RZ0 + + TX-1.5 + TY29.556666666666498,0.263472663139432,-0.018206701940039,0.000072373113855 + TZ91.5590595452,-16.4202062811,1.16941963489,-0.040640240455,0.000733782714288,-6.62393455442e-06,2.36410838911e-08 + RX-0.0055555555555569409,0.00014822163433269445,0.000027586713698,-0.000000077732053 + RY-0.036111111111111108 + RZ0 + + TX-1.5 + TY28.256852219272844,-0.002707044952,-0.009870218853,0.000031617958 + TZ7.92754535681,-1.73279985542,0.147346047014,-0.00516934108597,7.69094654954e-05,-4.0697957632e-07 + RX-0.011392527142374848,-0.0001426193499425,0.000039508844799,-0.000000131010010 + RY-0.03611111111111111 + RZ0 + + TX-1.5 + TY29.556666666666498,0.263472663139432,-0.018206701940039,0.000072373113855 + TZ-38.3143893309,4.30888128547,-0.18265795755,0.00350049382452,-3.17057523513e-05,1.11006707448e-07 + RX-0.0055555555555569409,0.00014822163433269445,0.000027586713698,-0.000000077732053 + RY-0.036111111111111108 + RZ0 + + diff --git a/SRT/CDB/alma/DataBlock/MinorServo/SRP/Properties/Properties.xml b/SRT/CDB/alma/DataBlock/MinorServo/SRP/Properties/Properties.xml new file mode 100644 index 0000000000000000000000000000000000000000..165d5198f16d2ba7b25b0ef180a546a7ec6e3e5f --- /dev/null +++ b/SRT/CDB/alma/DataBlock/MinorServo/SRP/Properties/Properties.xml @@ -0,0 +1,47 @@ + + + + + + Z1_ENABLED + Z2_ENABLED + Z3_ENABLED + Y1_ENABLED + Y2_ENABLED + X1_ENABLED + + ELONG_Z1 + ELONG_Z2 + ELONG_Z3 + ELONG_Y1 + ELONG_Y2 + ELONG_X1 + + TX + TY + TZ + RX + RY + RZ + + mm + mm + mm + degree + degree + degree + + OFFSET_TX + OFFSET_TY + OFFSET_TZ + OFFSET_RX + OFFSET_RY + OFFSET_RZ + diff --git a/SRT/CDB/alma/DataBlock/PointingModel/PointingModel.xml b/SRT/CDB/alma/DataBlock/PointingModel/PointingModel.xml index b1cf45978a996c46c959fe1265e3c493fb9560b0..14a8fbca79253096394c45cb03559f3718cf6a8c 100644 --- a/SRT/CDB/alma/DataBlock/PointingModel/PointingModel.xml +++ b/SRT/CDB/alma/DataBlock/PointingModel/PointingModel.xml @@ -38,36 +38,36 @@ KKG 90.0 - 1 -2.2964186668 - 1 0.0 - 1 -0.0061030770 - 1 -0.0035287447 - 1 -0.0014408963 - 1 -0.0017973853 - 1 0.0719125122 - 1 0.0925239921 - 0 0 - 0 0 - 1 0.0128885703 + 1 -2.2956719398 + 0 0.0 + 1 -0.0056679383 + 1 -0.0074789114 + 1 -0.0008009398 + 1 -0.0015651340 + 1 0.0572288483 + 1 0.1136546656 + 0 0.0 + 0 0.0 + 1 -0.0004978125 0 0.0 - 1 0.0006690503 - 1 0.0000107827 + 1 0.0002358370 + 1 0.0000537828 0 0.0 0 0.0 0 0.0 0 0.0 0 0.0 0 0.0 - 0 0 - 0 0 - 0 0 - 0 0 - 0 0 - 0 0 - 0 0 - 0 0 - 0 0 - 0 0 + 0 0.0 + 0 0.0 + 0 0.0 + 0 0.0 + 0 0.0 + 0 0.0 + 0 0.0 + 0 0.0 + 0 0.0 + 0 0.0 diff --git a/SRT/CDB/alma/MANAGEMENT/CalibrationTool/CalibrationTool.xml b/SRT/CDB/alma/MANAGEMENT/CalibrationTool/CalibrationTool.xml index 221b861fd225a0ff3d8808b0ce2dedd2f6bb08a9..c4ab33015f5924b5336e39256d1a2876d6c3db38 100644 --- a/SRT/CDB/alma/MANAGEMENT/CalibrationTool/CalibrationTool.xml +++ b/SRT/CDB/alma/MANAGEMENT/CalibrationTool/CalibrationTool.xml @@ -15,7 +15,7 @@ RepetitionExpireTime="8000000" TrackingFlagDutyCycle="100000" AntennaBossInterface="IDL:alma/Antenna/AntennaBoss:1.0" - MinorServoBossInterface="" + MinorServoBossInterface="IDL:alma/MinorServo/SRTMinorServoBoss:1.0" SchedulerInterface="IDL:alma/Management/Scheduler:1.0" ObservatoryInterface="IDL:alma/Antenna/Observatory:1.0" GenerateFile="1" diff --git a/SRT/CDB/alma/MANAGEMENT/FitsZilla/FitsZilla.xml b/SRT/CDB/alma/MANAGEMENT/FitsZilla/FitsZilla.xml index c17f54267c617061b50ecdca97dccf56e599efc5..df04e8a0eda0871fbdce20fb941427da421358e8 100644 --- a/SRT/CDB/alma/MANAGEMENT/FitsZilla/FitsZilla.xml +++ b/SRT/CDB/alma/MANAGEMENT/FitsZilla/FitsZilla.xml @@ -20,7 +20,7 @@ AntennaBossInterface="IDL:alma/Antenna/AntennaBoss:1.0" ObservatoryInterface="IDL:alma/Antenna/Observatory:1.0" ReceiversBossInterface="IDL:alma/Receivers/ReceiversBoss:1.0" - MinorServoBossInterface="IDL:alma/MinorServo/MinorServoBoss:1.0" + MinorServoBossInterface="IDL:alma/MinorServo/SRTMinorServoBoss:1.0" SchedulerInterface="IDL:alma/Management/Scheduler:1.0" MeteoInstance="WEATHERSTATION/WeatherStation" > diff --git a/SRT/CDB/alma/MANAGEMENT/Gavino/Gavino.xml b/SRT/CDB/alma/MANAGEMENT/Gavino/Gavino.xml index 2fde0991d49dd3023be10f165002ae4f3afba2be..422590f7852e7b076fece11c9cc349da31efda83 100644 --- a/SRT/CDB/alma/MANAGEMENT/Gavino/Gavino.xml +++ b/SRT/CDB/alma/MANAGEMENT/Gavino/Gavino.xml @@ -23,7 +23,7 @@ AntennaBossInterface="IDL:alma/Antenna/AntennaBoss:1.0" ObservatoryInterface="IDL:alma/Antenna/Observatory:1.0" ReceiversBossInterface="IDL:alma/Receivers/ReceiversBoss:1.0" - MinorServoBossInterface="IDL:alma/MinorServo/MinorServoBoss:1.0" + MinorServoBossInterface="IDL:alma/MinorServo/SRTMinorServoBoss:1.0" ActiveSurfaceBossInterface="IDL:alma/ActiveSurface/SRTActiveSurfaceBoss:1.0" CustomLoggerInterface="IDL:alma/Management/CustomLogger:1.0" WeatherStationInstance="IDL:alma/Weather/GenericWeatherStation:1.0" diff --git a/SRT/CDB/alma/MANAGEMENT/Point/Point.xml b/SRT/CDB/alma/MANAGEMENT/Point/Point.xml index 88c53765006a69d2f82b56246800f3959484077d..02eb3b0a28f2fb239e1201cd330795d441cfd5f9 100644 --- a/SRT/CDB/alma/MANAGEMENT/Point/Point.xml +++ b/SRT/CDB/alma/MANAGEMENT/Point/Point.xml @@ -15,7 +15,7 @@ RepetitionExpireTime="8000000" TrackingFlagDutyCycle="100000" AntennaBossInterface="IDL:alma/Antenna/AntennaBoss:1.0" - MinorServoBossInterface="IDL:alma/MinorServo/MinorServoBoss:1.0" + MinorServoBossInterface="IDL:alma/MinorServo/SRTMinorServoBoss:1.0" SchedulerInterface="IDL:alma/Management/Scheduler:1.0" ObservatoryInterface="IDL:alma/Antenna/Observatory:1.0" GenerateFile="0" diff --git a/SRT/CDB/alma/MINORSERVO/Boss/Boss.xml b/SRT/CDB/alma/MINORSERVO/Boss/Boss.xml index d4e2d5e3df522cf17aa21247c842ed51e2af16e7..4dab29cf9a1f74667c9a2f1cedf26a0639659644 100644 --- a/SRT/CDB/alma/MINORSERVO/Boss/Boss.xml +++ b/SRT/CDB/alma/MINORSERVO/Boss/Boss.xml @@ -1,197 +1,37 @@ - - - - - LLP=" - @ SRP: park; - @ PFP: RY(mm) = (-25.75); TX(mm) = (458); TZ(mm)= (-46.2); - @ PFP: power_off_encoder; - @ GFR: RZ(mm) = (-600); - @ M3R: RZ(mm) = (860); - " - - PPP=" - @ SRP: park; - @ PFP: RY(mm) = (-25.75); TX(mm) = (458); TZ(mm) = (-45.9); - @ PFP: power_off_encoder; - @ GFR: RZ(mm) = (-600); - @ M3R: RZ(mm) = (860); - " - - PLP=" - @ SRP: park; - @ PFP: RY(mm) = (-25.75); TX(mm) = (458); TZ(mm) = (-45.9); - @ PFP: power_off_encoder; - @ GFR: RZ(mm) = (-600); - @ M3R: RZ(mm) = (860); - " - - HHP=" - @ SRP: park; - @ PFP: RY(mm) = (-25.72); TX(mm) = (1312.0); TZ(mm) = (-40.0); - @ PFP: power_off_encoder; - @ GFR: RZ(mm) = (-600); - @ M3R: RZ(mm) = (860); - " - - XKP=" - @ SRP: park; - @ PFP: RY(mm) = (-25.75); TX(mm) = (-1060); TZ(mm) = (-45.9); - @ GFR: RZ(mm) = (-600); - @ M3R: RZ(mm) = (860); - " - - - slaves="GFR, M3R, PFP, SRP" -> - - + + - + - + - - - - - - + + + + + + + + + + + + diff --git a/SRT/CDB/alma/MINORSERVO/GFR/GFR.xml b/SRT/CDB/alma/MINORSERVO/GFR/GFR.xml index e13e504dbeaa1345b95d891c7f214ebe1880fc8c..75f01dc18ea9a7725c44e1ff4ee932bca707c79c 100644 --- a/SRT/CDB/alma/MINORSERVO/GFR/GFR.xml +++ b/SRT/CDB/alma/MINORSERVO/GFR/GFR.xml @@ -1,356 +1,33 @@ - - - - - - - - - - - + physical_axes="2" + virtual_axes="1" + max_speed="3.5" + acceleration="2" + min_range="-166" + max_range="168.5"> - - - - - - - - - - - + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + diff --git a/SRT/CDB/alma/MINORSERVO/M3R/M3R.xml b/SRT/CDB/alma/MINORSERVO/M3R/M3R.xml index c741e7cd88786eb2a84b20b467beceab4a3f1b9c..83ebde14b51cd3250df5b94928731f99fe361122 100644 --- a/SRT/CDB/alma/MINORSERVO/M3R/M3R.xml +++ b/SRT/CDB/alma/MINORSERVO/M3R/M3R.xml @@ -1,357 +1,33 @@ - - - - - - actionThreadStackSize="2048" - monitoringThreadStackSize="4096" - number_of_axis="1" - number_of_slaves="3" - scale_factor="1" - scale_offset="0" - server_ip="127.0.0.1" - server_port="10000" - timeout="2000000" - servo_address="3" - zero="0" - park_position="0" - max_speed="0" - min_speed="0" - driver_type = "unknown" - virtual_rs = "0" - require_calibration = "0" - expire_time = "0.05000" - tracking_delta = "1.0" - limits="(-945, 945)" -> - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + diff --git a/SRT/CDB/alma/MINORSERVO/PFP/PFP.xml b/SRT/CDB/alma/MINORSERVO/PFP/PFP.xml index 0c7cd46b392bc4dc49713c10851c65bcfbca093d..6fdbf0722f77112d9648179e3c966aedc86e501c 100644 --- a/SRT/CDB/alma/MINORSERVO/PFP/PFP.xml +++ b/SRT/CDB/alma/MINORSERVO/PFP/PFP.xml @@ -1,358 +1,39 @@ - - - - - - actionThreadStackSize="2048" - monitoringThreadStackSize="4096" - number_of_axis="3" - number_of_slaves="5" - scale_factor="1" - scale_offset="0" - server_ip="127.0.0.1" - server_port="10000" - timeout="2000000" - servo_address="0" - zero="0" - park_position="2730.15,0,-195" - max_speed="0" - min_speed="0" - driver_type = "unknown" - virtual_rs = "0" - require_calibration = "0" - expire_time = "0.05000" - tracking_delta = "1.0" - limits="(-28.51, 2740); (-1480, 1480); (-199, 40)" -> - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + - + diff --git a/SRT/CDB/alma/MINORSERVO/SRP/SRP.xml b/SRT/CDB/alma/MINORSERVO/SRP/SRP.xml index 44730072b1dacc57384f62a94f022b154609dab1..eebfba2c722a57e3d9262b26478fc7035d10e76e 100644 --- a/SRT/CDB/alma/MINORSERVO/SRP/SRP.xml +++ b/SRT/CDB/alma/MINORSERVO/SRP/SRP.xml @@ -1,359 +1,39 @@ - - - - - - - actionThreadStackSize="2048" - monitoringThreadStackSize="4096" - number_of_axis="6" - number_of_slaves="7" - scale_factor="1" - scale_offset="0" - server_ip="127.0.0.1" - server_port="10000" - timeout="2000000" - servo_address="1" - zero="1570.00" - park_position="-5,5,-125,0,0,0" - max_speed="0" - min_speed="0" - driver_type = "unknown" - virtual_rs = "1" - require_calibration = "0" - expire_time = "0.05000" - tracking_delta = "1.0" - limits="(-50, 50); (-110, 110); (-110, 110); (-0.25, 0.25); (-0.25, 0.25); (-0.25, 0.25)" -> - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + - + diff --git a/SRT/CDB/alma/MINORSERVO/Socket/Socket.xml b/SRT/CDB/alma/MINORSERVO/Socket/Socket.xml new file mode 100644 index 0000000000000000000000000000000000000000..b1de61c195de0202f9b9c039bb4d087212a945b6 --- /dev/null +++ b/SRT/CDB/alma/MINORSERVO/Socket/Socket.xml @@ -0,0 +1,12 @@ + + + + diff --git a/SRT/CDB/alma/Procedures/StationProcedures/StationProcedures.xml b/SRT/CDB/alma/Procedures/StationProcedures/StationProcedures.xml index b6d47acd774164acfa21fb4b22ffe0415cb63e3a..4d86ea178dff64af66c899f217ffe0bbf72224be 100644 --- a/SRT/CDB/alma/Procedures/StationProcedures/StationProcedures.xml +++ b/SRT/CDB/alma/Procedures/StationProcedures/StationProcedures.xml @@ -6,7 +6,7 @@ antennaSetup=CCB - servoSetup=CCB + servoSetup=CCB receiversSetup=CCB chooseBackend=TotalPower initialize=CCB @@ -19,16 +19,22 @@ + antennaSetup=CCG + servoSetup=CCG receiversSetup=CCG chooseBackend=TotalPower + initialize=CCG + device=0 calOff + restFrequency=0 + azelOffsets=0d,0d antennaSetup=KKG - servoSetup=KKG + servoSetup=KKG receiversSetup=KKG receiversMode=SINGLEDISH chooseBackend=TotalPower @@ -43,7 +49,7 @@ antennaSetup=LP - servoSetup=LLP + servoSetup=LLP receiversSetup=LLP receiversMode=XXC4 chooseBackend=TotalPower @@ -58,7 +64,7 @@ antennaSetup=LP - servoSetup=PPP + servoSetup=PPP receiversSetup=PPP receiversMode=C3XX chooseBackend=TotalPower @@ -73,7 +79,7 @@ antennaSetup=LP - servoSetup=PLP + servoSetup=PLP receiversSetup=PLP receiversMode=C3C4 chooseBackend=TotalPower @@ -88,28 +94,28 @@ antennaSetup=XB - servoSetup=XB + servoSetup=XB receiversSetup=CCB chooseBackend=TotalPower initialize=XB device=0 restFrequency=0 - setLO=7500 + setLO=7500 azelOffsets=0d,0d - antennaSetup=LP - servoSetup=SSP - receiversSetup=CCB - chooseBackend=TotalPower - initialize=PPP - device=0 - calOff - restFrequency=0 - azelOffsets=0d,0d + antennaSetup=LP + servoSetup=SSP + receiversSetup=CCB + chooseBackend=TotalPower + initialize=PPP + device=0 + calOff + restFrequency=0 + azelOffsets=0d,0d diff --git a/SRT/Configuration/CDB/MACI/Components/MINORSERVO/Boss/Boss.xml b/SRT/Configuration/CDB/MACI/Components/MINORSERVO/Boss/Boss.xml index 2705230c3a2f779384bd66d681fd3f9fa9161d56..b7a0bfd4102cdc65503946ad7446f43602ea2196 100644 --- a/SRT/Configuration/CDB/MACI/Components/MINORSERVO/Boss/Boss.xml +++ b/SRT/Configuration/CDB/MACI/Components/MINORSERVO/Boss/Boss.xml @@ -6,10 +6,9 @@ xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" Name="Boss" - Code="MinorServoBossImpl" + Code="SRTMinorServoBossImpl" ImplLang="cpp" - Type="IDL:alma/MinorServo/MinorServoBoss:1.0" - Container="MinorServoBossContainer" - Default="true" - + Type="IDL:alma/MinorServo/SRTMinorServoBoss:1.0" + Container="MinorServoContainer" + Default="true" /> diff --git a/SRT/Configuration/CDB/MACI/Components/MINORSERVO/GFR/GFR.xml b/SRT/Configuration/CDB/MACI/Components/MINORSERVO/GFR/GFR.xml index ed2c44f77034a4d76cac0bacce6b735f562874aa..23da6db86964dbb40d2a49684fb7a0b11280d257 100644 --- a/SRT/Configuration/CDB/MACI/Components/MINORSERVO/GFR/GFR.xml +++ b/SRT/Configuration/CDB/MACI/Components/MINORSERVO/GFR/GFR.xml @@ -6,10 +6,9 @@ xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" Name="GFR" - Code="WPServoImpl" + Code="SRTGenericMinorServoImpl" ImplLang="cpp" - Type="IDL:alma/MinorServo/WPServo:1.0" + Type="IDL:alma/MinorServo/SRTGenericMinorServo:1.0" Container="MinorServoContainer" - Default="true" - + Default="true" /> diff --git a/SRT/Configuration/CDB/MACI/Components/MINORSERVO/M3R/M3R.xml b/SRT/Configuration/CDB/MACI/Components/MINORSERVO/M3R/M3R.xml index 8a01ba209e1e753a055233df1075f687a4f907b7..94efe94c9728624cfaaab98a2f33e6adadde1420 100644 --- a/SRT/Configuration/CDB/MACI/Components/MINORSERVO/M3R/M3R.xml +++ b/SRT/Configuration/CDB/MACI/Components/MINORSERVO/M3R/M3R.xml @@ -6,10 +6,9 @@ xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" Name="M3R" - Code="WPServoImpl" + Code="SRTGenericMinorServoImpl" ImplLang="cpp" - Type="IDL:alma/MinorServo/WPServo:1.0" + Type="IDL:alma/MinorServo/SRTGenericMinorServo:1.0" Container="MinorServoContainer" Default="true" - /> diff --git a/SRT/Configuration/CDB/MACI/Components/MINORSERVO/PFP/PFP.xml b/SRT/Configuration/CDB/MACI/Components/MINORSERVO/PFP/PFP.xml index d75a83d755efa2d9ef22a12bfdf60f12822268f5..a5801b66da5c3bdcc192282f036f16269a5c689c 100644 --- a/SRT/Configuration/CDB/MACI/Components/MINORSERVO/PFP/PFP.xml +++ b/SRT/Configuration/CDB/MACI/Components/MINORSERVO/PFP/PFP.xml @@ -6,9 +6,9 @@ xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" Name="PFP" - Code="WPServoImpl" + Code="SRTProgramTrackMinorServoImpl" ImplLang="cpp" - Type="IDL:alma/MinorServo/WPServo:1.0" + Type="IDL:alma/MinorServo/SRTProgramTrackMinorServo:1.0" Container="MinorServoContainer" Default="true" diff --git a/SRT/Configuration/CDB/MACI/Components/MINORSERVO/SRP/SRP.xml b/SRT/Configuration/CDB/MACI/Components/MINORSERVO/SRP/SRP.xml index 4534c3d829738e843555dc04ed020e31c9743fdc..b957480d671b04fb21d14738f5a90d079e1dab46 100644 --- a/SRT/Configuration/CDB/MACI/Components/MINORSERVO/SRP/SRP.xml +++ b/SRT/Configuration/CDB/MACI/Components/MINORSERVO/SRP/SRP.xml @@ -5,11 +5,11 @@ xmlns:cdb="urn:schemas-cosylab-com:CDB:1.0" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" - Name="SRP" - Code="WPServoImpl" + Name="SRP" + Code="SRTProgramTrackMinorServoImpl" ImplLang="cpp" - Type="IDL:alma/MinorServo/WPServo:1.0" + Type="IDL:alma/MinorServo/SRTProgramTrackMinorServo:1.0" Container="MinorServoContainer" - Default="true" + Default="true" /> diff --git a/SRT/Configuration/CDB/MACI/Containers/MinorServoBossContainer/MinorServoBossContainer.xml b/SRT/Configuration/CDB/MACI/Containers/MinorServoBossContainer/MinorServoBossContainer.xml deleted file mode 100644 index 64fcda240156c3af04b69288aecc2ec87d71420b..0000000000000000000000000000000000000000 --- a/SRT/Configuration/CDB/MACI/Containers/MinorServoBossContainer/MinorServoBossContainer.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/SRT/Configuration/CDB/alma/DataBlock/MinorServo/GFR/LookupTables/LookupTables.xml b/SRT/Configuration/CDB/alma/DataBlock/MinorServo/GFR/LookupTables/LookupTables.xml new file mode 100644 index 0000000000000000000000000000000000000000..00e681dfa7356bc407de6884408d293551f70f76 --- /dev/null +++ b/SRT/Configuration/CDB/alma/DataBlock/MinorServo/GFR/LookupTables/LookupTables.xml @@ -0,0 +1,32 @@ + + + + + + ROTATION -88.70659 + ROTATION -88.70659 + ROTATION -159.8899 + ROTATION -159.8899 + ROTATION 90.971610 + ROTATION 90.971610 + ROTATION 162.771 + ROTATION 162.771 + ROTATION 55.373967 + ROTATION 55.373967 + ROTATION -51.821170 + ROTATION -51.821170 + + + ROTATION 0 + ROTATION 0 + ROTATION 0 + ROTATION 0 + + diff --git a/SRT/Configuration/CDB/alma/DataBlock/MinorServo/GFR/Properties/Properties.xml b/SRT/Configuration/CDB/alma/DataBlock/MinorServo/GFR/Properties/Properties.xml new file mode 100644 index 0000000000000000000000000000000000000000..d68395104cb04be48c9716da3edc66c7e7141a03 --- /dev/null +++ b/SRT/Configuration/CDB/alma/DataBlock/MinorServo/GFR/Properties/Properties.xml @@ -0,0 +1,24 @@ + + + + + + CLOCKWISE_ENABLED + COUNTERCLOCKWISE_ENABLED + + CLOCKWISE + COUNTERCLOCKWISE + + ROTATION + + degree + + OFFSET + diff --git a/SRT/Configuration/CDB/alma/DataBlock/MinorServo/M3R/LookupTables/LookupTables.xml b/SRT/Configuration/CDB/alma/DataBlock/MinorServo/M3R/LookupTables/LookupTables.xml new file mode 100644 index 0000000000000000000000000000000000000000..fccec0b0fbb14be2c72f208fb4b41091176b6fea --- /dev/null +++ b/SRT/Configuration/CDB/alma/DataBlock/MinorServo/M3R/LookupTables/LookupTables.xml @@ -0,0 +1,18 @@ + + + + + + ROTATION 0 + ROTATION 0 + ROTATION 0 + ROTATION 0 + + diff --git a/SRT/Configuration/CDB/alma/DataBlock/MinorServo/M3R/Properties/Properties.xml b/SRT/Configuration/CDB/alma/DataBlock/MinorServo/M3R/Properties/Properties.xml new file mode 100644 index 0000000000000000000000000000000000000000..d68395104cb04be48c9716da3edc66c7e7141a03 --- /dev/null +++ b/SRT/Configuration/CDB/alma/DataBlock/MinorServo/M3R/Properties/Properties.xml @@ -0,0 +1,24 @@ + + + + + + CLOCKWISE_ENABLED + COUNTERCLOCKWISE_ENABLED + + CLOCKWISE + COUNTERCLOCKWISE + + ROTATION + + degree + + OFFSET + diff --git a/SRT/Configuration/CDB/alma/DataBlock/MinorServo/PFP/LookupTables/LookupTables.xml b/SRT/Configuration/CDB/alma/DataBlock/MinorServo/PFP/LookupTables/LookupTables.xml new file mode 100644 index 0000000000000000000000000000000000000000..086b3785bfad3d4cf429fc8d5b6703514c07c6f6 --- /dev/null +++ b/SRT/Configuration/CDB/alma/DataBlock/MinorServo/PFP/LookupTables/LookupTables.xml @@ -0,0 +1,33 @@ + + + + + + TX 0 + TZ 0 + RTHETA 0 + + TX 0 + TZ 0 + RTHETA 0 + + TX 0 + TZ 0 + RTHETA 0 + + TX 0 + TZ 0 + RTHETA 0 + + TX 0 + TZ 0 + RTHETA 0 + + diff --git a/SRT/Configuration/CDB/alma/DataBlock/MinorServo/PFP/Properties/Properties.xml b/SRT/Configuration/CDB/alma/DataBlock/MinorServo/PFP/Properties/Properties.xml new file mode 100644 index 0000000000000000000000000000000000000000..688491e3051fc40ae7a0b13e14b0ff78ca5f6ec9 --- /dev/null +++ b/SRT/Configuration/CDB/alma/DataBlock/MinorServo/PFP/Properties/Properties.xml @@ -0,0 +1,36 @@ + + + + + + X_ENABLED + Z_MASTER_ENABLED + Z_SLAVE_ENABLED + THETA_MASTER_ENABLED + THETA_SLAVE_ENABLED + + ELONG_X + ELONG_Z_MASTER + ELONG_Z_SLAVE + ELONG_THETA_MASTER + ELONG_THETA_SLAVE + + TX + TZ + RTHETA + + mm + mm + degree + + OFFSET_TX + OFFSET_TZ + OFFSET_RTHETA + diff --git a/SRT/Configuration/CDB/alma/DataBlock/MinorServo/SRP/LookupTables/LookupTables.xml b/SRT/Configuration/CDB/alma/DataBlock/MinorServo/SRP/LookupTables/LookupTables.xml new file mode 100644 index 0000000000000000000000000000000000000000..6869a5c77c731224691bcc2d122940bceb4a6e70 --- /dev/null +++ b/SRT/Configuration/CDB/alma/DataBlock/MinorServo/SRP/LookupTables/LookupTables.xml @@ -0,0 +1,125 @@ + + + + + + TX-1.5 + TY28.256852219272844,-0.002707044952,-0.009870218853,0.000031617958 + TZ9.530113849340003,-0.169826241752,0.000419997047,0.000003985237 + RX-0.011392527142374848,-0.0001426193499425,0.000039508844799,-0.000000131010010 + RY-0.03611111111111111 + RZ0 + + TX-1.5 + TY29.556666666666498,0.263472663139432,-0.018206701940039,0.000072373113855 + TZ-15.6669651675,1.9293068324,-0.0628990613,0.0007771141,-0.0000032940 + RX-0.0055555555555569409, 0.00014822163433269445, 0.000027586713698, -0.000000077732053 + RY-0.036111111111111108 + RZ0 + + TX-1.5 + TY28.256852219272844,-0.002707044952,-0.009870218853,0.000031617958 + TZ9.530113849340003,-0.169826241752,0.000419997047,0.000003985237 + RX-0.011392527142374848,-0.0001426193499425,0.000039508844799,-0.000000131010010 + RY-0.03611111111111111 + RZ0 + + TX-1.5 + TY29.556666666666498,0.263472663139432,-0.018206701940039,0.000072373113855 + TZ-15.6669651675,1.9293068324,-0.0628990613,0.0007771141,-0.0000032940 + RX-0.0055555555555569409, 0.00014822163433269445, 0.000027586713698, -0.000000077732053 + RY-0.036111111111111108 + RZ0 + + TX0 + TY0 + TZ0 + RX0 + RY0 + RZ0 + + TX0 + TY0 + TZ0 + RX0 + RY0 + RZ0 + + TX0 + TY0 + TZ0 + RX0 + RY0 + RZ0 + + TX0 + TY0 + TZ0 + RX0 + RY0 + RZ0 + + TX0 + TY0 + TZ0 + RX0 + RY0 + RZ0 + + TX0 + TY0 + TZ0 + RX0 + RY0 + RZ0 + + TX38 + TY31 + TZ-12.788622422210521 + RX-0.005254963341757046 + RY-0.03611111111111111 + RZ0 + + TX38 + TY31 + TZ-12.788622422210521 + RX-0.005254963341757046 + RY-0.03611111111111111 + RZ0 + + TX-1.5 + TY28.256852219272844,-0.002707044952,-0.009870218853,0.000031617958 + TZ9.530113849340003,-0.169826241752,0.000419997047,0.000003985237 + RX-0.011392527142374848,-0.0001426193499425,0.000039508844799,-0.000000131010010 + RY-0.03611111111111111 + RZ0 + + TX-1.5 + TY29.556666666666498,0.263472663139432,-0.018206701940039,0.000072373113855 + TZ91.5590595452,-16.4202062811,1.16941963489,-0.040640240455,0.000733782714288,-6.62393455442e-06,2.36410838911e-08 + RX-0.0055555555555569409,0.00014822163433269445,0.000027586713698,-0.000000077732053 + RY-0.036111111111111108 + RZ0 + + TX-1.5 + TY28.256852219272844,-0.002707044952,-0.009870218853,0.000031617958 + TZ7.92754535681,-1.73279985542,0.147346047014,-0.00516934108597,7.69094654954e-05,-4.0697957632e-07 + RX-0.011392527142374848,-0.0001426193499425,0.000039508844799,-0.000000131010010 + RY-0.03611111111111111 + RZ0 + + TX-1.5 + TY29.556666666666498,0.263472663139432,-0.018206701940039,0.000072373113855 + TZ-38.3143893309,4.30888128547,-0.18265795755,0.00350049382452,-3.17057523513e-05,1.11006707448e-07 + RX-0.0055555555555569409,0.00014822163433269445,0.000027586713698,-0.000000077732053 + RY-0.036111111111111108 + RZ0 + + diff --git a/SRT/Configuration/CDB/alma/DataBlock/MinorServo/SRP/Properties/Properties.xml b/SRT/Configuration/CDB/alma/DataBlock/MinorServo/SRP/Properties/Properties.xml new file mode 100644 index 0000000000000000000000000000000000000000..165d5198f16d2ba7b25b0ef180a546a7ec6e3e5f --- /dev/null +++ b/SRT/Configuration/CDB/alma/DataBlock/MinorServo/SRP/Properties/Properties.xml @@ -0,0 +1,47 @@ + + + + + + Z1_ENABLED + Z2_ENABLED + Z3_ENABLED + Y1_ENABLED + Y2_ENABLED + X1_ENABLED + + ELONG_Z1 + ELONG_Z2 + ELONG_Z3 + ELONG_Y1 + ELONG_Y2 + ELONG_X1 + + TX + TY + TZ + RX + RY + RZ + + mm + mm + mm + degree + degree + degree + + OFFSET_TX + OFFSET_TY + OFFSET_TZ + OFFSET_RX + OFFSET_RY + OFFSET_RZ + diff --git a/SRT/Configuration/CDB/alma/DataBlock/PointingModel/PointingModel.xml b/SRT/Configuration/CDB/alma/DataBlock/PointingModel/PointingModel.xml index b1cf45978a996c46c959fe1265e3c493fb9560b0..14a8fbca79253096394c45cb03559f3718cf6a8c 100644 --- a/SRT/Configuration/CDB/alma/DataBlock/PointingModel/PointingModel.xml +++ b/SRT/Configuration/CDB/alma/DataBlock/PointingModel/PointingModel.xml @@ -38,36 +38,36 @@ KKG 90.0 - 1 -2.2964186668 - 1 0.0 - 1 -0.0061030770 - 1 -0.0035287447 - 1 -0.0014408963 - 1 -0.0017973853 - 1 0.0719125122 - 1 0.0925239921 - 0 0 - 0 0 - 1 0.0128885703 + 1 -2.2956719398 + 0 0.0 + 1 -0.0056679383 + 1 -0.0074789114 + 1 -0.0008009398 + 1 -0.0015651340 + 1 0.0572288483 + 1 0.1136546656 + 0 0.0 + 0 0.0 + 1 -0.0004978125 0 0.0 - 1 0.0006690503 - 1 0.0000107827 + 1 0.0002358370 + 1 0.0000537828 0 0.0 0 0.0 0 0.0 0 0.0 0 0.0 0 0.0 - 0 0 - 0 0 - 0 0 - 0 0 - 0 0 - 0 0 - 0 0 - 0 0 - 0 0 - 0 0 + 0 0.0 + 0 0.0 + 0 0.0 + 0 0.0 + 0 0.0 + 0 0.0 + 0 0.0 + 0 0.0 + 0 0.0 + 0 0.0 diff --git a/SRT/Configuration/CDB/alma/MANAGEMENT/CalibrationTool/CalibrationTool.xml b/SRT/Configuration/CDB/alma/MANAGEMENT/CalibrationTool/CalibrationTool.xml index d18bf0948f25b431ebeffad9cb5d64b4363579b2..43a7772807d4ed7689796b7e2694d8661c640cbb 100644 --- a/SRT/Configuration/CDB/alma/MANAGEMENT/CalibrationTool/CalibrationTool.xml +++ b/SRT/Configuration/CDB/alma/MANAGEMENT/CalibrationTool/CalibrationTool.xml @@ -15,7 +15,7 @@ RepetitionExpireTime="8000000" TrackingFlagDutyCycle="100000" AntennaBossInterface="IDL:alma/Antenna/AntennaBoss:1.0" - MinorServoBossInterface="IDL:alma/MinorServo/MinorServoBoss:1.0" + MinorServoBossInterface="IDL:alma/MinorServo/SRTMinorServoBoss:1.0" SchedulerInterface="IDL:alma/Management/Scheduler:1.0" ObservatoryInterface="IDL:alma/Antenna/Observatory:1.0" GenerateFile="1" diff --git a/SRT/Configuration/CDB/alma/MANAGEMENT/FitsZilla/FitsZilla.xml b/SRT/Configuration/CDB/alma/MANAGEMENT/FitsZilla/FitsZilla.xml index 66f1454879dc12ba68b1ba6a2a427c469f6a2edf..f139a84fc4e9de110517c9e15f04a8cfaabb465b 100644 --- a/SRT/Configuration/CDB/alma/MANAGEMENT/FitsZilla/FitsZilla.xml +++ b/SRT/Configuration/CDB/alma/MANAGEMENT/FitsZilla/FitsZilla.xml @@ -20,7 +20,7 @@ AntennaBossInterface="IDL:alma/Antenna/AntennaBoss:1.0" ObservatoryInterface="IDL:alma/Antenna/Observatory:1.0" ReceiversBossInterface="IDL:alma/Receivers/ReceiversBoss:1.0" - MinorServoBossInterface="IDL:alma/MinorServo/MinorServoBoss:1.0" + MinorServoBossInterface="IDL:alma/MinorServo/SRTMinorServoBoss:1.0" SchedulerInterface="IDL:alma/Management/Scheduler:1.0" MeteoInstance="WEATHERSTATION/WeatherStation" > diff --git a/SRT/Configuration/CDB/alma/MANAGEMENT/Gavino/Gavino.xml b/SRT/Configuration/CDB/alma/MANAGEMENT/Gavino/Gavino.xml index 3b8c89f50a053ff0f9ab775d7cc9d41bfcd80a70..eb9ea73b86dc3cccf78680f814c4f38fd388cd94 100644 --- a/SRT/Configuration/CDB/alma/MANAGEMENT/Gavino/Gavino.xml +++ b/SRT/Configuration/CDB/alma/MANAGEMENT/Gavino/Gavino.xml @@ -23,7 +23,7 @@ AntennaBossInterface="IDL:alma/Antenna/AntennaBoss:1.0" ObservatoryInterface="IDL:alma/Antenna/Observatory:1.0" ReceiversBossInterface="IDL:alma/Receivers/ReceiversBoss:1.0" - MinorServoBossInterface="IDL:alma/MinorServo/MinorServoBoss:1.0" + MinorServoBossInterface="IDL:alma/MinorServo/SRTMinorServoBoss:1.0" ActiveSurfaceBossInterface="IDL:alma/ActiveSurface/SRTActiveSurfaceBoss:1.0" CustomLoggerInterface="IDL:alma/Management/CustomLogger:1.0" WeatherStationInstance="IDL:alma/Weather/GenericWeatherStation:1.0" diff --git a/SRT/Configuration/CDB/alma/MANAGEMENT/Point/Point.xml b/SRT/Configuration/CDB/alma/MANAGEMENT/Point/Point.xml index 88c53765006a69d2f82b56246800f3959484077d..02eb3b0a28f2fb239e1201cd330795d441cfd5f9 100644 --- a/SRT/Configuration/CDB/alma/MANAGEMENT/Point/Point.xml +++ b/SRT/Configuration/CDB/alma/MANAGEMENT/Point/Point.xml @@ -15,7 +15,7 @@ RepetitionExpireTime="8000000" TrackingFlagDutyCycle="100000" AntennaBossInterface="IDL:alma/Antenna/AntennaBoss:1.0" - MinorServoBossInterface="IDL:alma/MinorServo/MinorServoBoss:1.0" + MinorServoBossInterface="IDL:alma/MinorServo/SRTMinorServoBoss:1.0" SchedulerInterface="IDL:alma/Management/Scheduler:1.0" ObservatoryInterface="IDL:alma/Antenna/Observatory:1.0" GenerateFile="0" diff --git a/SRT/Configuration/CDB/alma/MINORSERVO/Boss/Boss.xml b/SRT/Configuration/CDB/alma/MINORSERVO/Boss/Boss.xml index ac992a0c5a5df2db76af83a70d93337d2aaa9c53..4dab29cf9a1f74667c9a2f1cedf26a0639659644 100644 --- a/SRT/Configuration/CDB/alma/MINORSERVO/Boss/Boss.xml +++ b/SRT/Configuration/CDB/alma/MINORSERVO/Boss/Boss.xml @@ -1,197 +1,37 @@ - - - - - LLP=" - @ SRP: park; - @ PFP: RY(mm) = (-25.75); TX(mm) = (458); TZ(mm)= (-46.2); - @ PFP: power_off_encoder; - @ GFR: RZ(mm) = (-600); - @ M3R: RZ(mm) = (860); - " - - PPP=" - @ SRP: park; - @ PFP: RY(mm) = (-25.75); TX(mm) = (458); TZ(mm) = (-45.9); - @ PFP: power_off_encoder; - @ GFR: RZ(mm) = (-600); - @ M3R: RZ(mm) = (860); - " - - PLP=" - @ SRP: park; - @ PFP: RY(mm) = (-25.75); TX(mm) = (458); TZ(mm) = (-45.9); - @ PFP: power_off_encoder; - @ GFR: RZ(mm) = (-600); - @ M3R: RZ(mm) = (860); - " - - HHP=" - @ SRP: park; - @ PFP: RY(mm) = (-25.72); TX(mm) = (1312.0); TZ(mm) = (-40.0); - @ PFP: power_off_encoder; - @ GFR: RZ(mm) = (-600); - @ M3R: RZ(mm) = (860); - " - - XKP=" - @ SRP: park; - @ PFP: RY(mm) = (-25.75); TX(mm) = (-1060); TZ(mm) = (-45.9); - @ GFR: RZ(mm) = (-600); - @ M3R: RZ(mm) = (860); - " - - - slaves="GFR, M3R, PFP, SRP" -> - - + + - + - + - - - - - - + + + + + + + + + + + + diff --git a/SRT/Configuration/CDB/alma/MINORSERVO/GFR/GFR.xml b/SRT/Configuration/CDB/alma/MINORSERVO/GFR/GFR.xml index 9535f226d2734d8e797188cc5b6614e10de741a1..75f01dc18ea9a7725c44e1ff4ee932bca707c79c 100644 --- a/SRT/Configuration/CDB/alma/MINORSERVO/GFR/GFR.xml +++ b/SRT/Configuration/CDB/alma/MINORSERVO/GFR/GFR.xml @@ -1,356 +1,33 @@ - - - - - - - - - - - + physical_axes="2" + virtual_axes="1" + max_speed="3.5" + acceleration="2" + min_range="-166" + max_range="168.5"> - - - - - - - - - - - + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + diff --git a/SRT/Configuration/CDB/alma/MINORSERVO/M3R/M3R.xml b/SRT/Configuration/CDB/alma/MINORSERVO/M3R/M3R.xml index cd650b487295e5a03f16aa71efd2e44f2d75af99..83ebde14b51cd3250df5b94928731f99fe361122 100644 --- a/SRT/Configuration/CDB/alma/MINORSERVO/M3R/M3R.xml +++ b/SRT/Configuration/CDB/alma/MINORSERVO/M3R/M3R.xml @@ -1,357 +1,33 @@ - - - - - - actionThreadStackSize="2048" - monitoringThreadStackSize="4096" - number_of_axis="1" - number_of_slaves="3" - scale_factor="1" - scale_offset="0" - server_ip="192.168.200.16" - server_port="10000" - timeout="2000000" - servo_address="3" - zero="0" - park_position="0" - max_speed="0" - min_speed="0" - driver_type = "unknown" - virtual_rs = "0" - require_calibration = "0" - expire_time = "0.05000" - tracking_delta = "1.0" - limits="(-945, 945)" -> - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + diff --git a/SRT/Configuration/CDB/alma/MINORSERVO/PFP/PFP.xml b/SRT/Configuration/CDB/alma/MINORSERVO/PFP/PFP.xml index c5f176b8adb03de365dbe765053a77d5f5488cac..6fdbf0722f77112d9648179e3c966aedc86e501c 100644 --- a/SRT/Configuration/CDB/alma/MINORSERVO/PFP/PFP.xml +++ b/SRT/Configuration/CDB/alma/MINORSERVO/PFP/PFP.xml @@ -1,358 +1,39 @@ - - - - - - actionThreadStackSize="2048" - monitoringThreadStackSize="4096" - number_of_axis="3" - number_of_slaves="5" - scale_factor="1" - scale_offset="0" - server_ip="192.168.200.16" - server_port="10000" - timeout="2000000" - servo_address="0" - zero="0" - park_position="2730.15,0,-195" - max_speed="0" - min_speed="0" - driver_type = "unknown" - virtual_rs = "0" - require_calibration = "0" - expire_time = "0.05000" - tracking_delta = "1.0" - limits="(-34.0, 2740); (-1480, 1480); (-199, 40)" -> - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + - + diff --git a/SRT/Configuration/CDB/alma/MINORSERVO/SRP/SRP.xml b/SRT/Configuration/CDB/alma/MINORSERVO/SRP/SRP.xml index 90a39717cb142bdbc8a9a484199779f77c1e2baf..eebfba2c722a57e3d9262b26478fc7035d10e76e 100644 --- a/SRT/Configuration/CDB/alma/MINORSERVO/SRP/SRP.xml +++ b/SRT/Configuration/CDB/alma/MINORSERVO/SRP/SRP.xml @@ -1,359 +1,39 @@ - - - - - - - actionThreadStackSize="2048" - monitoringThreadStackSize="4096" - number_of_axis="6" - number_of_slaves="7" - scale_factor="1" - scale_offset="0" - server_ip="192.168.200.16" - server_port="10000" - timeout="2000000" - servo_address="1" - zero="1570.00" - park_position="-5,5,-125,0,0,0" - max_speed="0" - min_speed="0" - driver_type = "unknown" - virtual_rs = "1" - require_calibration = "0" - expire_time = "0.05000" - tracking_delta = "1.0" - limits="(-50, 50); (-110, 110); (-110, 110); (-0.25, 0.25); (-0.25, 0.25); (-0.25, 0.25)" -> - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + - + diff --git a/SRT/Configuration/CDB/alma/MINORSERVO/Socket/Socket.xml b/SRT/Configuration/CDB/alma/MINORSERVO/Socket/Socket.xml new file mode 100644 index 0000000000000000000000000000000000000000..5ca037aaa212c39ac2449718985dd47655f24a95 --- /dev/null +++ b/SRT/Configuration/CDB/alma/MINORSERVO/Socket/Socket.xml @@ -0,0 +1,13 @@ + + + + diff --git a/SRT/Configuration/CDB/alma/Procedures/StationProcedures/StationProcedures.xml b/SRT/Configuration/CDB/alma/Procedures/StationProcedures/StationProcedures.xml index 1c63bba48658f2d40dc3bad24a44bbdd8063210d..4d86ea178dff64af66c899f217ffe0bbf72224be 100644 --- a/SRT/Configuration/CDB/alma/Procedures/StationProcedures/StationProcedures.xml +++ b/SRT/Configuration/CDB/alma/Procedures/StationProcedures/StationProcedures.xml @@ -20,6 +20,7 @@ antennaSetup=CCG + servoSetup=CCG receiversSetup=CCG chooseBackend=TotalPower initialize=CCG diff --git a/SRT/Interfaces/SRTMinorServoInterface/idl/.gitignore b/SRT/Interfaces/SRTMinorServoInterface/idl/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..981c93f2192d506314e94f606cccad38b341d73b --- /dev/null +++ b/SRT/Interfaces/SRTMinorServoInterface/idl/.gitignore @@ -0,0 +1 @@ +SRTMinorServoCommon.idl diff --git a/SRT/Interfaces/SRTMinorServoInterface/idl/SRTMinorServo.idl b/SRT/Interfaces/SRTMinorServoInterface/idl/SRTMinorServo.idl new file mode 100644 index 0000000000000000000000000000000000000000..5c3eb142aadd2f4861b2ca7ef884c4c7e73f189c --- /dev/null +++ b/SRT/Interfaces/SRTMinorServoInterface/idl/SRTMinorServo.idl @@ -0,0 +1,283 @@ +#ifndef __SRTMINORSERVO_IDL__ +#define __SRTMINORSERVO_IDL__ +/***************************************************************\ + * Authors: + * Giuseppe Carboni + * + * Created: Mon Mar 06 12:30:00 CEST 2023 +\***************************************************************/ + +#include "SRTMinorServoCommon.idl" + +#pragma prefix "alma" + +module MinorServo +{ + /** + * This IDL interface describes the CORBA interface of a common SRTMinorServo component. + * This is simply the base interface from which the SRTGenericMinorServo and SRTProgramTrackMinorServo interfaces inherit. + */ + interface SRTBaseMinorServo : ACS::CharacteristicComponent + { + /** + * This property tells if all the servo axes are enabled + */ + readonly attribute Management::ROTBoolean enabled; + + /** + * This property tells the status of the servo drive cabinet + */ + readonly attribute ROSRTMinorServoCabinetStatus drive_cabinet_status; + + /** + * This property tells if the servo is in blocked state and cannot move + */ + readonly attribute Management::ROTBoolean block; + + /** + * This property returns the current operative mode of the servo + */ + readonly attribute ROSRTMinorServoOperativeMode operative_mode; + + /** + * This property returns the sequence of statuses of the servo physical axes + */ + readonly attribute ACS::RObooleanSeq physical_axes_enabled; + + /** + * This property returns the sequence of current positions of the physical axes of the servo + */ + readonly attribute ACS::ROdoubleSeq physical_positions; + + /** + * This property returns the number of virtual axes of the servo + */ + readonly attribute ACS::ROlong virtual_axes; + + /** + * This property returns the current plain positions of the virtual axes of the servo + * The plain positions are the positions returned from the Leonardo servo system, without subtracting the offsets + */ + readonly attribute ACS::ROdoubleSeq plain_virtual_positions; + + /** + * This property returns the current positions of the virtual axes of the servo + * The positions returned by this property equals to the plain virtual positions minus the offsets + */ + readonly attribute ACS::ROdoubleSeq virtual_positions; + + /** + * This property returns the current offsets of the virtual axes of the servo + * The offset values are the sum of user and system offsets + */ + readonly attribute ACS::ROdoubleSeq virtual_offsets; + + /** + * This property returns the current user offsets of the virtual axes of the servo + */ + readonly attribute ACS::ROdoubleSeq virtual_user_offsets; + + /** + * This property returns the current system offsets of the virtual axes of the servo + */ + readonly attribute ACS::ROdoubleSeq virtual_system_offsets; + + /** + * This property indicates whether the servo is in use in the current configuration + */ + readonly attribute Management::ROTBoolean in_use; + + /** + * This property returns the configuration of the servo. Each configuration has different positioning coefficients + */ + readonly attribute ACS::ROstring current_setup; + + /** + * This method asks the PLC the status for the corresponding servo + * @throw MinorServoErrors::MinorServoErrorsEx when trying to reset the offsets when they don't match the ones loaded into the hardware + * @return true if the servo is not in an error state, false otherwise + */ + boolean status() raises (MinorServoErrors::MinorServoErrorsEx); + + /** + * This method commands a STOW operation to the servo + * @param stow_position the index of the position we want the servo to stow to + * @throw MinorServoErrors::MinorServoErrorsEx if there has been a communication error or if the command was not accepted + */ + void stow(in long stow_position) raises (MinorServoErrors::MinorServoErrorsEx); + + /** + * This method commands a STOP operation to the servo + * @throw MinorServoErrors::MinorServoErrorsEx if there has been a communication error or if the command was not accepted + */ + void stop() raises (MinorServoErrors::MinorServoErrorsEx); + + /** + * This method commands a PRESET operation to the servo + * @param coordinates, a sequence of double + * @throw MinorServoErrors::MinorServoErrorsEx if the length of the coordinates sequence doesn't match the number of virtual axes of the servo, + * if the resulting position summing the offsets would go outside the accepted range of the servo, + * if there has been a communication error or if the command was not accepted + */ + void preset(in ACS::doubleSeq coordinates) raises (MinorServoErrors::MinorServoErrorsEx); + + /** + * This method loads from the CDB the positioning coefficients related to the given configuration + * @param configuration the string representing the name of the table from which the coefficients will be loaded + * @throw ComponentErrors::ComponentErrorsEx when there is an error while trying to load the table for the given configuration + * @return true if the servo is in use with the current configuration, false otherwise + */ + boolean setup(in string configuration) raises (ComponentErrors::ComponentErrorsEx); + + /** + * This method calculates the servo coordinates for a given elevation + * @param elevation the elevation to use for the coordinates calculation, expressed in degrees + * @throw MinorServoErrors::MinorServoErrorsEx when the servo has not been configured yet and has not loaded any coefficient for the position calculation + * @return the calculated coordinates as a sequence of doubles + */ + ACS::doubleSeq calcCoordinates(in double elevation) raises (MinorServoErrors::MinorServoErrorsEx); + + /** + * This method returns the user offsets of the servo + * @return the user offsets as a sequence of doubles + */ + ACS::doubleSeq getUserOffsets(); + + /** + * This method sets the user offset of the servo for a given axis + * @param axis_name a string corresponding to the axis we want to set the offset for + * @param offset the absolute user offset value for the given servo axis + * @throw MinorServoErrors::MinorServoErrorsEx when the given axis is unknown, when the sum of user and system offsets for the given axis are out of range, + * when there has been a communication error or when the command was not accepted + */ + void setUserOffset(in string axis_name, in double offset) raises (MinorServoErrors::MinorServoErrorsEx); + + /** + * This method sets the user offsets to zero for all the servo axes + * @throw MinorServoErrors::MinorServoErrorsEx when there has been a communication error or when the command was not accepted + */ + void clearUserOffsets() raises (MinorServoErrors::MinorServoErrorsEx); + + /** + * This method returns the system offsets of the servo + * @return the system offsets as a sequence of doubles + */ + ACS::doubleSeq getSystemOffsets(); + + /** + * This method sets the system offset of the servo for a given axis + * @param axis_name a string corresponding to the axis we want to set the offset for + * @param offset the absolute system offset value for the given servo axis + * @throw MinorServoErrors::MinorServoErrorsEx when the given axis is unknown, when the sum of user and system offsets for the given axis are out of range, + * when there has been a communication error or when the command was not accepted + */ + void setSystemOffset(in string axis_name, in double offset) raises (MinorServoErrors::MinorServoErrorsEx); + + /** + * This method sets the system offsets to zero for all the servo axes + * @throw MinorServoErrors::MinorServoErrorsEx when there has been a communication error or when the command was not accepted + */ + void clearSystemOffsets() raises (MinorServoErrors::MinorServoErrorsEx); + + /** + * This method resets the Leonardo offsets to the sum of the DISCOS user and system offsets. + * @throw MinorServoErrors::MinorServoErrorsEx when there has been a communication error or when the command was not accepted + */ + void reloadOffsets() raises (MinorServoErrors::MinorServoErrorsEx); + + /** + * This method returns in the two parameters passed as reference, the names and the units of measure of the axes of the servo, respectively + * @param axes_names the sequence of strings containing the names of the virtual axes of the servo + * @param axes_units the sequence of strings containing the units of measure of each virtual axis of the servo + */ + void getAxesInfo(out ACS::stringSeq axes_names, out ACS::stringSeq axes_units); + + /** + * This method returns the positions of all the axes of the servo for a given time + * @param acs_time the ACS::Time for which we want to retrieve the servo positions + * @throw MinorServoErrors::MinorServoErrorsEx when the position history is empty + * @return a sequence of doubles containing the positions of the servo for the given time + */ + ACS::doubleSeq getAxesPositions(in ACS::Time acs_time) raises (MinorServoErrors::MinorServoErrorsEx); + + /** + * This method returns the time it would take for the servo to get from a starting position to a destination position + * An empty starting position means the function will calculate the travel time from the current position, taking into account the current speed as well + * To account for the current speed, it is sufficient to call the method in this way: + * getTravelTime(ACS::doubleSeq(), ); + * @param starting_position the sequence of starting axes positions or an empty sequence + * @param destination_position the sequence of destination axes positions, mandatory + * @throw MinorServoErrors::MinorServoErrorsEx when receiving a starting position sequence of length different from zero or the number of virtual axes of the servo, + * when receiving a destination position sequence of lenght different from the number of virtual axes of the servo + * @return an ACS::TimeInterval object representing the total duration of the movement from the starting position to the destination position + */ + ACS::TimeInterval getTravelTime(in ACS::doubleSeq starting_position, in ACS::doubleSeq destination_position); + + /** + * This method returns the minimum and maximun ranges for all the servo axes in the given sequence passed by reference + * @param min_ranges the minimum range of the servo axes + * @param max_ranges the maximum range of the servo axes + */ + void getAxesRanges(out ACS::doubleSeq min_ranges, out ACS::doubleSeq max_ranges); + }; + + /** + * This IDL interface describes the CORBA interface of a generic SRTMinorServo component. + * This interface is derived from the SRTBaseMinorServo and it is empty. + * It's sole purpose is to generate the POA_MinorServo::SRTGenericMinorServo class needed for the components. + */ + interface SRTGenericMinorServo : SRTBaseMinorServo {}; + + /** + * This IDL interface describes the CORBA interface of a ProgramTrack-capable SRTMinorServo component. + * It extends the SRTBaseMinorServo interface with some more attributes and methods described below. + */ + interface SRTProgramTrackMinorServo : SRTBaseMinorServo + { + /** + * This property indicates whether the servo system is tracking the given trajectory + */ + readonly attribute Management::ROTBoolean tracking; + + /** + * This property indicates the ID of the trajectory. It always corresponds to starting time of the trajectory, expressed as the UNIX Epoch * 1000 (long) + */ + readonly attribute ACS::ROlong trajectory_id; + + /** + * This property indicates the total number of points loaded into the current trajectory + */ + readonly attribute ACS::ROlong total_trajectory_points; + + /** + * This property indicates the number of points of the current trajectory which remain to be tracked + */ + readonly attribute ACS::ROlong remaining_trajectory_points; + + /** + * This property returns the sequence of tracking error for each virtual axis of the servo + */ + readonly attribute ACS::ROdoubleSeq tracking_error; + + /** + * This method loads a set of coordinates that have to be tracked by the servo system + * @param trajectory_id the ID of the trajectory to which the current point belongs + * @param point_id the ID of the point inside the current trajectory. It must be a consecutive number for the servo system to acknowledge + * @param point_time an ACS::Time object indicating the time associated with the coordinates to be tracked. + * Only the start time is sent to the servo system and the points after are spaced by 0.2 seconds from one another. + * It is still necessary to fill this field in order to associate a time to the coordinates inside the component. + * @param coordinates the coordinates to track at the given time + * @throw MinorServoErrorsEx when the length of the coordinates sequence does not match the number of virtual axis of the servo system, + * when there has been a communication error or when the command was not accepted + */ + void programTrack(in long trajectory_id, in long point_id, in ACS::Time point_time, in ACS::doubleSeq coordinates); + + /** + * This method returns a boolean indicating if the servo is tracking or not + * @return true if the servo is tracking, false otherwise + */ + boolean isTracking(); + }; +}; + +#endif diff --git a/SRT/Interfaces/SRTMinorServoInterface/idl/SRTMinorServoBoss.idl b/SRT/Interfaces/SRTMinorServoInterface/idl/SRTMinorServoBoss.idl new file mode 100644 index 0000000000000000000000000000000000000000..9748e827e817b8a909bab388f050abbe3a52fa94 --- /dev/null +++ b/SRT/Interfaces/SRTMinorServoInterface/idl/SRTMinorServoBoss.idl @@ -0,0 +1,86 @@ +#ifndef __SRTMINORSERVOBOSS_IDL__ +#define __SRTMINORSERVOBOSS_IDL__ +/***************************************************************\ + * Authors: + * Giuseppe Carboni + * + * Created: Mon Mar 06 12:30:00 CEST 2023 + * Last Modified: Mon Mar 06 12:30:00 CEST 2023 +\***************************************************************/ + +#include +#include "SRTMinorServoCommon.idl" + +#pragma prefix "alma" + +module MinorServo +{ + interface SRTMinorServoBoss: MinorServoBoss + { + /** + * Boolean indicating whether the component is connected to the Leonardo Minor Servo system + */ + readonly attribute Management::ROTBoolean connected; + + /** + * Current configuration of the Leonardo minor servo system + */ + readonly attribute ROSRTMinorServoFocalConfiguration current_configuration; + + /** + * Boolean indicating whether the system is in simulation mode or not + */ + readonly attribute Management::ROTBoolean simulation_enabled; + + /** + * UNIX Epoch of the Leonardo minor servo system PLC machine + */ + readonly attribute ACS::ROdouble plc_time; + + /** + * Version of the software on the PLC machine + */ + readonly attribute ACS::ROstring plc_version; + + /** + * Enumeration indicating whether the system is controlled by VBrain or DISCOS + */ + readonly attribute ROSRTMinorServoControlStatus control; + + /** + * Boolean indicating whether all the axes are powered or not + */ + readonly attribute Management::ROTBoolean power; + + /** + * Boolean indicating whether the system entered an emergency status + */ + readonly attribute Management::ROTBoolean emergency; + + /** + * Position of the gregorian cover + */ + readonly attribute ROSRTMinorServoGregorianCoverStatus gregorian_cover; + + /** + * UNIX Epoch of the last executed command (STATUS commands are not shown here) + */ + readonly attribute ACS::ROdouble last_executed_command; + + /** + * This command sets all the minor servos involved in the current focal configuration to their position calculated from the elevation argument + * @param elevation, the elevation the minor servos should use to calculate their position + * @throw MinorServoErrors::MinorServoErrorsEx + */ + void preset(in double elevation) raises (MinorServoErrors::MinorServoErrorsEx); + + /** + * This command opens or closes the gregorian cover + * @param value, the desired gregorian cover position, accepted values are 'open', 'OPEN', 'closed' or 'CLOSED' + * @throw MinorServoErrors::MinorServoErrorsEx + */ + //void setGregorianCoverPosition(in string value) raises (MinorServoErrors::MinorServoErrorsEx); + }; +}; + +#endif diff --git a/SRT/Interfaces/SRTMinorServoInterface/idl/SRTMinorServoCommon.midl b/SRT/Interfaces/SRTMinorServoInterface/idl/SRTMinorServoCommon.midl new file mode 100644 index 0000000000000000000000000000000000000000..6addf4c0d3be8dba27452d68b362163d948e75b5 --- /dev/null +++ b/SRT/Interfaces/SRTMinorServoInterface/idl/SRTMinorServoCommon.midl @@ -0,0 +1,102 @@ +/***************************************************************\ + * Author: Giuseppe Carboni +\***************************************************************/ +#ifndef __SRTMINORSERVOCOMMON_MIDL__ +#define __SRTMINORSERVOCOMMON_MIDL__ + +#include +#include +#include +#include +#include +#include + +#pragma prefix "alma" + +module MinorServo +{ + /** + * Enumerator which indicates a Leonardo minor servo configuration + */ + enum SRTMinorServoFocalConfiguration + { + CONFIGURATION_UNKNOWN, + CONFIGURATION_PARK, + CONFIGURATION_PRIMARY, + CONFIGURATION_GREGORIAN1, + CONFIGURATION_GREGORIAN2, + CONFIGURATION_GREGORIAN3, + CONFIGURATION_GREGORIAN4, + CONFIGURATION_GREGORIAN5, + CONFIGURATION_GREGORIAN6, + CONFIGURATION_GREGORIAN7, + CONFIGURATION_GREGORIAN8, + CONFIGURATION_BWG1, + CONFIGURATION_BWG2, + CONFIGURATION_BWG3, + CONFIGURATION_BWG4 + }; + ACS_ENUM(SRTMinorServoFocalConfiguration); + + /** + * Enumerator which indicates the status of the control for the Leonardo minor servo system + */ + enum SRTMinorServoControlStatus + { + CONTROL_DISCOS, + CONTROL_VBRAIN + }; + ACS_ENUM(SRTMinorServoControlStatus); + + /** + * Enumerator which indicates the position of the gregorian cover + */ + enum SRTMinorServoGregorianCoverStatus + { + COVER_STATUS_UNKNOWN, + COVER_STATUS_CLOSED, + COVER_STATUS_OPEN + }; + ACS_ENUM(SRTMinorServoGregorianCoverStatus); + + /** + * Enumerator which indicates the status of a servo drive cabinet + */ + enum SRTMinorServoCabinetStatus + { + DRIVE_CABINET_OK, + DRIVE_CABINET_WARNING, + DRIVE_CABINET_ERROR + }; + ACS_ENUM(SRTMinorServoCabinetStatus); + + /** + * Enumerator which indicates the operating mode of a single servo + */ + enum SRTMinorServoOperativeMode + { + OPERATIVE_MODE_UNKNOWN, + OPERATIVE_MODE_SETUP, + OPERATIVE_MODE_STOW, + OPERATIVE_MODE_STOP, + OPERATIVE_MODE_PRESET, + OPERATIVE_MODE_PROGRAMTRACK + }; + ACS_ENUM(SRTMinorServoOperativeMode); + + /** + * Enumerator which indicates the type of movement made by the whole system (i.e.: performing setup, parking, tracking...) + */ + enum SRTMinorServoMotionStatus + { + MOTION_STATUS_UNCONFIGURED, + MOTION_STATUS_STARTING, + MOTION_STATUS_CONFIGURED, + MOTION_STATUS_TRACKING, + MOTION_STATUS_PARKING, + MOTION_STATUS_PARKED, + MOTION_STATUS_ERROR + }; + ACS_ENUM(SRTMinorServoMotionStatus); +}; +#endif diff --git a/SRT/Interfaces/SRTMinorServoInterface/src/Makefile b/SRT/Interfaces/SRTMinorServoInterface/src/Makefile new file mode 100644 index 0000000000000000000000000000000000000000..9cac56365c702b4372361e45dceed9178f460c00 --- /dev/null +++ b/SRT/Interfaces/SRTMinorServoInterface/src/Makefile @@ -0,0 +1,64 @@ +#************************************************************************************** +# E.S.O. - ACS project +# +# Makefile of SRTMinorServo IDL +# +#-------------------------------------------------------------------------------------- +# Giuseppe Carboni (giuseppe.carboni@inaf.it) +#-------------------------------------------------------------------------------------- +# +#************************************************************************************** + +#************************************************************************************** +# This Makefile follows VLT Standards (see Makefile(5) for more). +#************************************************************************************** + +MAKE_PDF = ON + +# list of all possible C-sources (used to create automatic dependencies) +# ------------------------------ +CSOURCENAMES = \ + $(foreach exe, $(EXECUTABLES) $(EXECUTABLES_L), $($(exe)_OBJECTS)) \ + $(foreach lib, $(LIBRARIES) $(LIBRARIES_L), $($(lib)_OBJECTS)) + +# +# IDL FILES +# +IDL_FILES = SRTMinorServoBoss SRTMinorServo SRTMinorServoCommon + +SRTMinorServoCommonStubs_LIBS = baciStubs ManagmentDefinitionsStubs MinorServoDefinitionsStubs ComponentErrorsStubs MinorServoErrorsStubs +SRTMinorServoStubs_LIBS = SRTMinorServoCommonStubs +SRTMinorServoBossStubs_LIBS = SRTMinorServoCommonStubs MinorServoBossStubs + +# +# INCLUDE STANDARDS +# ----------------- + +MAKEDIRTMP := $(shell searchFile include/acsMakefile) +ifneq ($(MAKEDIRTMP),\#error\#) + MAKEDIR := $(MAKEDIRTMP)/include + include $(MAKEDIR)/acsMakefile +endif + + +# TARGETS +all: do_all + @echo " . . . 'all' done" + +clean : clean_all + $(RM) *~ ../include/*~ ../idl/*~ ../*~ ../../*~ core + $(RM) ../doc/html + $(RM) tmp.txt acsexmplbeans.jar ../doc/abeans.log + @echo " . . . clean done" + +clean_dist : clean clean_dist_all + @echo " . . . clean_dist done" + +man : do_man + # cp ../doc/html/group__ACSEXMPLDOC.html ../doc/html/main.html + @echo " . . . man page(s) done" + +install : install_all + @echo " . . . installation done" + +#___oOo___ diff --git a/SRT/Interfaces/SRTMinorServoInterface/test/Makefile b/SRT/Interfaces/SRTMinorServoInterface/test/Makefile new file mode 100644 index 0000000000000000000000000000000000000000..e70daa63e47a9486f30106ddfcb5f37ec19104ff --- /dev/null +++ b/SRT/Interfaces/SRTMinorServoInterface/test/Makefile @@ -0,0 +1,92 @@ +# CPP UNIT TESTING SETUP +#-------------- +# GTEST_HOME=/usr/local/include/gtest +# GMOCK_HOME=/usr/local/include/gmock +# GTEST_LIBS=gtest gtest_main + +# USER_INC=-I$(GTEST_HOME) -I$(GMOCK_HOME) +# USER_LIBS=C++ pthread +# END OF CPP UNIT TESTING SETUP +#--------------------- + +# DEFINE YOUR CPP UNIT TEST EXECUTABLES HERE as: +# +# EXECTUABLES_L = unittest +# unittest_OBJECTS = unittest +# unittest_LIBS = $(GTEST_LIBS) + +# EXECUTABLES_L = unittest +# unittest_OBJECTS = unittest +# unittest_LIBS = $(GTEST_LIBS) + +# END OF CUSTOMIZATION +# do not edit below this line +#---------------------------- + +CSOURCENAMES = \ + $(foreach exe, $(EXECUTABLES) $(EXECUTABLES_L), $($(exe)_OBJECTS)) \ + $(foreach rtos, $(RTAI_MODULES) , $($(rtos)_OBJECTS)) \ + $(foreach lib, $(LIBRARIES) $(LIBRARIES_L), $($(lib)_OBJECTS)) + +MAKEDIRTMP := $(shell searchFile include/acsMakefile) +ifneq ($(MAKEDIRTMP),\#error\#) + MAKEDIR := $(MAKEDIRTMP)/include + include $(MAKEDIR)/acsMakefile +endif + +# TEST TARGETS +#TODO: unittest(2) discover pyunit + +do_unit: all + @echo "running cpp unit tests" + ../bin/unittest --gtest_output=xml:results/cppunittest.xml + +do_pyunit: + @echo "running python unit tests" + python -m unittest pyunit + +do_functional: + @echo "running python functional tests" + python -m unittest functional + python -m unittest functional/commands + +do_external: + @echo "running python external tests" + python -m unittest external + +clean_test: + rm -f results/*.xml + rm -f functional/*.pyc + rm -f pyunit/*.pyc + rm -f external/*.pyc + rm -rf ../lib/python/site-packages/* + +unit: do_unit + @echo " . . . 'unit' done" + +pyunit: do_pyunit + @echo " . . . 'pyunit' done" + +functional: do_functional + @echo " . . . 'functional' done" + +external: do_external + @echo " . . . 'external' done" + +# TARGETS +# ------- +all: do_all + @echo " . . . 'all' done" + +clean : clean_all clean_test + @echo " . . . clean done" + +clean_dist : clean_all clean_dist_all clean_test + @echo " . . . clean_dist done" + +man : do_man + @echo " . . . man page(s) done" + +install : install_all + @echo " . . . installation done" + diff --git a/SRT/Servers/SRTMinorServo/test/external/__init__.py b/SRT/Interfaces/SRTMinorServoInterface/test/external/__init__.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/external/__init__.py rename to SRT/Interfaces/SRTMinorServoInterface/test/external/__init__.py diff --git a/SRT/Servers/SRTMinorServo/test/functional/__init__.py b/SRT/Interfaces/SRTMinorServoInterface/test/functional/__init__.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/functional/__init__.py rename to SRT/Interfaces/SRTMinorServoInterface/test/functional/__init__.py diff --git a/SRT/Servers/SRTMinorServo/test/pyunit/__init__.py b/SRT/Interfaces/SRTMinorServoInterface/test/functional/commands/__init__.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/pyunit/__init__.py rename to SRT/Interfaces/SRTMinorServoInterface/test/functional/commands/__init__.py diff --git a/SRT/Interfaces/SRTMinorServoInterface/test/functional/commands/test_servoSetup.py b/SRT/Interfaces/SRTMinorServoInterface/test/functional/commands/test_servoSetup.py new file mode 100644 index 0000000000000000000000000000000000000000..aa862cae023e2ef745a3d8516fdd8a9580296eac --- /dev/null +++ b/SRT/Interfaces/SRTMinorServoInterface/test/functional/commands/test_servoSetup.py @@ -0,0 +1,62 @@ +from __future__ import with_statement + +import os +import math +import time +import unittest + +import MinorServo +import Management +import Antenna + +from Acspy.Clients.SimpleClient import PySimpleClient +from MinorServoErrors import MinorServoErrorsEx +from Acspy.Common.TimeHelper import getTimeStamp + +__author__ = "Marco Buttu " + + +class TestServoSetupCmd(unittest.TestCase): + """Test the servoSetup command""" + + telescope = os.getenv('STATION') + + def setUp(self): + self.client = PySimpleClient() + self.boss = self.client.getComponent('MINORSERVO/Boss') + self.setup_code = "CCB" if self.telescope == "SRT" else "CCC" + + def tearDown(self): + self.boss.park() + time.sleep(0.2) + self.wait_parked() + self.client.releaseComponent('MINORSERVO/Boss') + + def test_wrong_code(self): + success, answer = self.boss.command('servoSetup=FOO') + self.assertFalse(success) + time.sleep(0.2) + if self.boss.isStarting(): + self.wait_ready() + + def test_right_code(self): + success, answer = self.boss.command('servoSetup=' + self.setup_code) + self.assertTrue(success) + time.sleep(0.2) + if self.boss.isStarting(): + self.wait_ready() + + def wait_ready(self): + while not self.boss.isReady(): + time.sleep(0.1) + + def wait_parked(self): + while self.boss.isParking(): + time.sleep(0.1) + +if __name__ == '__main__': + if 'Configuration' in os.getenv('ACS_CDB'): + unittest.main() # Real test using the antenna CDB + else: + from PyMinorServoTest import simunittest + simunittest.run(TestServoSetupCmd) diff --git a/SRT/Interfaces/SRTMinorServoInterface/test/functional/commands/test_setServoASConfiguration.py b/SRT/Interfaces/SRTMinorServoInterface/test/functional/commands/test_setServoASConfiguration.py new file mode 100644 index 0000000000000000000000000000000000000000..a4d0b257d144262fa87e7b4f801653999ffbe810 --- /dev/null +++ b/SRT/Interfaces/SRTMinorServoInterface/test/functional/commands/test_setServoASConfiguration.py @@ -0,0 +1,44 @@ +from __future__ import with_statement + +import os +import math +import time +import unittest + +import MinorServo +import Management +import Antenna + +from PyMinorServoTest import simunittest +from Acspy.Clients.SimpleClient import PySimpleClient +from MinorServoErrors import MinorServoErrorsEx +from Acspy.Common.TimeHelper import getTimeStamp + +__author__ = "Marco Buttu " + + +class TestSetServoASConfigurationCmd(unittest.TestCase): + """Test the setServoASConfiguration command""" + + telescope = os.getenv('STATION') + + def setUp(self): + self.client = PySimpleClient() + self.boss = self.client.getComponent('MINORSERVO/Boss') + + def tearDown(self): + self.client.releaseComponent('MINORSERVO/Boss') + + def test_wrong_axis_code(self): + success, answer = self.boss.command('setServoASConfiguration=FOO') + self.assertFalse(success) + + def test_right_axis_code(self): + success, answer = self.boss.command('setServoASConfiguration=on') + self.assertTrue(success) + +if __name__ == '__main__': + if 'Configuration' in os.getenv('ACS_CDB'): + unittest.main() # Real test using the antenna CDB + else: + simunittest.run(TestSetServoASConfigurationCmd) diff --git a/SRT/Interfaces/SRTMinorServoInterface/test/functional/commands/test_setServoElevationTracking.py b/SRT/Interfaces/SRTMinorServoInterface/test/functional/commands/test_setServoElevationTracking.py new file mode 100644 index 0000000000000000000000000000000000000000..7d67f7ad17e8c49b29f56006dd71ead1355dfefa --- /dev/null +++ b/SRT/Interfaces/SRTMinorServoInterface/test/functional/commands/test_setServoElevationTracking.py @@ -0,0 +1,46 @@ +from __future__ import with_statement + +import os +import math +import time +import unittest + +import MinorServo +import Management +import Antenna + +from Acspy.Clients.SimpleClient import PySimpleClient +from MinorServoErrors import MinorServoErrorsEx +from Acspy.Common.TimeHelper import getTimeStamp + +__author__ = "Marco Buttu " + + +class TestSetServoElevationTrackingCmd(unittest.TestCase): + """Test the setServoElevationTracking command""" + + telescope = os.getenv('STATION') + + def setUp(self): + self.client = PySimpleClient() + self.boss = self.client.getComponent('MINORSERVO/Boss') + + def tearDown(self): + self.client.releaseComponent('MINORSERVO/Boss') + + def test_wrong_flag(self): + success, answer = self.boss.command('setServoElevationTracking=FOO') + self.assertFalse(success) + + def test_right_flag(self): + #FIXME: we need servoSetup before this + success, answer = self.boss.command('setServoElevationTracking=on') + self.assertTrue(success) + + +if __name__ == '__main__': + if 'Configuration' in os.getenv('ACS_CDB'): + unittest.main() # Real test using the antenna CDB + else: + from PyMinorServoTest import simunittest + simunittest.run(TestSetServoElevationTrackingCmd) diff --git a/SRT/Interfaces/SRTMinorServoInterface/test/functional/commands/test_setServoOffset.py b/SRT/Interfaces/SRTMinorServoInterface/test/functional/commands/test_setServoOffset.py new file mode 100644 index 0000000000000000000000000000000000000000..64888d527253e1b121befa304476279ac76b07f5 --- /dev/null +++ b/SRT/Interfaces/SRTMinorServoInterface/test/functional/commands/test_setServoOffset.py @@ -0,0 +1,38 @@ +from __future__ import with_statement + +import os +import math +import time +import unittest + +import MinorServo +import Management +import Antenna + +from Acspy.Clients.SimpleClient import PySimpleClient +from MinorServoErrors import MinorServoErrorsEx +from Acspy.Common.TimeHelper import getTimeStamp + +__author__ = "Marco Buttu " + + +class TestSetServoOffsetCmd(unittest.TestCase): + """Test the setServoOffset command""" + + telescope = os.getenv('STATION') + + def setUp(self): + self.client = PySimpleClient() + self.boss = self.client.getComponent('MINORSERVO/Boss') + + def tearDown(self): + self.boss.park() + self.client.releaseComponent('MINORSERVO/Boss') + + def test_wrong_axis_code(self): + success, answer = self.boss.command('setServoOffset=FOO_TX,0') + self.assertFalse(success) + + +if __name__ == '__main__': + unittest.main() diff --git a/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_clearUserOffset.py b/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_clearUserOffset.py new file mode 100644 index 0000000000000000000000000000000000000000..98ab9110f771c22df81139a9627beea0c12e1661 --- /dev/null +++ b/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_clearUserOffset.py @@ -0,0 +1,58 @@ +from __future__ import with_statement + +import os +import math +import time +import unittest + +import MinorServo +import Management +import Antenna + +from Acspy.Clients.SimpleClient import PySimpleClient +from MinorServoErrors import MinorServoErrorsEx +from Acspy.Common.TimeHelper import getTimeStamp + +from acswrapper.system import acs +from acswrapper.containers import ( + Container, ContainerError, start_containers_and_wait, + stop_containers_and_wait +) + +__author__ = "Marco Buttu " + + +class TestClearUserOffset(unittest.TestCase): + + @classmethod + def setUpClass(cls): + if not acs.is_running(): + acs.start() + cls.containers = [ + Container('MinorServoContainer', 'cpp'), + Container('MinorServoBossContainer', 'cpp'), + ] + try: + start_containers_and_wait(cls.containers) + except ContainerError, ex: + cls.fail(ex.message) + + cls.client = PySimpleClient() + cls.boss = cls.client.getComponent('MINORSERVO/Boss') + + @classmethod + def tearDownClass(cls): + cls.client.releaseComponent('MINORSERVO/Boss') + stop_containers_and_wait(cls.containers) + + def test_wrong_servo_name(self): + """Raise a MinorServoErrorsEx in case of wrong servo name""" + with self.assertRaises(MinorServoErrorsEx): + self.boss.clearUserOffset('FOO') + +if __name__ == '__main__': + if 'Configuration' in os.getenv('ACS_CDB'): + unittest.main() # Real test using the antenna CDB + else: + from testing import simulator + simulator.run(TestClearUserOffset, 'srt-mscu-sim') diff --git a/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_getAxesInfo.py b/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_getAxesInfo.py new file mode 100644 index 0000000000000000000000000000000000000000..ed2b37f2cece596d2397231c5567450dcb9557e2 --- /dev/null +++ b/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_getAxesInfo.py @@ -0,0 +1,92 @@ +from __future__ import with_statement + +import os +import math +import time +import datetime + +import unittest + +import MinorServo +import Management +import Antenna + +from Acspy.Clients.SimpleClient import PySimpleClient +from MinorServoErrors import MinorServoErrorsEx +from Acspy.Common.TimeHelper import getTimeStamp + + +from acswrapper.system import acs +from acswrapper.containers import ( + Container, ContainerError, start_containers_and_wait, + stop_containers_and_wait +) + +__author__ = "Marco Buttu " + + +class TestGetAxesInfo(unittest.TestCase): + + telescope = os.getenv('STATION') + + @classmethod + def setUpClass(cls): + if not acs.is_running(): + acs.start() + cls.containers = [ + Container('MinorServoContainer', 'cpp'), + Container('MinorServoBossContainer', 'cpp'), + ] + try: + start_containers_and_wait(cls.containers) + except ContainerError, ex: + cls.fail(ex.message) + + cls.client = PySimpleClient() + cls.boss = cls.client.getComponent('MINORSERVO/Boss') + + @classmethod + def tearDownClass(cls): + cls.client.releaseComponent('MINORSERVO/Boss') + cls.client.disconnect() + stop_containers_and_wait(cls.containers) + + def setUp(self): + self.setup_code = "CCB" if self.telescope == "SRT" else "CCC" + + def tearDown(self): + self.boss.park() + time.sleep(0.2) + self.wait_parked() + + def test_not_ready(self): + """Raise a MinorServoErrorsEx in case the system is not ready""" + with self.assertRaises(MinorServoErrorsEx): + axes, units = self.boss.getAxesInfo() + + def test_ready(self): + """Get the axes information""" + self.boss.setup(self.setup_code) + counter = 0 # Seconds + now = time_ref = datetime.datetime.now() + while not self.boss.isReady() or (time_ref - now).seconds < 20: + time.sleep(1) + now = datetime.datetime.now() + + axes, units = self.boss.getAxesInfo() + self.assertTrue(any(axes)) + self.assertTrue(any(units)) + self.assertEqual(len(units), len(axes)) + + def wait_parked(self): + while self.boss.isParking(): + time.sleep(0.1) + + +if __name__ == '__main__': + if 'Configuration' in os.getenv('ACS_CDB'): + unittest.main() # Real test using the antenna CDB + else: + from testing import simulator + simulator.run(TestGetAxesInfo, 'srt-mscu-sim') + diff --git a/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_getAxesPosition.py b/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_getAxesPosition.py new file mode 100644 index 0000000000000000000000000000000000000000..7a1f575113c9dc6fb1f16c35158ae20f83873e0f --- /dev/null +++ b/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_getAxesPosition.py @@ -0,0 +1,87 @@ +from __future__ import with_statement + +import os +import math +import time +import datetime + +import unittest + +import MinorServo +import Management +import Antenna + +from Acspy.Clients.SimpleClient import PySimpleClient +from MinorServoErrors import MinorServoErrorsEx +from Acspy.Common.TimeHelper import getTimeStamp + +from acswrapper.system import acs +from acswrapper.containers import ( + Container, ContainerError, start_containers_and_wait, + stop_containers_and_wait +) + +__author__ = "Marco Buttu " + + +class TestGetAxesPosition(unittest.TestCase): + + telescope = os.getenv('STATION') + + @classmethod + def setUpClass(cls): + if not acs.is_running(): + acs.start() + cls.containers = [ + Container('MinorServoContainer', 'cpp'), + Container('MinorServoBossContainer', 'cpp'), + ] + try: + start_containers_and_wait(cls.containers) + except ContainerError, ex: + cls.fail(ex.message) + cls.client = PySimpleClient() + cls.boss = cls.client.getComponent('MINORSERVO/Boss') + + @classmethod + def tearDownClass(cls): + cls.client.releaseComponent('MINORSERVO/Boss') + stop_containers_and_wait(cls.containers) + + def setUp(self): + self.setup_code = "CCB" if self.telescope == "SRT" else "CCC" + + def tearDown(self): + self.boss.park() + time.sleep(0.2) + self.wait_parked() + + def test_not_ready(self): + """Raise a MinorServoErrorsEx in case the system is not ready""" + with self.assertRaises(MinorServoErrorsEx): + position = self.boss.getAxesPosition(0) + + def test_ready(self): + """Get the axes position""" + self.boss.setup(self.setup_code) + counter = 0 # Seconds + now = time_ref = datetime.datetime.now() + while not self.boss.isReady() or (time_ref - now).seconds < 20: + time.sleep(1) + now = datetime.datetime.now() + + position = self.boss.getAxesPosition(0) + self.assertTrue(any(position)) + + def wait_parked(self): + while self.boss.isParking(): + time.sleep(0.1) + + +if __name__ == '__main__': + if 'Configuration' in os.getenv('ACS_CDB'): + unittest.main() # Real test using the antenna CDB + else: + from testing import simulator + simulator.run(TestGetAxesPosition, 'srt-mscu-sim') + diff --git a/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_getCentralScanPosition.py b/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_getCentralScanPosition.py new file mode 100644 index 0000000000000000000000000000000000000000..600c3ea58e2b92473272b1d9d6d85c52cbde0456 --- /dev/null +++ b/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_getCentralScanPosition.py @@ -0,0 +1,58 @@ +from __future__ import with_statement + +import os + +import unittest + +from Acspy.Clients.SimpleClient import PySimpleClient +from MinorServoErrors import MinorServoErrorsEx + +from acswrapper.system import acs +from acswrapper.containers import ( + Container, ContainerError, start_containers_and_wait, + stop_containers_and_wait +) + +__author__ = "Marco Buttu " + +class TestGetCentralScanPosition(unittest.TestCase): + + telescope = os.getenv('STATION') + + @classmethod + def setUpClass(cls): + if not acs.is_running(): + acs.start() + cls.containers = [ + Container('MinorServoContainer', 'cpp'), + Container('MinorServoBossContainer', 'cpp'), + ] + try: + start_containers_and_wait(cls.containers) + except ContainerError, ex: + cls.fail(ex.message) + + @classmethod + def tearDownClass(cls): + stop_containers_and_wait(cls.containers) + + def setUp(self): + self.client = PySimpleClient() + self.boss = self.client.getComponent('MINORSERVO/Boss') + + def tearDown(self): + self.client.releaseComponent('MINORSERVO/Boss') + + def test_scan_not_active(self): + """Raise a MinorServoErrorsEx if the scan is not active""" + with self.assertRaises(MinorServoErrorsEx): + self.boss.getCentralScanPosition() + + +if __name__ == '__main__': + if 'Configuration' in os.getenv('ACS_CDB'): + unittest.main() # Real test using the antenna CDB + else: + from testing import simulator + simulator.run(TestGetCentralScanPosition, 'srt-mscu-sim') + diff --git a/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_position.py b/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_position.py new file mode 100644 index 0000000000000000000000000000000000000000..892aa9be6698885a53d252431c3d860618b4c7db --- /dev/null +++ b/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_position.py @@ -0,0 +1,126 @@ +from __future__ import with_statement +import random +import math +import time +import os +from datetime import datetime + +import unittest +import Management +import MinorServo +import Antenna + +from MinorServoErrors import MinorServoErrorsEx +from Acspy.Common.TimeHelper import getTimeStamp +from Acspy.Clients.SimpleClient import PySimpleClient +from Acspy.Util import ACSCorba + +from acswrapper.system import acs +from acswrapper.containers import ( + Container, ContainerError, start_containers_and_wait, + stop_containers_and_wait +) + +__author__ = "Marco Buttu " + + +class PositionTest(unittest.TestCase): + + telescope = os.getenv('STATION') + + @classmethod + def setUpClass(cls): + if not acs.is_running(): + acs.start() + cls.containers = [ + Container('MinorServoContainer', 'cpp'), + Container('MinorServoBossContainer', 'cpp'), + ] + try: + start_containers_and_wait(cls.containers) + except ContainerError, ex: + cls.fail(ex.message) + cls.client = PySimpleClient() + cls.boss = cls.client.getComponent('MINORSERVO/Boss') + + @classmethod + def tearDownClass(cls): + cls.client.releaseComponent('MINORSERVO/Boss') + stop_containers_and_wait(cls.containers) + + def setUp(self): + self.axis_code='SRP_TZ' if self.telescope == 'SRT' else 'Z' + setupCode = 'KKG' if self.telescope == 'SRT' else 'CCC' + # Wait (maximum one minute) in case the boss is parking + if self.boss.isParking(): + t0 = datetime.now() + while self.boss.isParking() and (datetime.now() - t0).seconds < 60: + time.sleep(2) + if self.boss.isParking(): + self.fail('The system can not exit form a parking state') + + if self.boss.getActualSetup() != setupCode: + self.boss.setup(setupCode) + # Wait (maximum 5 minutes) in case the boss is starting + t0 = datetime.now() + while not self.boss.isReady() and (datetime.now() - t0).seconds < 60*5: + time.sleep(2) + if not self.boss.isReady(): + self.fail('The system is not ready for executing the tests') + self.boss.setElevationTracking('OFF') + self.boss.setASConfiguration('OFF') + axes, units = self.boss.getAxesInfo() + self.idx = axes.index(self.axis_code) + + def tearDown(self): + # self.boss.clearUserOffset(self.axis_code) + self.boss.setUserOffset(self.axis_code, 0) + self.wait_tracking() + + def test_get_current_position(self): + timestamp = getTimeStamp().value + position = self.get_position() + position_now = self.get_position(timestamp) + self.assertAlmostEqual(position, position_now, delta=0.1) + + def test_get_offset_position(self): + position = self.get_position() + self.boss.setUserOffset(self.axis_code, 10) + self.wait_tracking() + position_now = self.get_position() + self.assertAlmostEqual(position_now, position + 10, delta=0.1) + + def test_get_past_position(self): + timestamp = getTimeStamp().value + position = self.get_position() + self.boss.setUserOffset(self.axis_code, 10) + self.wait_tracking() + position_past = self.get_position(timestamp) + self.assertAlmostEqual(position, position_past, delta=0.1) + + def test_get_past_position_with_sleep(self): + timestamp = getTimeStamp().value + position = self.get_position() + self.boss.setUserOffset(self.axis_code, 10) + self.wait_tracking() + time.sleep(10) + position_past = self.get_position(timestamp) + self.assertAlmostEqual(position, position_past, delta=0.1) + + def wait_tracking(self): + time.sleep(1) # Give the time to command the new position + # TODO: we need a better solution than this sleep to be sure the tests + # procuce always the same results + while not self.boss.isTracking(): + time.sleep(0.1) + + def get_position(self, timestamp=0): + return self.boss.getAxesPosition(timestamp)[self.idx] + + +if __name__ == '__main__': + if 'Configuration' in os.getenv('ACS_CDB'): + unittest.main(verbosity=2, failfast=True) # Real test using the antenna CDB + else: + from testing import simulator + simulator.run(PositionTest, 'srt-mscu-sim') diff --git a/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_scan.py b/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_scan.py new file mode 100644 index 0000000000000000000000000000000000000000..1c0c266f807bbd97a4d9e70f4c2ce0498752371d --- /dev/null +++ b/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_scan.py @@ -0,0 +1,430 @@ +from __future__ import with_statement +import random +import math +import time +import os +from datetime import datetime + +import unittest +import Management +import MinorServo +import Antenna + +from MinorServoErrors import MinorServoErrorsEx +from Acspy.Common.TimeHelper import getTimeStamp +from Acspy.Clients.SimpleClient import PySimpleClient +from Acspy.Util import ACSCorba + +from acswrapper.system import acs +from acswrapper.containers import ( + Container, ContainerError, start_containers_and_wait, + stop_containers_and_wait +) + +__author__ = "Marco Buttu " + + +class ScanBaseTest(unittest.TestCase): + + telescope = os.getenv('STATION') + + @classmethod + def setUpClass(cls): + if not acs.is_running(): + acs.start() + cls.containers = [ + Container('MinorServoContainer', 'cpp'), + Container('MinorServoBossContainer', 'cpp'), + ] + try: + start_containers_and_wait(cls.containers) + except ContainerError, ex: + cls.fail(ex.message) + cls.client = PySimpleClient() + cls.boss = cls.client.getComponent('MINORSERVO/Boss') + + @classmethod + def tearDownClass(cls): + cls.client.releaseComponent('MINORSERVO/Boss') + cls.client.disconnect() + stop_containers_and_wait(cls.containers) + + def setUp(self): + self.antennaInfo = Antenna.TRunTimeParameters( + targetName='dummy', + azimuth=math.pi, + elevation=math.pi/2 * 1/random.randrange(2, 10), + rightAscension=0, + declination=0, + startEpoch=getTimeStamp().value + 100000000, + onTheFly=False, + slewingTime=100000000, + section=Antenna.ANT_SOUTH, + axis=Management.MNG_TRACK, + timeToStop=0) + + self.scan = MinorServo.MinorServoScan( + range=20, + total_time=100000000, # 10 seconds + axis_code='SRP_TZ' if self.telescope == 'SRT' else 'Z', + is_empty_scan=False) + + def tearDown(self): + if self.boss.isScanActive(): + t = self.boss.closeScan() + self.waitUntilTime(t) + + def waitUntilTime(self, targetTime): + while getTimeStamp().value < targetTime: + time.sleep(0.1) + + def waitUntil(self, action, value): + """For instance: waitUntil(isReady, True)""" + while action() != value: + time.sleep(0.1) + + +class ScanTest(ScanBaseTest): + """Test checkScan(), startScan() and closeScan()""" + + def setUp(self): + super(ScanTest, self).setUp() + setupCode = 'KKG' if self.telescope == 'SRT' else 'CCC' + + # Wait (maximum one minute) in case the boss is parking + if self.boss.isParking(): + t0 = datetime.now() + while self.boss.isParking() and (datetime.now() - t0).seconds < 60: + time.sleep(2) + if self.boss.isParking(): + self.fail('The system can not exit form a parking state') + + if self.boss.getActualSetup() != setupCode or not self.boss.isReady(): + self.boss.setup(setupCode) + + # Wait (maximum 5 minutes) in case the boss is starting + t0 = datetime.now() + while not self.boss.isReady() and (datetime.now() - t0).seconds < 60*5: + time.sleep(2) + + if not self.boss.isReady(): + self.fail('The system is not ready for executing the tests') + + self.boss.setElevationTracking('OFF') + self.boss.setASConfiguration('OFF') + axes, units = self.boss.getAxesInfo() + self.idx = axes.index(self.scan.axis_code) + + getPosition = getattr(self, 'get%sPosition' %self.telescope) + centerScanPosition = getPosition( + self.boss.getActualSetup(), + 'SRP', + math.degrees(self.antennaInfo.elevation)) + self.centerScan = centerScanPosition[self.idx] + + def test_startScan_empty_scan_system_ready(self): + """Do nothing in case of empty scan and system ready""" + self.scan.is_empty_scan = True + startTime = 0 + self.boss.startScan(startTime, self.scan, self.antennaInfo) + self.assertFalse(self.boss.isScanActive()) + + def test_startScan_empty_scan_system_not_ready(self): + """Do nothing in case of empty scan and system NOT ready""" + self.scan.is_empty_scan = True + startTime = 0 + self.boss.park() + self.waitUntil(self.boss.isReady, False) + self.assertFalse(self.boss.isReady()) + self.boss.startScan(startTime, self.scan, self.antennaInfo) + self.assertFalse(self.boss.isScanActive()) + + def test_startScan_ASAP(self): + """Starting time unknown: the scan must start ASAP""" + startTime = 0 + t = self.boss.startScan(startTime, self.scan, self.antennaInfo) + self.assertGreater(t, getTimeStamp().value) + self.isAssertScan(t) + + def test_startScan_ASAP_at_checkScan_time(self): + """Starting time given by checkScan()""" + startTime = 0 + res, msInfo = self.boss.checkScan(startTime, self.scan, self.antennaInfo) + startTime = msInfo.startEpoch + t = self.boss.startScan(startTime, self.scan, self.antennaInfo) + self.assertGreater(t, getTimeStamp().value) + self.isAssertScan(t) + + def test_startScan_at_given_time(self): + """Start at given time""" + startTime = getTimeStamp().value + 60*10**7 # Start in one minute + t = self.boss.startScan(startTime, self.scan, self.antennaInfo) + self.assertEqual(t, startTime) + self.isAssertScan(t) + + def test_startScan_too_fast(self): + """Servo not enough fast for accomplishing the scan in total_time""" + startTime = getTimeStamp().value + 60*10**7 # Start in one minute + self.scan.total_time = 5000000 # 0.5 seconds + with self.assertRaises(MinorServoErrorsEx): + self.boss.startScan(startTime, self.scan, self.antennaInfo) + + def test_startScan_out_of_range(self): + """Scan out of the servo position limits""" + startTime = 0 + self.scan.range = 5000 # 5 meters + with self.assertRaises(MinorServoErrorsEx): + self.boss.startScan(startTime, self.scan, self.antennaInfo) + + def test_startScan_time_too_close_to_now(self): + """Starting time too close to the current time""" + startTime = getTimeStamp().value + 1*10**7 # Start in 1 second from now + with self.assertRaises(MinorServoErrorsEx): + self.boss.startScan(startTime, self.scan, self.antennaInfo) + + def test_closeScan_time_to_stop(self): + """Return the time_to_stop""" + startTime = 0 + t = self.boss.startScan(startTime, self.scan, self.antennaInfo) + self.waitUntilTime(startTime) + time_to_stop = self.boss.closeScan() + # The time_to_stop should be greater than now + self.assertGreater(time_to_stop, getTimeStamp().value) + + def test_checkScan_with_scanActive(self): + """checkScan() raises an exception in case there is an active scan.""" + startTime = getTimeStamp().value + 15*10**7 # Start in 15 seconds + self.boss.startScan(startTime, self.scan, self.antennaInfo) + # Wait untill the scan finishes (one second after the scan) + targetTime = startTime + self.scan.total_time + 1*10**7 + self.waitUntilTime(targetTime) + with self.assertRaises(MinorServoErrorsEx): + self.boss.checkScan(startTime, self.scan, self.antennaInfo) + + def test_checkScan_with_scan_in_execution(self): + """checkScan() raises an exception in case there is a scan in execution.""" + startTime = getTimeStamp().value + 15*10**7 # Start in 15 seconds + self.boss.startScan(startTime, self.scan, self.antennaInfo) + # Wait untill the scan starts + targetTime = startTime + 2*10**7 # 2 seconds after the starting time + self.waitUntilTime(targetTime) + with self.assertRaises(MinorServoErrorsEx): + self.boss.checkScan(startTime, self.scan, self.antennaInfo) + + def test_checkScan_empty_scan_start_ASAP(self): + """Starting time unknown: the scan must start ASAP""" + startTime = 0 + self.scan.is_empty_scan = True + + res, msInfo = self.boss.checkScan(startTime, self.scan, self.antennaInfo) + self.assertTrue(res) + self.assertAlmostEqual(msInfo.centerScan, self.centerScan, delta=0.01) + self.assertGreater(msInfo.startEpoch, getTimeStamp().value) + self.assertEqual(msInfo.scanAxis, self.scan.axis_code) + self.assertEqual(msInfo.timeToStop, + msInfo.startEpoch + self.scan.total_time) + + def test_checkScan_not_empty_scan_start_ASAP(self): + """Scan not empty: starting time unknown, the scan must start ASAP""" + startTime = 0 + + res, msInfo = self.boss.checkScan(startTime, self.scan, self.antennaInfo) + self.assertTrue(res) + self.assertAlmostEqual(msInfo.centerScan, self.centerScan, delta=0.01) + self.assertTrue(msInfo.onTheFly) + self.assertGreater(msInfo.startEpoch, getTimeStamp().value) + self.assertEqual(msInfo.scanAxis, self.scan.axis_code) + self.assertEqual( + msInfo.timeToStop, + msInfo.startEpoch + self.scan.total_time) + + def test_checkScan_empty_scan_start_at_given_time(self): + """Starting time known and achievable""" + startTime = getTimeStamp().value + 60*10**7 # Start in a minute + self.scan.is_empty_scan = True + + res, msInfo = self.boss.checkScan(startTime, self.scan, self.antennaInfo) + self.assertTrue(res) + self.assertAlmostEqual(msInfo.centerScan, self.centerScan, delta=0.01) + self.assertFalse(msInfo.onTheFly) + self.assertEqual(msInfo.startEpoch, startTime) + self.assertEqual(msInfo.scanAxis, self.scan.axis_code) + self.assertEqual( + msInfo.timeToStop, + msInfo.startEpoch + self.scan.total_time) + + def test_checkScan_not_empty_scan_start_at_given_time(self): + """Scan not empty: starting time known and achievable""" + startTime = getTimeStamp().value + 60*10**7 # Start in a minute + + res, msInfo = self.boss.checkScan(startTime, self.scan, self.antennaInfo) + self.assertTrue(res) + self.assertAlmostEqual(msInfo.centerScan, self.centerScan, delta=0.01) + self.assertTrue(msInfo.onTheFly) + self.assertEqual(msInfo.startEpoch, startTime) + self.assertEqual(msInfo.scanAxis, self.scan.axis_code) + self.assertEqual( + msInfo.timeToStop, + msInfo.startEpoch + self.scan.total_time) + + def test_checkScan_too_fast(self): + """Servo not enough fast for accomplishing the scan in total_time""" + startTime = getTimeStamp().value + 60*10**7 # Start in a minute from now + self.scan.total_time = 5000000 # 0.5 seconds + res, msInfo = self.boss.checkScan(startTime, self.scan, self.antennaInfo) + self.assertFalse(res) + + def test_checkScan_out_of_range(self): + """The scan goes out of the servo position limits""" + startTime = 0 + self.scan.range = 1000 # 1 meter + res, msInfo = self.boss.checkScan(startTime, self.scan, self.antennaInfo) + self.assertFalse(res) + + def test_checkScan_start_time_too_close_to_now(self): + """Starting time too close to the current time""" + startTime = getTimeStamp().value + 1*10**6 # Start in 0.1 second from now + res, msInfo = self.boss.checkScan(startTime, self.scan, self.antennaInfo) + self.assertFalse(res) + + def isAssertScan(self, startTime): + self.assertFalse(self.boss.isScanning()) + self.assertTrue(self.boss.isScanActive()) + # Assertions to verify right after startTime + self.waitUntilTime(startTime) + self.assertTrue(self.boss.isScanning()) + self.assertTrue(self.boss.isScanActive()) + self.assertAlmostEqual( + self.boss.getCentralScanPosition(), + self.centerScan, + delta=0.1) + # Wait untill the scan finishes (one second after the scan) + targetTime = startTime + self.scan.total_time + 1*10**7 + self.waitUntilTime(targetTime) + startPos = self.boss.getAxesPosition(startTime)[self.idx] + endPos = self.boss.getAxesPosition(targetTime)[self.idx] + self.assertTrue(self.boss.isScanActive()) + self.assertFalse(self.boss.isScanning()) + self.assertAlmostEqual(startPos + self.scan.range, endPos, delta=0.1) + + def getSRTPosition(self, conf_code, servo_name, elevation=45): + """Return the servo position related to the elevation. + + Parameters: + - conf_code: value returned by getActualSetup() (CCB, CCB_ASACTIVE,...) + - servo_name: SRP, GFR, M3R, PFP + - elevation: the antenna elevation, in degrees + """ + dal = ACSCorba.cdb() + dao = dal.get_DAO_Servant('alma/MINORSERVO/Boss') + body = dao.get_field_data(conf_code) + configurations = body.strip().split('@') + servos_conf = {} + for conf in configurations: + if conf: + name, value = conf.split(':') + servos_conf[name.strip()] = value.strip() + + # Example of servo_conf: + # >>> servos_conf['PFP'] + # 'RY(mm)=(-25.75); TX(mm)=(458); TZ(mm)=(-46.2);' + srp_conf = servos_conf[servo_name] + srp_items = [item.strip() for item in srp_conf.split(';')] + srp_axes = [] + for item in srp_items: + if '=' in item: + name, value = item.split('=') + srp_axes.append(value.strip()) + + # Example of srp_axes: + # >>> srp_axes + # ['(-25.75)', '(458)', '(-46.2)'] + position = [] + for axis in srp_axes: + axis = axis.lstrip('(') + axis = axis.rstrip(')') + # At this point, axis is something like '-0.23, 0.01, 3.2' + coeffs = [float(item) for item in axis.split(',')] + value = 0 + for idx, coeff in enumerate(coeffs): + value += coeff * elevation**idx + # Value: -25.75*elevation**0 + 485*elevation**1 -46.2*elevation**2 + position.append(value) + return position + + def getMEDPosition(self, conf_code, servo_name="", elevation=45): + """Return the servo position related to the elevation for MED + radiotelescope. + + Parameters: + - conf_code: value returned by getActualSetup() (CCC, KKC,...) + - servo_name: "" not use at Med + - elevation: the antenna elevation, in degrees + """ + from xml.dom.minidom import parseString + dal = ACSCorba.cdb() + dao = dal.get_DAO('alma/DataBlock/MinorServoParameters') + root = parseString(dao).documentElement + position = [] + coefficients = [] + for minorservo in root.getElementsByTagName("MinorServo"): + for code in minorservo.getElementsByTagName("code"): + if code.firstChild.data == conf_code: + if minorservo.getElementsByTagName("primary")[0].firstChild.data == "1": + yp_string_poly = minorservo.getElementsByTagName("YPaxis")[0].firstChild.data + coefficients.append(map(float,yp_string_poly.split(",")[3:])) + zp_string_poly = minorservo.getElementsByTagName("ZPaxis")[0].firstChild.data + coefficients.append(map(float,zp_string_poly.split(",")[3:])) + else: + x_string_poly = minorservo.getElementsByTagName("Xaxis")[0].firstChild.data + coefficients.append(map(float,x_string_poly.split(",")[3:])) + y_string_poly = minorservo.getElementsByTagName("Yaxis")[0].firstChild.data + coefficients.append(map(float,y_string_poly.split(",")[3:])) + z_string_poly = minorservo.getElementsByTagName("Zaxis")[0].firstChild.data + coefficients.append(map(float,z_string_poly.split(",")[3:])) + tx_string_poly = minorservo.getElementsByTagName("THETAXaxis")[0].firstChild.data + coefficients.append(map(float,tx_string_poly.split(",")[3:])) + ty_string_poly = minorservo.getElementsByTagName("THETAYaxis")[0].firstChild.data + coefficients.append(map(float,ty_string_poly.split(",")[3:])) + for coefficient in coefficients: + axis_position = 0 + for exp, coeff in enumerate(coefficient): + axis_position += (elevation)**(exp) * coeff + position.append(axis_position) + return position + + +class ScanInterfaceTest(ScanBaseTest): + """Test the interface of startScan() and closeScan()""" + + def test_checkScan_not_empty_system_not_ready(self): + """Raise a MinorServoErrorsEx in case the system is not ready""" + try: + with self.assertRaises(MinorServoErrorsEx): + t = self.boss.checkScan(0, self.scan, self.antennaInfo) + except: + with self.assertRaises(MinorServoErrorsEx): + t = self.boss.checkScan(0, self.scan, self.antennaInfo) + + def _test_checkScan_empty_scan_system_not_ready(self): + """Do nothing in case of empty scan and system NOT ready""" + self.scan.is_empty_scan = True + self.assertFalse(self.boss.isReady()) + t = self.boss.checkScan(0, self.scan, self.antennaInfo) + self.assertFalse(self.boss.isScanActive()) + + def test_closeScan_scan_not_active(self): + """Do nothing in case no scan is active""" + try: + self.boss.closeScan() + except: + self.boss.closeScan() + + +if __name__ == '__main__': + if 'Configuration' in os.getenv('ACS_CDB'): + unittest.main(verbosity=2, failfast=True) # Real test using the antenna CDB + else: + from testing import simulator + simulator.run(ScanTest, 'srt-mscu-sim') + simulator.run(ScanInterfaceTest, 'srt-mscu-sim') diff --git a/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_setASConfiguration.py b/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_setASConfiguration.py new file mode 100644 index 0000000000000000000000000000000000000000..b9cf2809636c3250f60e151aaf0c7aa00012cbea --- /dev/null +++ b/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_setASConfiguration.py @@ -0,0 +1,56 @@ +from __future__ import with_statement + +import os +import unittest + +from Acspy.Clients.SimpleClient import PySimpleClient +from MinorServoErrors import MinorServoErrorsEx + +from acswrapper.system import acs +from acswrapper.containers import ( + Container, ContainerError, start_containers_and_wait, + stop_containers_and_wait +) + +__author__ = "Marco Buttu " + + +class TestSetASConfiguration(unittest.TestCase): + + @classmethod + def setUpClass(cls): + if not acs.is_running(): + acs.start() + cls.containers = [ + Container('MinorServoContainer', 'cpp'), + Container('MinorServoBossContainer', 'cpp'), + ] + try: + start_containers_and_wait(cls.containers) + except ContainerError, ex: + cls.fail(ex.message) + cls.client = PySimpleClient() + cls.boss = cls.client.getComponent('MINORSERVO/Boss') + + @classmethod + def tearDownClass(cls): + cls.client.releaseComponent('MINORSERVO/Boss') + stop_containers_and_wait(cls.containers) + + def test_right_flag(self): + """Set the AS configuration properly""" + self.boss.setASConfiguration('on') + self.assertTrue(self.boss.isASConfiguration()) + + def test_wrong_flag(self): + """Raise a MinorServoErrorsEx in case of wrong code""" + with self.assertRaises(MinorServoErrorsEx): + self.boss.setASConfiguration('foo') + + +if __name__ == '__main__': + if 'Configuration' in os.getenv('ACS_CDB'): + unittest.main() # Real test using the antenna CDB + else: + from testing import simulator + simulator.run(TestSetASConfiguration, 'srt-mscu-sim') diff --git a/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_setElevationTracking.py b/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_setElevationTracking.py new file mode 100644 index 0000000000000000000000000000000000000000..8f24da6d380fcaefbc6bc73f399e2f9d24d4fa96 --- /dev/null +++ b/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_setElevationTracking.py @@ -0,0 +1,56 @@ +from __future__ import with_statement + +import os +import unittest + +from Acspy.Clients.SimpleClient import PySimpleClient +from MinorServoErrors import MinorServoErrorsEx + +from acswrapper.system import acs +from acswrapper.containers import ( + Container, ContainerError, start_containers_and_wait, + stop_containers_and_wait +) + +__author__ = "Marco Buttu " + + +class TestSetElevationTracking(unittest.TestCase): + + @classmethod + def setUpClass(cls): + if not acs.is_running(): + acs.start() + cls.containers = [ + Container('MinorServoContainer', 'cpp'), + Container('MinorServoBossContainer', 'cpp'), + ] + try: + start_containers_and_wait(cls.containers) + except ContainerError, ex: + cls.fail(ex.message) + cls.client = PySimpleClient() + cls.boss = cls.client.getComponent('MINORSERVO/Boss') + + @classmethod + def tearDownClass(cls): + cls.client.releaseComponent('MINORSERVO/Boss') + stop_containers_and_wait(cls.containers) + + def test_right_flag(self): + """Set the elevation tracking properly""" + self.boss.setElevationTracking('on') + self.assertTrue(self.boss.isElevationTrackingEn()) + + def test_wrong_flag(self): + """Raise a MinorServoErrorsEx in case of wrong code""" + with self.assertRaises(MinorServoErrorsEx): + self.boss.setElevationTracking('foo') + + +if __name__ == '__main__': + if 'Configuration' in os.getenv('ACS_CDB'): + unittest.main() # Real test using the antenna CDB + else: + from testing import simulator + simulator.run(TestSetElevationTracking, 'srt-mscu-sim') diff --git a/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_systemOffset.py b/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_systemOffset.py new file mode 100644 index 0000000000000000000000000000000000000000..e25314614de906bc3d852a24d937b3ba7a24a541 --- /dev/null +++ b/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_systemOffset.py @@ -0,0 +1,95 @@ +from __future__ import with_statement + +import os +import math +import time +import datetime +import unittest + +import MinorServo +import Management +import Antenna + +from Acspy.Clients.SimpleClient import PySimpleClient +from MinorServoErrors import MinorServoErrorsEx +from Acspy.Common.TimeHelper import getTimeStamp + +from acswrapper.system import acs +from acswrapper.containers import ( + Container, ContainerError, start_containers_and_wait, + stop_containers_and_wait +) + +__author__ = "Marco Buttu " + + +class TestSystemOffset(unittest.TestCase): + + telescope = os.getenv('STATION') + + @classmethod + def setUpClass(cls): + if not acs.is_running(): + acs.start() + cls.containers = [ + Container('MinorServoContainer', 'cpp'), + Container('MinorServoBossContainer', 'cpp'), + ] + try: + start_containers_and_wait(cls.containers) + except ContainerError, ex: + cls.fail(ex.message) + cls.client = PySimpleClient() + cls.boss = cls.client.getComponent('MINORSERVO/Boss') + + @classmethod + def tearDownClass(cls): + cls.client.releaseComponent('MINORSERVO/Boss') + stop_containers_and_wait(cls.containers) + + def setUp(self): + self.setup_code = "CCB" if self.telescope == "SRT" else "CCC" + self.axis_code = "SRP_TX" if self.telescope == "SRT" else "X" + + def tearDown(self): + #self.boss.clearSystemOffset(self.axis_code) + self.boss.setSystemOffset(self.axis_code, 0) # TODO + self.boss.park() + time.sleep(0.2) + self.wait_parked() + + def test_wrong_servo_name(self): + """Raise a MinorServoErrorsEx in case of wrong servo name""" + self.boss.setup(self.setup_code) + counter = 0 # Seconds + now = time_ref = datetime.datetime.now() + while not self.boss.isReady() or (time_ref - now).seconds < 20: + time.sleep(1) + now = datetime.datetime.now() + with self.assertRaises(MinorServoErrorsEx): + self.boss.setSystemOffset(self.axis_code + "WRONG", 0) + + def test_get_offset(self): + self.boss.setup(self.setup_code) + counter = 0 # Seconds + now = time_ref = datetime.datetime.now() + while not self.boss.isReady() or (time_ref - now).seconds < 20: + time.sleep(1) + now = datetime.datetime.now() + + target_offset = 5.0 + self.boss.setSystemOffset(self.axis_code, target_offset) + offset = self.boss.getSystemOffset()[0] # SRP_TX and X both have index 0 + self.assertAlmostEqual(offset, target_offset, delta=0.1) + + def wait_parked(self): + while self.boss.isParking(): + time.sleep(0.1) + + +if __name__ == '__main__': + if 'Configuration' in os.getenv('ACS_CDB'): + unittest.main() # Real test using the antenna CDB + else: + from testing import simulator + simulator.run(TestSystemOffset, 'srt-mscu-sim') diff --git a/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_userOffset.py b/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_userOffset.py new file mode 100644 index 0000000000000000000000000000000000000000..269c44ce6345775709934d12822674bd1d053162 --- /dev/null +++ b/SRT/Interfaces/SRTMinorServoInterface/test/functional/test_userOffset.py @@ -0,0 +1,96 @@ +from __future__ import with_statement + +import os +import math +import time +import datetime +import unittest + +import MinorServo +import Management +import Antenna + +from Acspy.Clients.SimpleClient import PySimpleClient +from MinorServoErrors import MinorServoErrorsEx +from Acspy.Common.TimeHelper import getTimeStamp + +from acswrapper.system import acs +from acswrapper.containers import ( + Container, ContainerError, start_containers_and_wait, + stop_containers_and_wait +) + + +__author__ = "Marco Buttu " + + +class TestUserOffset(unittest.TestCase): + + telescope = os.getenv('STATION') + + @classmethod + def setUpClass(cls): + if not acs.is_running(): + acs.start() + cls.containers = [ + Container('MinorServoContainer', 'cpp'), + Container('MinorServoBossContainer', 'cpp'), + ] + try: + start_containers_and_wait(cls.containers) + except ContainerError, ex: + cls.fail(ex.message) + cls.client = PySimpleClient() + cls.boss = cls.client.getComponent('MINORSERVO/Boss') + + @classmethod + def tearDownClass(cls): + cls.client.releaseComponent('MINORSERVO/Boss') + stop_containers_and_wait(cls.containers) + + def setUp(self): + self.setup_code = "CCB" if self.telescope == "SRT" else "CCC" + self.axis_code = "SRP_TX" if self.telescope == "SRT" else "X" + + def tearDown(self): + # self.boss.clearUserOffset(self.axis_code) + self.boss.setUserOffset(self.axis_code, 0) # TODO + self.boss.park() + time.sleep(0.2) + self.wait_parked() + + def test_wrong_servo_name(self): + """Raise a MinorServoErrorsEx in case of wrong servo name""" + self.boss.setup(self.setup_code) + counter = 0 # Seconds + now = time_ref = datetime.datetime.now() + while not self.boss.isReady() or (time_ref - now).seconds < 20: + time.sleep(1) + now = datetime.datetime.now() + with self.assertRaises(MinorServoErrorsEx): + self.boss.setUserOffset(self.axis_code + "WRONG", 0) + + def test_get_offset(self): + self.boss.setup(self.setup_code) + counter = 0 # Seconds + now = time_ref = datetime.datetime.now() + while not self.boss.isReady() or (time_ref - now).seconds < 20: + time.sleep(1) + now = datetime.datetime.now() + + target_offset = 5.0 + self.boss.setUserOffset(self.axis_code, target_offset) + offset = self.boss.getUserOffset()[0] # SRP_TX and X both have index 0 + self.assertAlmostEqual(offset, target_offset, delta=0.1) + + def wait_parked(self): + while self.boss.isParking(): + time.sleep(0.1) + + +if __name__ == '__main__': + if 'Configuration' in os.getenv('ACS_CDB'): + unittest.main() # Real test using the antenna CDB + else: + from testing import simulator + simulator.run(TestUserOffset, 'srt-mscu-sim') diff --git a/SRT/Interfaces/SRTMinorServoInterface/test/pyunit/__init__.py b/SRT/Interfaces/SRTMinorServoInterface/test/pyunit/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/SRT/Servers/SRTMinorServo/test/unittest.cpp b/SRT/Interfaces/SRTMinorServoInterface/test/unittest.cpp similarity index 100% rename from SRT/Servers/SRTMinorServo/test/unittest.cpp rename to SRT/Interfaces/SRTMinorServoInterface/test/unittest.cpp diff --git a/SRT/Libraries/SRTMinorServoLibrary/include/PySRTMinorServoCommandLibrary.h b/SRT/Libraries/SRTMinorServoLibrary/include/PySRTMinorServoCommandLibrary.h new file mode 100644 index 0000000000000000000000000000000000000000..a3a33a3bcc23900688ebc6cfec8cd6f6b5801303 --- /dev/null +++ b/SRT/Libraries/SRTMinorServoLibrary/include/PySRTMinorServoCommandLibrary.h @@ -0,0 +1,130 @@ +#ifndef _PYSRTMINORSERVOCOMMANDLIBRARY_H +#define _PYSRTMINORSERVOCOMMANDLIBRARY_H + +/** + * PySRTMinorServoCommandLibrary.h + * 2021/12/13 + * Giuseppe Carboni (giuseppe.carboni@inaf.it) + */ + +#include +#include +#include "SRTMinorServoCommandLibrary.h" + + +namespace MinorServo +{ + /** + * SRT Minor Servo Command Library Python Wrapper + * + * This class wraps the SRTMinorServoCommandLibrary with boost_python in order to grant access to its static functions via Python + */ + class PySRTMinorServoCommandLibrary : public SRTMinorServoCommandLibrary + { + public: + /** + * Builds the command used to ask the general status of the minor servos or, eventually, a single servo + * @param servo_id the ID string of the eventual single servo to retrieve the status. Send no servo_id argument to retrieve the general status of the system + * @return the composed message + */ + static boost::python::object status(const std::string servo_id = ""); + + /** + * Builds the command used to configure the telescope for a specific focal path + * @param configuration the desired focal path to command to the minor servo systems + * @return the composed message + */ + static boost::python::object setup(const std::string& configuration); + + /* + * Builds the command used to stow a single servo system to a given stow position + * @param servo_id the ID string of the single servo to be stowed + * @param stow_position the position to which the servo have to stow to + * @return the composed message + */ + static boost::python::object stow(const std::string& servo_id, unsigned int stow_position = 1); + + /* + * Builds the command used to stop a single servo system + * @param servo_id the ID string of the single servo to be stopped + * @return the composed message + */ + static boost::python::object stop(const std::string& servo_id); + + /* + * Builds the command used to move a single servo to a given set of coordinates + * This is an overload of the original SRTMinorServoCommandLibrary::preset function + * @param servo_id the ID of the single servo to be moved + * @param coordinates a Python list containing the N coordinates to be sent to the servo + * @return the composed message + */ + static boost::python::object preset(const std::string& servo_id, const boost::python::list& coordinates); + + /* + * Builds the command used to provide a single tracking set of coordinates to a single servo + * This is an overload of the original SRTMinorServoCommandLibrary::programTrack function + * @param servo_id the ID of the single servo to send the command to + * @param trajectory_id the ID number of the trajectory the given set of coordinates belongs to + * @param point_id the ID number of the given set of coordinates inside the trajectory + * @param coordinates a Python list containing the N coordinates the servo have to move to at the given time + * @param start_time only mandatory for the first point in the trajectory, a double representing the UNIX epoch of the starting instant of the trajectory + * @return the composed message + */ + static boost::python::object programTrack(const std::string& servo_id, const unsigned long& trajectory_id, const unsigned long& point_id, const boost::python::list& coordinates, double start_time=-1); + + /* + * Builds the command used to provide a set of offsets to a given servo + * This is an overload of the original SRTMinorServoCommandLibrary::offset function + * @param servo_id the ID of the single servo to be moved + * @param coordinates a Python list containing the N coordinates to be sent to the servo + * @return the composed message + */ + static boost::python::object offset(const std::string& servo_id, const boost::python::list& coordinates); + + /* + * Parses the received answer by splitting it and synamically populating a std::map + * This is an overload of the original SRTMinorServoCommandLibrary::parseAnswer function + * @param answer the string containing the answer received from the VBrain proxy + * @return a Python dictionary containing the answer splitted into keys and values. The keys are always strings, the values can either be int, double or strings. + */ + static boost::python::dict parseAnswer(const std::string& answer); + private: + /* + * Converts the given Python list into a C++ std::vector object + * @param py_list the given Python list to be converted + * @return the composed C++ std::vector containing doubles + */ + static std::vector pylist2cppvector(const boost::python::list& py_list); + + /** + * Converts the given std::string to a Python bytestring + * @param command a reference to the given command string + * @return the bytestring containing the given command string + */ + static boost::python::object stringToBytes(const std::string& command); + }; +} + +/* + * The following 3 lines of code allow the overloaded functions to ignore the optional parameter and use the default one defined in the original SRTMinorServoCommandLibrary header file + */ +BOOST_PYTHON_FUNCTION_OVERLOADS(status, MinorServo::PySRTMinorServoCommandLibrary::status, 0, 1) +BOOST_PYTHON_FUNCTION_OVERLOADS(stow, MinorServo::PySRTMinorServoCommandLibrary::stow, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(programTrack, MinorServo::PySRTMinorServoCommandLibrary::programTrack, 4, 5) + +/* + * Python module definition. Since the original SRTMinorServoCommandLibrary only contains static functions, we write the Python module with static functions only, omitting the class + */ +BOOST_PYTHON_MODULE(libPySRTMinorServoCommandLibrary) +{ + using namespace boost::python; + def("status", &MinorServo::PySRTMinorServoCommandLibrary::status, status(arg("servo_id") = "")); + def("setup", &MinorServo::PySRTMinorServoCommandLibrary::setup); + def("stow", &MinorServo::PySRTMinorServoCommandLibrary::stow, stow(arg("stow_position") = 1)); + def("stop", &MinorServo::PySRTMinorServoCommandLibrary::stop); + def("preset", &MinorServo::PySRTMinorServoCommandLibrary::preset); + def("programTrack", &MinorServo::PySRTMinorServoCommandLibrary::programTrack, programTrack(arg("start_time") = -1)); + def("offset", &MinorServo::PySRTMinorServoCommandLibrary::offset); + def("parseAnswer", &MinorServo::PySRTMinorServoCommandLibrary::parseAnswer); +} +#endif diff --git a/SRT/Libraries/SRTMinorServoLibrary/include/SRTMinorServoCommandLibrary.h b/SRT/Libraries/SRTMinorServoLibrary/include/SRTMinorServoCommandLibrary.h index 4eb6061491075214a555a4c488ac1180bd548ced..7023342523e20526c0f84e5c504b96d810383842 100644 --- a/SRT/Libraries/SRTMinorServoLibrary/include/SRTMinorServoCommandLibrary.h +++ b/SRT/Libraries/SRTMinorServoLibrary/include/SRTMinorServoCommandLibrary.h @@ -7,66 +7,89 @@ * Giuseppe Carboni (giuseppe.carboni@inaf.it) */ +#include "SRTMinorServoContainers.h" #include #include -#include -#include +#include +#include -/** - * SRT Minor Servo Command Library - * - * This class features static functions used to build commands to be sent to the PON minor servo control unit of the Sardinia Radio Telescope - */ -class SRTMinorServoCommandLibrary + +#define CLOSER std::string("\r\n") + + +namespace MinorServo { -public: /** - * Builds the command used to ask the status of the MSCU or, eventually, a single servo - * @param servo_id the ID number of the eventual single servo to retrieve the status - * @return the composed message + * SRT Minor Servo Command Library + * + * This class features static functions used to build commands to be sent to the PON minor servo control unit of the Sardinia Radio Telescope */ - static std::string status(int servo_id = -1); + class SRTMinorServoCommandLibrary + { + public: + /** + * Builds the command used to ask the status of the MSCU or, eventually, a single servo + * @param servo_id the ID string of the eventual single servo to retrieve the status. Send no servo_id argument to retrieve the general status of the system + * @return the composed message + */ + static std::string status(const std::string servo_id = ""); - /** - * Builds the command used to configure the telescope for an observation - * @param configuration the desired observing configuration - * @return the composed message - */ - static std::string setup(std::string configuration); + /** + * Builds the command used to configure the telescope for a specific focal path + * @param configuration the desired focal path to command to the minor servo systems + * @return the composed message + */ + static std::string setup(const std::string& configuration); - /* - * Builds the command used to stow a single servo system to a given stow position - * @param servo_id the ID number of the single servo to be stowed - * @param stow_position the position to which the servo have to stow to - * @return the composed message - */ - static std::string stow(unsigned int servo_id, unsigned int stow_position = 0); + /* + * Builds the command used to stow a single servo system to a given stow position + * @param servo_id the ID string of the single servo to be stowed + * @param stow_position the position to which the servo have to stow to + * @return the composed message + */ + static std::string stow(const std::string& servo_id, const unsigned int stow_position = 1); - /* - * Builds the command used to stop a single servo system - * @param servo_id the ID number of the single servo to be stopped - * @return the composed message - */ - static std::string stop(unsigned int servo_id); + /* + * Builds the command used to stop a single servo system + * @param servo_id the ID string of the single servo to be stopped + * @return the composed message + */ + static std::string stop(const std::string& servo_id); - /* - * Builds the command used to move a single servo to a given set of coordinates - * @param servo_id the ID number of the single servo to be moved - * @param coordinates a vector containing the N coordinates to be sent to the servo - * @return the composed message - */ - static std::string preset(unsigned int servo_id, std::vector coordinates); + /* + * Builds the command used to move a single servo to a given set of virtual coordinates + * @param servo_id the ID string of the single servo to be moved + * @param coordinates a vector containing the N coordinates to be sent to the servo + * @return the composed message + */ + static std::string preset(const std::string& servo_id, const std::vector& coordinates); - /* - * Builds the command used to provide a single tracking set of coordinates to a single servo - * @param servo_id the ID number of the single servo to send the command to - * @param trajectory_id the ID number of the trajectory the given set of coordinates belongs to - * @param point_id the ID number of the given set of coordinates inside the trajectory - * @param coordinates a vector containing the N coordinates the servo have to move to at the given time - * @param start_time only mandatory for the first point in the trajectory, a double representing the UNIX epoch of the starting instant of the trajectory - * @return the composed message - */ - static std::string programTrack(unsigned int servo_id, unsigned int trajectory_id, unsigned int point_id, std::vector coordinates, double start_time = -1); -}; + /* + * Builds the command used to provide a single tracking point of virtual coordinates to a single servo + * @param servo_id the ID string of the single servo to send the command to + * @param trajectory_id the ID number of the trajectory the given set of coordinates belongs to + * @param point_id the ID number of the given set of coordinates inside the trajectory + * @param coordinates a vector containing the N coordinates the servo have to move to at the given time + * @param start_time only mandatory for the first point in the trajectory, a double representing the UNIX Epoch of the starting instant of the trajectory + * @return the composed message + */ + static std::string programTrack(const std::string& servo_id, const unsigned long& trajectory_id, const unsigned long& point_id, const std::vector& coordinates, const double start_time = 0); + + /* + * Builds the command used to send a set of virtual offsets to a single servo + * @param servo_id the ID string of the single servo to send the offsets to + * @param coordinates a vector containing the N offsets to be added the servo coordinates + * @return the composed message + */ + static std::string offset(const std::string& servo_id, const std::vector& coordinates); + + /* + * Parses the received answer by splitting it and dynamically populating a SRTMinorServoAnswerMap object + * @param original_answer the string containing the answer received from the VBrain proxy + * @return a SRTMinorServoAnswerMap dictionary containing the answer splitted into keys and values. The keys are always std::string, the values can either be long, double or std::string. + */ + static SRTMinorServoAnswerMap parseAnswer(const std::string& original_answer); + }; +} #endif diff --git a/SRT/Libraries/SRTMinorServoLibrary/include/SRTMinorServoContainers.h b/SRT/Libraries/SRTMinorServoLibrary/include/SRTMinorServoContainers.h new file mode 100644 index 0000000000000000000000000000000000000000..dfea1df6621e5505b40075243215db2376093904 --- /dev/null +++ b/SRT/Libraries/SRTMinorServoLibrary/include/SRTMinorServoContainers.h @@ -0,0 +1,894 @@ +#ifndef __SRTMINORSERVOCONTAINERS_H__ +#define __SRTMINORSERVOCONTAINERS_H__ + +/** + * SRTMinorServoContainers.h + * Giuseppe Carboni (giuseppe.carboni@inaf.it) + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "SRTMinorServoUtils.h" + + +namespace MinorServo +{ + /** + * Object used to store some info regarding a scan. + */ + struct SRTMinorServoScan + { + /** + * Name of the servo involved in the scan. + */ + std::string servo_name = ""; + + /** + * Name of the axis involved in the scan. + */ + std::string axis_name = ""; + + /** + * Index of the axis involved in the scan. + */ + size_t axis_index = 0; + + /** + * Range of the scan. + */ + double scan_range = 0; + + /** + * Starting time of the scan. + */ + ACS::Time start_time = 0; + + /** + * Closing time of the scan. + */ + ACS::Time close_time = 0; + + /** + * Duration of the scan. + */ + ACS::TimeInterval scan_duration = 0; + + /** + * Central position of the scan axis. + */ + double central_position = 0; + + /** + * Starting elevation for the scan. + */ + double starting_elevation = 0; + }; + + /** + * This dictionary contains information regarding the possibile focal configurations. + * The key indicates the configuration as known by DISCOS. + * The value is a pair, the first element of the pair is the DISCOS enumeration for the relative Leonardo configuration, + * the second element of the pair indicates whether the configuration has a ASACTIVE twin configuraiton. + */ + using DiscosConfigurationNameTableType = std::map>; + const DiscosConfigurationNameTableType DiscosConfigurationNameTable = + { + {"LLP", std::make_pair(CONFIGURATION_PRIMARY, false)}, + {"PPP", std::make_pair(CONFIGURATION_PRIMARY, false)}, + {"PLP", std::make_pair(CONFIGURATION_PRIMARY, false)}, + {"HHP", std::make_pair(CONFIGURATION_PRIMARY, false)}, + {"XKP", std::make_pair(CONFIGURATION_PRIMARY, false)}, + {"CCG", std::make_pair(CONFIGURATION_GREGORIAN1, true )}, + {"KKG", std::make_pair(CONFIGURATION_GREGORIAN2, true )}, + {"WWG", std::make_pair(CONFIGURATION_GREGORIAN3, true )}, + {"QQG", std::make_pair(CONFIGURATION_GREGORIAN4, true )}, + {"TRI", std::make_pair(CONFIGURATION_GREGORIAN5, true )}, + {"MISTRAL", std::make_pair(CONFIGURATION_GREGORIAN6, true )}, + {"CCB", std::make_pair(CONFIGURATION_BWG1, true )}, + {"XB", std::make_pair(CONFIGURATION_BWG3, true )}, + }; + + /** + * This dictionary contains the Leonardo focal configurations DISCOS enumerations, alongside their name inside the Leonardo minor servo system. + */ + using LDOConfigurationNameTableType = boost::bimap; + const LDOConfigurationNameTableType LDOConfigurationNameTable = boost::assign::list_of + (CONFIGURATION_PRIMARY, "Primario") + (CONFIGURATION_GREGORIAN1, "Gregoriano 1") + (CONFIGURATION_GREGORIAN2, "Gregoriano 2") + (CONFIGURATION_GREGORIAN3, "Gregoriano 3") + (CONFIGURATION_GREGORIAN4, "Gregoriano 4") + (CONFIGURATION_GREGORIAN5, "Gregoriano 5") + (CONFIGURATION_GREGORIAN6, "Gregoriano 6") + (CONFIGURATION_GREGORIAN7, "Gregoriano 7") + (CONFIGURATION_GREGORIAN8, "Gregoriano 8") + (CONFIGURATION_BWG1, "BWG1") + (CONFIGURATION_BWG2, "BWG2") + (CONFIGURATION_BWG3, "BWG3") + (CONFIGURATION_BWG4, "BWG4"); + + /** + * This dictionary containes the Leonardo focal configurations DISCOS enumerations, alongside their ID counterpart as read from the Leonardo minor servo system proxy. + */ + using LDOConfigurationIDTableType = boost::bimap; + const LDOConfigurationIDTableType LDOConfigurationIDTable = boost::assign::list_of + (CONFIGURATION_UNKNOWN, 0) + (CONFIGURATION_PRIMARY, 1) + (CONFIGURATION_GREGORIAN1, 11) + (CONFIGURATION_GREGORIAN2, 12) + (CONFIGURATION_GREGORIAN3, 13) + (CONFIGURATION_GREGORIAN4, 14) + (CONFIGURATION_GREGORIAN5, 15) + (CONFIGURATION_GREGORIAN6, 16) + (CONFIGURATION_GREGORIAN7, 17) + (CONFIGURATION_GREGORIAN8, 18) + (CONFIGURATION_BWG1, 21) + (CONFIGURATION_BWG2, 22) + (CONFIGURATION_BWG3, 23) + (CONFIGURATION_BWG4, 24); + + using SRTMinorServoLookupTable = std::map>; + + /** + * This class implements a queue of time tagged positions. it extends a simple std::map with some specific methods. + */ + class SRTMinorServoPositionsQueue : private std::map> + { + public: + /** + * Default constructor. Used only for lazy initialization. + */ + SRTMinorServoPositionsQueue() : std::map>(), m_queue_size(0), m_vector_size(0), m_mutex() {} + + /** + * Constructor with queue size as parameter. + * @param queue_size the maximum size of the queue. Once this value is reached the oldest entry gets discarded. + */ + SRTMinorServoPositionsQueue(size_t queue_size) : std::map>(), m_queue_size(queue_size), m_vector_size(0), m_mutex() + { + std::unique_lock lock(m_mutex); + queueLazyInit(queue_size); + } + + /** + * Constructor with queue size and vector size as parameters. + * @param queue_size the maximum size of the queue. Once this value is reached the oldest entry gets discarded. + * @param vector_size the length of the vectors that this object will store. Once set it cannot be changed. This assures we are always dealing with the same number of virtual axes. + */ + SRTMinorServoPositionsQueue(size_t queue_size, size_t vector_size) : std::map>(), m_queue_size(queue_size), m_vector_size(vector_size), m_mutex() + { + std::unique_lock lock(m_mutex); + queueLazyInit(queue_size); + vectorLazyInit(vector_size); + } + + /** + * Custom assignment operator. It locks both the current object and the passed one with a mutex in order for the assignment operation to be thread safe. + * @param other the other SRTMinorServoPositionsQueue we are assigning its value to the current object. + * @return the newly populated SRTMinorServoPositionsQueue object. + */ + SRTMinorServoPositionsQueue& operator=(const SRTMinorServoPositionsQueue& other) + { + if(this != &other) + { + std::unique_lock lockThis(m_mutex, std::defer_lock); + std::unique_lock lockOther(other.m_mutex, std::defer_lock); + std::lock(lockThis, lockOther); + m_queue_size = other.m_queue_size; + m_vector_size = other.m_vector_size; + std::map>::operator=(other); + } + + return *this; + } + + /** + * Put method, with initializer list argument. + * @param key the time the given coordinates are related to. + * @param values the given set of coordinates. + */ + void put(ACS::Time key, const std::initializer_list& values) + { + put(key, std::vector(values)); + } + + /** + * Put method, with ACS::doubleSeq argument. + * @param key the time the given coordinates are related to. + * @param values the given set of coordinates. + */ + void put(ACS::Time key, const ACS::doubleSeq& values) + { + put(key, std::vector(values.get_buffer(), values.get_buffer() + values.length())); + } + + /** + * Put method, with std::vector argument. + * @param key the time the given coordinates are related to. + * @param values the given set of coordinates. + */ + void put(ACS::Time key, const std::vector& values) + { + std::unique_lock lock(m_mutex); + + vectorLazyInit(values.size()); + queueLazyInit(m_queue_size); + if(std::map>::size() == m_queue_size) + { + // Remove the oldest one + this->erase(this->begin()); + } + this->emplace(std::make_pair(key, values)); + } + + /** + * Get method. It retrieves a set of coordinates from a given ACS::Time, giving back the time as well. + * @param key the time the user wants to retrieve the related coordinates. + * @param exact a boolean indicating whether the user wants to interpolate (false) or not (true). + * @throw std::logic_error when the queue is empty. + * @throw std::out_of_range when the exact point was not found in the queue. + * @return a std::pair containing the ACS::Time as first element and the set of coordinates as second element. + */ + std::pair> get(ACS::Time key, bool exact = false) + { + std::shared_lock lock(m_mutex); + + if(this->empty()) + { + throw std::logic_error("The queue is empty!"); + } + + if(const SRTMinorServoPositionsQueue::iterator point = this->find(key); point != this->end()) + { + return *point; + } + else if(exact) + { + // Exact point not found, we throw an exception + throw std::out_of_range("Exact point not found!"); + } + else + { + // Key not found, should check outside the boundaries or interpolate + if(key <= this->begin()->first) + { + // Aksed for a timestamp older than the earliest point in the queue + return *this->cbegin(); + } + else if(key >= this->rbegin()->first) + { + // Asked for a timestamp newer than the latest point in the queue + return *this->crbegin(); + } + else + { + std::vector positions(m_vector_size, 0.0); + SRTMinorServoPositionsQueue::iterator p0, p1; + p1 = this->lower_bound(key); + p0 = p1; + p0--; + + // Calculate the linear fit for each position + double fraction = (key - p0->first) / (p1->first - p0->first); + + for(size_t i = 0; i < m_vector_size; i++) + { + positions[i] = p0->second[i] + fraction * (p1->second[i] - p0->second[i]); + } + + return std::make_pair(key, (const std::vector)positions); + } + } + } + + /** + * Size method thread safe override. + * @return the number of elements in the queue. + */ + size_t size() const + { + std::shared_lock lock(m_mutex); + return std::map>::size(); + } + + /** + * Clear method, thread safe. It empties the queue. + */ + void clear() + { + std::unique_lock lock(m_mutex); + std::map>::clear(); + } + + /** + * This method returns the number of points having a higher tag time than the one passed as argument. + * @param t the time threshold. This method counts and returns the number of points having a higher time than this value. + * @throw std::logic_error when the queue is empty. + * @return the number of points having a higher time than the given one. + */ + size_t getRemainingPoints(ACS::Time t) + { + std::shared_lock lock(m_mutex); + if(this->empty()) + { + throw std::logic_error("The queue is empty!"); + } + return std::distance(this->lower_bound(t), this->end()); + } + + private: + /** + * This method gets called by the constructors for the lazy intialization of the queue size value. + * @param queue_size the desired maximum queue size. + * @throw std::length_error when the desired queue size is equal to 0 or when it is greater than the maximum size that can be currently allocated. + */ + void queueLazyInit(size_t queue_size) + { + if(m_queue_size == 0) + { + if(queue_size == 0) + { + // The maximum queue size was not set yet + throw std::length_error("Queue length cannot be 0."); + } + else if(queue_size > this->max_size()) + { + // The requested size is greater than the maximum possible queue size + throw std::length_error("Queue length cannot exceed " + std::to_string(this->max_size()) + "."); + } + else + { + m_queue_size = queue_size; + } + } + } + + /** + * This method gets called by the constructors for the lazy initialization of the vector size value. + * @param vector_size the desired length of the vector containing the set of points. + * @throw std::length_error when the desired vector size is equal to 0 or when it is longer than 6. We don't have more than 6 axes, so we hard cap this value to 6. + * It also throws this when the user tries to insert a vector with different lenght then the already defined one. This ensures all stored vecors are of equal length. + */ + void vectorLazyInit(size_t vector_size) + { + if(vector_size == 0) + { + throw std::length_error("Vector length cannot be 0."); + } + else if(vector_size > 6) + { + // Hard cap to 6, we don't need more + throw std::length_error("Vector length cannot be longer than 6."); + } + else if(m_vector_size == 0) + { + m_vector_size = vector_size; + } + else if(m_vector_size != vector_size) + { + throw std::length_error("New vector length does not match the initial one."); + } + } + + /** + * The maximum size of the queue. + */ + size_t m_queue_size; + + /** + * The desired length of the vectors stored by this object. + */ + size_t m_vector_size; + + /** + * The shared mutex used for access synchronization. + */ + mutable std::shared_mutex m_mutex; + }; + + + class SRTMinorServoAnswerMap : private std::map> + { + /** + * This class privately extends the type std::map>. + * It is therefore an std::map which can hold different types of values, such as long, double and str::string, in the same container. + * It is used to store the answers received from the SRTMinorServo Leonardo system. + * This design was critical since all the received values have heterogeneous keys and values. + * With this object, the SRTMinorServoSocket can correctly retrieve and store all the received values without having to know the keys or types a priori. + */ + + /* + * Declare this class as friend since it will have to iterate through the inner map + */ + friend class PySRTMinorServoCommandLibrary; + public: + /** + * Use the same clear method of the parent class + */ + using std::map>::clear; + + /** + * Default constructor, initialize the std::map object and the synchronization mutex + */ + SRTMinorServoAnswerMap() : std::map>(), m_mutex() {} + + /** + * Initialize the std::map with the content of another SRTMinorServoAnswerMap, initialize the mutex, lock both objects + * @param other the SRTMinorServoAnswerMap with which the content of the current object will be initialized + */ + SRTMinorServoAnswerMap(const SRTMinorServoAnswerMap& other) : m_mutex() + { + std::unique_lock lockThis(m_mutex, std::defer_lock); + std::shared_lock lockOther(other.m_mutex, std::defer_lock); + std::lock(lockThis, lockOther); + static_cast>&>(*this) = static_cast>&>(other); + } + + /** + * Assignment operator. It lock both the current object and the assigned one's mutexes + * @param other the SRTMinorServoAnswerMap which values have to be stored in the current object + */ + SRTMinorServoAnswerMap& operator=(const SRTMinorServoAnswerMap& other) + { + if(this != &other) + { + std::unique_lock lockThis(m_mutex, std::defer_lock); + std::shared_lock lockOther(other.m_mutex, std::defer_lock); + std::lock(lockThis, lockOther); + static_cast>&>(*this) = static_cast>&>(other); + } + + return *this; + } + + /** + * Update operator. It merges the current object with the elements of another object + * @param other the SRTMinorServoAnswerMap which values have to be copied inside the current object + */ + SRTMinorServoAnswerMap& operator+=(const SRTMinorServoAnswerMap& other) + { + std::unique_lock lockThis(m_mutex, std::defer_lock); + std::shared_lock lockOther(other.m_mutex, std::defer_lock); + std::lock(lockThis, lockOther); + for(const auto& entry : other) + { + this->operator[](entry.first) = entry.second; + } + + return *this; + } + + /** + * Equality operator, only check the std::map and avoid comparing the mutexes, which will obviously be different + * @param other the SRTMinorServoAnswerMap object to compare the current object with + */ + bool operator==(const SRTMinorServoAnswerMap& other) const + { + return static_cast>&>(*this) == static_cast>&>(other); + } + + /** + * get method. It must be used with a template parameter, in order for the SRTMinorServoAnswerMap to be able to retrieve the correct type of object for the given key. + * The method will automatically convert the retrieved long, double or std::string to the given template type. + * @param T the type (i.e.: int, long, double, char*) of the object to be retrieved. It can be anything derived from integral, floating point or string values. + * @param key the key assigned to the value you want to retrieve + * @return the value associated to given key 'key', returned as template type 'T', if possible. Be aware that some casts (i.e.: long to int, double to float) will lose precision and/or overflow + * @throw std::bad_variant_access when retrieving the stored value by asking the wrong type (i.e.: stored type is a double but T is char*) + * @throw std::runtime_error when attempting to retrieve a value with a type which cannot be stored in the container (anything not integral, not floating point and not similar to std::string) + */ + template + T get(const std::string& key) const + { + if constexpr(std::negation_v>) + { + throw std::runtime_error("Unsupported type."); + } + + std::shared_lock lock(m_mutex); + + try + { + if constexpr(std::is_integral_v) + { + return (T)std::get(this->at(key)); + } + else if constexpr(std::is_floating_point_v) + { + return (T)std::get(this->at(key)); + } + else if constexpr(is_string_v) + { + return (T)std::get(this->at(key)).c_str(); + } + } + catch(std::out_of_range& ex) + { + std::cout << "PLAIN_COMMAND: " << this->getPlainCommand(); + std::cout << "PLAIN_ANSWER:" << this->getPlainAnswer(); + throw ex; + } + } + + /** + * put method. The template parameter is automatically deducted from the 'value' argument. Stores the given 'value' associated with key 'key' + * @param key the key associated to the stored value 'value' + * @param value the value we are storing with the given key 'key' + * @throw std::runtime_error when attempting to store a value with a type which cannot be stored in the container (anything not integral, not floating point and not similar to std::string) + */ + template + void put(const std::string& key, const T& value) + { + if constexpr(std::negation_v>) + { + throw std::runtime_error("Unsupported type."); + } + + std::unique_lock lock(m_mutex); + + if constexpr(std::is_integral_v) + { + this->operator[](key) = long(value); + } + else if constexpr(std::is_floating_point_v) + { + this->operator[](key) = double(value); + } + else if constexpr(is_string_v) + { + this->operator[](key) = std::string(value); + } + } + + /** + * This method checks whether the container holds a value for the given key 'key' + * @param key the key for the value we want to check if it's present in the container + * @return true if the value is present in the container, false otherwise + */ + bool contains(const std::string& key) const + { + std::shared_lock lock(m_mutex); + return this->find(key) != this->end(); + } + + /** + * This methods returns the std::variant type index for the value associated to the given key 'key' + * @param key the key for the value we want to retrieve the type index + * @throw std::out_of_range if the key is not found in the object + * @return 0 for long, 1 for double, 2 for std::string + */ + unsigned int index(const std::string& key) const + { + std::shared_lock lock(m_mutex); + return this->at(key).index(); + } + + /** + * This method checks whether the contained answer to a command sent to the SRTMinorServo system was positive or not + * @return true if the command was correctly accepted, false if the command was not accepted or the 'OUTPUT' key was not found (unlikely scenario) + */ + const bool checkOutput() const + { + std::shared_lock lock(m_mutex); + try + { + if(this->get("OUTPUT") == "GOOD") + { + return true; + } + } + catch(std::out_of_range& ex) + { + // Key not found + } + + return false; + } + + /** + * This method retrieves the ACS::Time associated with the received answer map. It converts the value from UNIX Epoch (double) to ACS::Time + * @return the ACS::Time associated with the answer map + */ + const ACS::Time getTimestamp() const + { + std::shared_lock lock(m_mutex); + return IRA::CIRATools::UNIXEpoch2ACSTime(this->get("TIMESTAMP")); + } + + /** + * This method returns the plain command sent using the socket. Useful for log purposes. + * @return a std::string containing the plain command sent using the socket. + */ + const std::string getPlainCommand() const + { + std::shared_lock lock(m_mutex); + return this->get("PLAIN_COMMAND"); + } + + /** + * This method returns the plain answer received from the socket. Useful for log purposes. + * @return a std::string containing the plain answer received from the socket. + */ + const std::string getPlainAnswer() const + { + std::shared_lock lock(m_mutex); + return this->get("PLAIN_ANSWER"); + } + + protected: + /** + * Shared mutex to control read and write accesses. Multiple reading access are permitted and will only block writing access. Writing access will block all accesses + */ + mutable std::shared_mutex m_mutex; + }; + + /** + * This class is a specialization of the SRTMinorServoAnswerMap for the general Leonardo Minor Servo System status. + */ + class SRTMinorServoGeneralStatus : public SRTMinorServoAnswerMap + { + public: + /** + * Retrieves the current configuration from the map. + * @return the current SRTMinorServoFocalConfiguration. + */ + SRTMinorServoFocalConfiguration getFocalConfiguration() const + { + return LDOConfigurationIDTable.right.at(this->get("CURRENT_CONFIG")); + } + + /** + * Retrieves a boolean indicating whether the simulation is enabled or not. + * @returns a boolean indicating whether the simulation is enabled or not. + */ + Management::TBoolean isSimulationEnabled() const + { + return this->get("SIMULATION_ENABLED") == 1 ? Management::MNG_TRUE : Management::MNG_FALSE; + } + + /** + * Returns the PLC time of the reading. + * @return a double indicating the PLC time, expressed as UNIX Epoch. + */ + double getPLCTime() const + { + return this->get("PLC_TIME"); + } + + /** + * Returns the firmware version present on the PLC. + * @return a string containing the firmware version. + */ + ACE_CString getPLCVersion() const + { + return this->get("PLC_VERSION").c_str(); + } + + /** + * Returns who is controlling the Leonardo Minor Servo System. + * @return an enum indicating who is controlling the system. + */ + SRTMinorServoControlStatus getControl() const + { + return SRTMinorServoControlStatus(this->get("CONTROL") - 1); + } + + /** + * Returns a boolean indicating whether the system is powered on or not. + * @return a boolean indicating whether the system is powered on or not. + */ + Management::TBoolean hasPower() const + { + return this->get("POWER") == 1 ? Management::MNG_TRUE : Management::MNG_FALSE; + } + + /** + * Is the emergency stop pressed somewhere? Is there an emergency situation? + * @return a boolean indicating if an emergency is present or not. + */ + Management::TBoolean emergencyPressed() const + { + return this->get("EMERGENCY") == 1 ? Management::MNG_TRUE : Management::MNG_FALSE; + } + + /** + * Returns the position of the gregorian cover. + * @return an enum indicating the position of the gregorian cover. + */ + SRTMinorServoGregorianCoverStatus getGregorianCoverPosition() const + { + return SRTMinorServoGregorianCoverStatus(this->get("GREGORIAN_CAP")); + } + + /** + * Returns the UNIX Epoch of the last executed command. + * @return a double containing the UNIX Epoch of the last executed command. + */ + double getLastExecutedCommand() const + { + return this->get("LAST_EXECUTED_COMMAND"); + } + }; + + /** + * This class is a specialization of the SRTMinorServoAnswerMap for a single servo status of the Leonardo Minor Servo System. + */ + class SRTMinorServoStatus : public SRTMinorServoAnswerMap + { + public: + /** + * Constructor. Accepts some lists of labels in order to correctly retrieve the values from the map. + * @param servo_name the servo name used as a prefix when retrieving the values. + * @param physical_axes_enabled the labels used to retrieve the status of each physical axis. + * @param physical_positions the labels used to retrieve the position of each physical axis. + * @param virtual_positions the labels used to retrieve the position of each virtual axis. + * @param virtual_offsets the labels used to retrieve the offset of each virtual axis. + */ + SRTMinorServoStatus( + const std::string& servo_name, + const std::vector& physical_axes_enabled, + const std::vector& physical_positions, + const std::vector& virtual_positions, + const std::vector& virtual_offsets + ) : + SRTMinorServoAnswerMap(), + m_servo_name(servo_name), + m_physical_axes_enabled(physical_axes_enabled), + m_physical_positions(physical_positions), + m_virtual_positions(virtual_positions), + m_virtual_offsets(virtual_offsets) + {} + + /** + * Returns a boolean indicating whether the servo is enabled. + * @returns true if enabled, false otherwise. + */ + Management::TBoolean isEnabled() const + { + return this->get(m_servo_name + "_ENABLED") == 1 ? Management::MNG_TRUE : Management::MNG_FALSE; + } + + /** + * Returns the status of the servo drive cabinet. + * @returns an enum indicating the status of the servo drive cabinet. + */ + SRTMinorServoCabinetStatus getDriveCabinetStatus() const + { + return SRTMinorServoCabinetStatus(this->get(m_servo_name + "_STATUS") - 1); + } + + /** + * Returns a boolean indicating whether the servo is blocked or not. + * @return true if the servo is blocked, false otherwise. + */ + Management::TBoolean isBlocked() const + { + return this->get(m_servo_name + "_BLOCK") == 1 ? Management::MNG_TRUE : Management::MNG_FALSE; + } + + /** + * Returns the operative mode of the servo. + * @return an enum indicating the operative mode of the servo. + */ + SRTMinorServoOperativeMode getOperativeMode() const + { + return SRTMinorServoOperativeMode(this->get(m_servo_name + "_OPERATIVE_MODE") / 10); + } + + /** + * Returns the status of each physical axis. + * @return a boolean sequence, true if the axis is enabled, false otherwise. + */ + ACS::booleanSeq getPhysicalAxesEnabled() const + { + std::shared_lock lock(m_mutex); + return getSequence(m_physical_axes_enabled); + } + + /** + * Returns the position of each physical axis. + * @return a double sequence containing the position of each physical axis. + */ + ACS::doubleSeq getPhysicalPositions() const + { + std::shared_lock lock(m_mutex); + return getSequence(m_physical_positions); + } + + /** + * Returns the plain position of each virtual axis. + * @return a double sequence containing the plain position of each virtual axis. + */ + ACS::doubleSeq getPlainVirtualPositions() const + { + std::shared_lock lock(m_mutex); + return getSequence(m_virtual_positions); + } + + /** + * Returns the actual position of each virtual axis, minus the offset. + * @return a double sequence containing the position of each virtual axis, minus the offset. + */ + ACS::doubleSeq getVirtualPositions() const + { + std::shared_lock lock(m_mutex); + ACS::doubleSeq virtual_positions = getPlainVirtualPositions(); + ACS::doubleSeq virtual_offsets = getVirtualOffsets(); + for(size_t i = 0; i < virtual_positions.length(); i++) + { + virtual_positions[i] -= virtual_offsets[i]; + } + + return virtual_positions; + } + + /** + * Returns the offset of each virtual axis. + * @return a double sequence containing the offset of each virtual axis. + */ + ACS::doubleSeq getVirtualOffsets() const + { + std::shared_lock lock(m_mutex); + return getSequence(m_virtual_offsets); + } + + private: + /** + * This method extracts a sequence, either boolean or double, from the map, and returns it. + * @param labels a vector of strings containing the labels to use in order to extract the corresponding values from the map. + * @return the composed sequence of booleans or doubles. + */ + template >> + T getSequence(const std::vector& labels) const + { + T sequence; + sequence.length(labels.size()); + + for(size_t i = 0; i < labels.size(); i++) + { + if constexpr(std::is_same_v) + { + sequence[i] = (bool)this->get(m_servo_name + "_" + labels[i]); + } + else if constexpr(std::is_same_v) + { + sequence[i] = this->get(m_servo_name + "_" + labels[i]); + } + } + + return sequence; + } + + /** + * The name of the servo, this is used as prefix when retrieving values from the map. + */ + const std::string m_servo_name; + + /** + * The labels for the enabled value of each physical axis. + */ + const std::vector m_physical_axes_enabled; + + /** + * The labels for the positions of each physical axis. + */ + const std::vector m_physical_positions; + + /** + * The labels for the positions of each virtual axis. + */ + const std::vector m_virtual_positions; + + /** + * The labels for the offsets of each virtual axis. + */ + const std::vector m_virtual_offsets; + }; +} + +#endif diff --git a/SRT/Libraries/SRTMinorServoLibrary/include/SRTMinorServoSocket.h b/SRT/Libraries/SRTMinorServoLibrary/include/SRTMinorServoSocket.h new file mode 100644 index 0000000000000000000000000000000000000000..a4a48973a9b547df613cfbf52296bf5330e7a76a --- /dev/null +++ b/SRT/Libraries/SRTMinorServoLibrary/include/SRTMinorServoSocket.h @@ -0,0 +1,203 @@ +#ifndef __SRTMINORSERVOSOCKET_H__ +#define __SRTMINORSERVOSOCKET_H__ + +/** + * SRTMinorServoSocket.h + * 2023/02/23 + * Giuseppe Carboni (giuseppe.carboni@inaf.it) + */ + +#include +#include +#include +#include +#include +#include "SRTMinorServoCommandLibrary.h" + +#define SOCKET_TIMEOUT 0.1 +#define CONFIG_DOMAIN "alma/" +#define CONFIG_DIRNAME "/MINORSERVO/Socket" + +namespace MinorServo +{ + /** + * Testing class forward declaration. + * The declaration of this class can be found in the SRTMinorServoTestingSocket.h header file. + * A developer must use this class for testing purposes if he needs to destroy the singleton socket instance as well. + */ + class SRTMinorServoTestingSocket; + + class SRTMinorServoSocket: public IRA::CSocket + { + /** + * This class implements a singleton socket. The singleton pattern was necessary to provide each servo system component communication capabilities with the Leonardo system. + * As long as all the said components run on the same container this will only be instanced once. + */ + public: + /** + * Calls the constructor and returns the singleton socket instance + * @param ip_address the IP address to which the socket will connect + * @param port the port to which the socket will connect + * @param timeout the timeout, in seconds, for the communication to be considered failed + * @throw MinorServoErrors::MinorServoErrorsEx when the user calls this method a second time with different IP address and port arguments + * @return the singleton socket instance, eventually connected to the given IP address and port, by reference + */ + static SRTMinorServoSocket& getInstance(std::string ip_address, int port, double timeout=SOCKET_TIMEOUT); + + /** + * Returns the previously generated singleton socket instance + * @throw MinorServoErrors::MinorServoErrorsExImpl when the user calls this method when the instance has not been generated yet + * @return the singleton socket instance, by reference + */ + static SRTMinorServoSocket& getInstance(); + + /** + * Sends a command on the socket and returns the received answer, if any + * @param command the command to be sent over the socket + * @param map, optional SRTMinorServoAnswerMap object. If provided, the 'map' argument content gets updated with the newly received answer + * @throw MinorServoErrors::MinorServoErrorsEx when the operation of sending or receiving fails unexpectedly + * @return the received answer to the given command + */ + SRTMinorServoAnswerMap sendCommand(std::string command, std::optional> map = {}); + + /** + * Copy constructor operator disabled by default + */ + SRTMinorServoSocket(SRTMinorServoSocket const&) = delete; + + /** + * Copy assignment operator disabled by default + */ + void operator=(SRTMinorServoSocket const&) = delete; + + /** + * Method to check if socket is connected + * @return true if the socket is connected, false otherwise + */ + const bool isConnected() const; + + private: + /** + * Declare the testing class as friend class in order for it to have access to private members for testing purposes + */ + friend class SRTMinorServoTestingSocket; + + /** + * Constructor method. Generates the singleton socket instance + * @param ip_address the IP address to which the socket will connect + * @param port the port to which the socket will connect + * @param timeout the timeout, in seconds, for the communication to be considered failed + */ + SRTMinorServoSocket(std::string ip_address, int port, double timeout); + + /** + * Destructor method. Closes the socket upon destruction + */ + ~SRTMinorServoSocket(); + + /** + * Connection function. It gets called every time the socket gets disconnected + * throw MinorServoErrors::MinorServoErrorsEx when the connection attempt fails + */ + void connect(); + + /** + * Instance of the socket. By default it gets initialized to a null pointer + */ + inline static SRTMinorServoSocket* m_instance = nullptr; + + /** + * IP address and port of the socket. Being object members their values only exist when a singleton socket object is created correctly + */ + std::string m_ip_address; + int m_port; + + /** + * Timeout for communication operations + */ + double m_timeout; + + /** + * Mutex object used to syncronize communications and prevent collisions between multiple threads + */ + std::mutex m_mutex; + + /** + * Library mutex, used only to synchronize the getInstance methods + */ + static std::mutex c_mutex; + + /** + * Socket status enumerator + */ + enum socket_status + { + NOTREADY, + TIMEOUT, + READY + } m_socket_status; + + /** + * Socket error variable. This stores an error condition in case it arises + */ + IRA::CError m_error; + }; + + + class SRTMinorServoSocketConfiguration + { + /** + * This class implements a singleton socket configuration object. The singleton pattern was necessary to provide each servo system component access to the singleton socket parameters. + * As long as all the said components run on the same container this will only be instanced once. + * The first component to instantiate the singleton configuration will also instantiate the socket, opening communications. + */ + public: + /** + * Calls the constructor and returns the singleton socket configuration instance + * @param containerServices, the container services necessary to read the configuration from the CDB + * @return the singleton socket configuration instance + */ + static SRTMinorServoSocketConfiguration& getInstance(maci::ContainerServices* containerServices); + + /** + * Copy constructor operator disabled by default + */ + SRTMinorServoSocketConfiguration(SRTMinorServoSocketConfiguration const&) = delete; + + /** + * Copy assignment operator disabled by default + */ + void operator=(SRTMinorServoSocketConfiguration const&) = delete; + + /** + * IP address and port of the socket configuration + */ + std::string m_ip_address; + int m_port; + + /** + * Timeout for communication operations + */ + double m_timeout; + + private: + /** + * Constructor method + * @param containerServices, the container services necessary to read the configuration from the CDB + * @return the socket configuration object + */ + SRTMinorServoSocketConfiguration(maci::ContainerServices* containerServices); + + /** + * Destructor method + */ + ~SRTMinorServoSocketConfiguration(); + + /** + * Instance of the socket configuration object. By default it gets initialized to a null pointer + */ + inline static SRTMinorServoSocketConfiguration* m_instance = nullptr; + }; +} + +#endif diff --git a/SRT/Libraries/SRTMinorServoLibrary/include/SRTMinorServoTestingSocket.h b/SRT/Libraries/SRTMinorServoLibrary/include/SRTMinorServoTestingSocket.h new file mode 100644 index 0000000000000000000000000000000000000000..da33e9dd9dfa89548598175e409f06bf907851e4 --- /dev/null +++ b/SRT/Libraries/SRTMinorServoLibrary/include/SRTMinorServoTestingSocket.h @@ -0,0 +1,25 @@ +#include "SRTMinorServoSocket.h" + +namespace MinorServo +{ + class SRTMinorServoTestingSocket: public SRTMinorServoSocket + { + /** + * This class is a friend class of SRTMinorServoSocket. It can be used for testing purposes without altering the behavior of the original class. + */ + public: + /** + * This method explicitly destroys the singleton socket instance. + * Each test in the same test file is executed under the same process, therefore a singleton instance that does not get destroyed will still exist in the next executed tests. + * We want to test a new instance of the socket every time, therefore the existence of this method is critical. + */ + static void destroyInstance() + { + if(SRTMinorServoSocket::m_instance != nullptr) + { + delete SRTMinorServoSocket::m_instance; + SRTMinorServoSocket::m_instance = nullptr; + } + } + }; +} diff --git a/SRT/Libraries/SRTMinorServoLibrary/include/SRTMinorServoUtils.h b/SRT/Libraries/SRTMinorServoLibrary/include/SRTMinorServoUtils.h new file mode 100644 index 0000000000000000000000000000000000000000..5df5c9eb174ad360a8d2e787152b7871373c7126 --- /dev/null +++ b/SRT/Libraries/SRTMinorServoLibrary/include/SRTMinorServoUtils.h @@ -0,0 +1,144 @@ +#ifndef _SRTMINORSERVOUTILS_H +#define _SRTMINORSERVOUTILS_H +#include +#include +#include +#include +#include +#include + +/** + * The following templates are useful if you want to check if a given type for the SRTMinorServoAnswerMap is accepted + */ +template +struct is_atomic { static constexpr bool value = false; }; + +template +struct is_atomic> { static constexpr bool value = true; }; + +template +inline constexpr bool is_atomic_v = is_atomic::value; + +template +struct is_any : public std::disjunction...> {}; + +template +inline constexpr bool is_any_v = (std::is_same_v || ...); + +template +struct is_string : public std::disjunction, char*, const char*, std::string>> {}; + +template +inline constexpr bool is_string_v = is_string::value; + +template +struct is_known : public std::disjunction>, is_string>> {}; + +template +inline constexpr bool is_known_v = is_known::value; + +template , std::is_same>>>> +struct DB_type +{ + using type = std::conditional_t, std::is_same>>, IRA::CString, std::conditional_t, long, double>>; +}; + +template +T getCDBValue(maci::ContainerServices* container_services, const std::string& field, const std::string component = "") +{ + using C = typename DB_type::type; + + C temp; + if(IRA::CIRATools::getDBValue(container_services, field.c_str(), temp, "alma/", component.c_str())) + { + if constexpr(std::is_same_v>) + { + std::vector values; + std::istringstream iss(std::string(temp).c_str()); + std::string token; + while(std::getline(iss, token, ',')) + { + double value; + try + { + value = std::stod(token); + } + catch(std::invalid_argument& ia) + { + _EXCPT(ComponentErrors::CDBAccessExImpl, ex, "SRTMinorServoUtils::getCDBValue()"); + ex.setFieldName(field.c_str()); + throw ex.getComponentErrorsEx(); + } + catch(std::out_of_range& oor) + { + _EXCPT(ComponentErrors::ValueOutofRangeExImpl, ex, "SRTMinorServoUtils::getCDBValue()"); + ex.setValueName(field.c_str()); + ex.setValueLimit(token.find('-') == std::string::npos ? std::numeric_limits::max() : std::numeric_limits::min()); + throw ex.getComponentErrorsEx(); + } + + values.push_back(value); + } + return values; + } + else if constexpr(std::is_same_v) + { + return (T)std::string(temp).c_str(); + } + else + { + return T(temp); + } + } + else + { + _EXCPT(ComponentErrors::CDBAccessExImpl, ex, "SRTMinorServoUtils::getCDBValue()"); + ex.setFieldName(field.c_str()); + throw ex.getComponentErrorsEx(); // Maybe throw the plain ex + } +} + +const char* getReasonFromEx(const auto& ex) +{ + std::string reason = "Unknown reason"; + + for(auto [name, value] : ex.errorTrace.data) + { + if(strcmp(name, "Reason") == 0) + { + reason = value; + break; + } + } + + return reason.c_str(); +} + +const char* getErrorFromEx(const auto& ex) +{ + std::string error(ex.errorTrace.routine); + + for(auto [name, value] : ex.errorTrace.data) + { + if(strcmp(name, "Reason") == 0) + { + error += ": " + std::string(value); + break; + } + } + + return error.c_str(); +} + +C11_IGNORE_WARNING_PUSH +C11_IGNORE_WARNING("-Wunused-function") + +static std::ostream& operator<<(std::ostream& out, const std::variant& value) +{ + std::visit([&out](const auto& val) { out << val; }, value); + return out; +} + +C11_IGNORE_WARNING_POP + +#endif diff --git a/SRT/Libraries/SRTMinorServoLibrary/src/Makefile b/SRT/Libraries/SRTMinorServoLibrary/src/Makefile index ab438abf3bb9442623a61c0c08b10ba08a67127d..9bc719dce482f1fe9ffc363592a5d4978d9b4232 100644 --- a/SRT/Libraries/SRTMinorServoLibrary/src/Makefile +++ b/SRT/Libraries/SRTMinorServoLibrary/src/Makefile @@ -23,7 +23,7 @@ USER_CFLAGS = -Wall # # additional include and library search paths # USER_INC = /usr/local/include -USER_LIB = -lgsl -lgslcblas -lm +# USER_LIB = # # MODULE CODE DESCRIPTION: @@ -35,11 +35,15 @@ USER_LIB = -lgsl -lgslcblas -lm # C programs (public and local) # ----------------------------- EXECUTABLES = -EXECUTABLES_L = +EXECUTABLES_L = + +xxxxx_OBJECTS = +xxxxx_CFLAGS = +xxxxx_LIBS = # # -xxxxx_OBJECTS = +xxxxx_OBJECTS = xxxxx_LDFLAGS = xxxxx_LIBS = @@ -50,17 +54,24 @@ xxxxx_LIBS = # # Includes (.h) files (public only) # --------------------------------- -INCLUDES = hexlib.h SRTMinorServoCommandLibrary.h +INCLUDES = SRTMinorServoCommandLibrary.h SRTMinorServoSocket.h SRTMinorServoTestingSocket.h SRTMinorServoUtils.h SRTMinorServoContainers.h #hexlib.h # # Libraries (public and local) # ---------------------------- -LIBRARIES = SRTMinorServoLibrary SRTMinorServoCommandLibrary +LIBRARIES = SRTMinorServoCommandLibrary SRTMinorServoSocketLibrary PySRTMinorServoCommandLibrary #SRTMinorServoLibrary LIBRARIES_L = -SRTMinorServoLibrary_OBJECTS = hexlib -SRTMinorServoLibrary_LIBS = +#SRTMinorServoLibrary_OBJECTS = hexlib +#SRTMinorServoLibrary_LIBS = gsl gslcblas m SRTMinorServoCommandLibrary_OBJECTS = SRTMinorServoCommandLibrary -SRTMinorServoCommandLibrary_LIBS = IRALibrary +SRTMinorServoCommandLibrary_CFLAGS = -std=c++17 -fconcepts +SRTMinorServoCommandLibrary_LIBS = pthread IRALibrary +PySRTMinorServoCommandLibrary_OBJECTS = PySRTMinorServoCommandLibrary +PySRTMinorServoCommandLibrary_CFLAGS = -std=c++17 -fconcepts +PySRTMinorServoCommandLibrary_LIBS = SRTMinorServoCommandLibrary boost_python3 +SRTMinorServoSocketLibrary_OBJECTS = SRTMinorServoSocket +SRTMinorServoSocketLibrary_LIBS = IRALibrary ComponentErrors MinorServoErrors SRTMinorServoCommandLibrary +SRTMinorServoSocket_CFLAGS = -std=c++17 -fconcepts # # @@ -87,7 +98,7 @@ PY_SCRIPTS_L = PY_MODULES = PY_MODULES_L = -PY_PACKAGES = +PY_PACKAGES = SRTMinorServoCommandLibrary PY_PACKAGES_L = pppppp_MODULES = diff --git a/SRT/Libraries/SRTMinorServoLibrary/src/PySRTMinorServoCommandLibrary.cpp b/SRT/Libraries/SRTMinorServoLibrary/src/PySRTMinorServoCommandLibrary.cpp new file mode 100644 index 0000000000000000000000000000000000000000..984618e0840382dc3e959038b60aa8185354291a --- /dev/null +++ b/SRT/Libraries/SRTMinorServoLibrary/src/PySRTMinorServoCommandLibrary.cpp @@ -0,0 +1,69 @@ +#include "PySRTMinorServoCommandLibrary.h" + +using namespace MinorServo; + +boost::python::object PySRTMinorServoCommandLibrary::status(const std::string servo_id) +{ + return stringToBytes(SRTMinorServoCommandLibrary::status(servo_id)); +} + +boost::python::object PySRTMinorServoCommandLibrary::setup(const std::string& configuration) +{ + return stringToBytes(SRTMinorServoCommandLibrary::setup(configuration)); +} + +boost::python::object PySRTMinorServoCommandLibrary::stow(const std::string& servo_id, unsigned int stow_position) +{ + return stringToBytes(SRTMinorServoCommandLibrary::stow(servo_id, stow_position)); +} + +boost::python::object PySRTMinorServoCommandLibrary::stop(const std::string& servo_id) +{ + return stringToBytes(SRTMinorServoCommandLibrary::stop(servo_id)); +} + +boost::python::object PySRTMinorServoCommandLibrary::preset(const std::string& servo_id, const boost::python::list& coordinates) +{ + return stringToBytes(SRTMinorServoCommandLibrary::preset(servo_id, pylist2cppvector(coordinates))); +} + +boost::python::object PySRTMinorServoCommandLibrary::programTrack(const std::string& servo_id, const unsigned long& trajectory_id, const unsigned long& point_id, const boost::python::list& coordinates, double start_time) +{ + return stringToBytes(SRTMinorServoCommandLibrary::programTrack(servo_id, trajectory_id, point_id, pylist2cppvector(coordinates), start_time)); +} + +boost::python::object PySRTMinorServoCommandLibrary::offset(const std::string& servo_id, const boost::python::list& coordinates) +{ + return stringToBytes(SRTMinorServoCommandLibrary::offset(servo_id, pylist2cppvector(coordinates))); +} + +boost::python::dict PySRTMinorServoCommandLibrary::parseAnswer(const std::string& answer) +{ + auto args = SRTMinorServoCommandLibrary::parseAnswer(answer); + + boost::python::dict dictionary; + + SRTMinorServoAnswerMap::iterator iter; + + for(iter = args.begin(); iter != args.end(); ++iter) + { + std::visit([dictionary, iter](const auto& var) mutable { dictionary[iter->first] = var; }, iter->second); + } + + return dictionary; +} + +std::vector PySRTMinorServoCommandLibrary::pylist2cppvector(const boost::python::list& py_list) +{ + std::vector cpp_vector; + for(unsigned int i = 0; i < len(py_list); i++) + { + cpp_vector.push_back(boost::python::extract(py_list[i])); + } + return cpp_vector; +} + +boost::python::object PySRTMinorServoCommandLibrary::stringToBytes(const std::string& command) +{ + return boost::python::object(boost::python::handle<>(PyBytes_FromString(command.c_str()))); +} diff --git a/SRT/Libraries/SRTMinorServoLibrary/src/SRTMinorServoCommandLibrary.cpp b/SRT/Libraries/SRTMinorServoLibrary/src/SRTMinorServoCommandLibrary.cpp index a3a937a93e18ade53fc35ef7b47a0856e7e2f63b..6d23c329131bf05030d9173766e63212d451e0b5 100644 --- a/SRT/Libraries/SRTMinorServoLibrary/src/SRTMinorServoCommandLibrary.cpp +++ b/SRT/Libraries/SRTMinorServoLibrary/src/SRTMinorServoCommandLibrary.cpp @@ -4,57 +4,62 @@ * Giuseppe Carboni (giuseppe.carboni@inaf.it) */ +#include +#include +#include #include "SRTMinorServoCommandLibrary.h" -std::string SRTMinorServoCommandLibrary::status(int servo_id) +using namespace MinorServo; + +std::string SRTMinorServoCommandLibrary::status(const std::string servo_id) { std::stringstream command; - command << "status"; - if(servo_id >= 0) + command << "STATUS"; + if(servo_id != "") { command << "=" << servo_id; } - command << std::endl; + command << CLOSER; return command.str(); } -std::string SRTMinorServoCommandLibrary::setup(std::string configuration) +std::string SRTMinorServoCommandLibrary::setup(const std::string& configuration) { std::stringstream command; - command << "setup=" << configuration << std::endl; + command << "SETUP=" << configuration << CLOSER; return command.str(); } -std::string SRTMinorServoCommandLibrary::stow(unsigned int servo_id, unsigned int stow_position) +std::string SRTMinorServoCommandLibrary::stow(const std::string& servo_id, const unsigned int stow_position) { std::stringstream command; - command << "stow=" << servo_id << "," << stow_position << std::endl; + command << "STOW=" << servo_id << "," << stow_position << CLOSER; return command.str(); } -std::string SRTMinorServoCommandLibrary::stop(unsigned int servo_id) +std::string SRTMinorServoCommandLibrary::stop(const std::string& servo_id) { std::stringstream command; - command << "stop=" << servo_id << std::endl; + command << "STOP=" << servo_id << CLOSER; return command.str(); } -std::string SRTMinorServoCommandLibrary::preset(unsigned int servo_id, std::vector coordinates) +std::string SRTMinorServoCommandLibrary::preset(const std::string& servo_id, const std::vector& coordinates) { std::stringstream command; - command << "preset=" << servo_id; + command << "PRESET=" << servo_id; for(unsigned int coordinate = 0; coordinate < coordinates.size(); coordinate++) { - command << "," << coordinates[coordinate]; + command << "," << std::fixed << std::setprecision(6) << coordinates[coordinate]; } - command << std::endl; + command << CLOSER; return command.str(); } -std::string SRTMinorServoCommandLibrary::programTrack(unsigned int servo_id, unsigned int trajectory_id, unsigned int point_id, std::vector coordinates, double start_time) +std::string SRTMinorServoCommandLibrary::programTrack(const std::string& servo_id, const unsigned long& trajectory_id, const unsigned long& point_id, const std::vector& coordinates, const double start_time) { std::stringstream command; - command << "programTrack=" << servo_id << "," << trajectory_id << "," << point_id << ","; + command << "PROGRAMTRACK=" << servo_id << "," << trajectory_id << "," << point_id << ","; if(start_time > 0) { command << std::fixed << std::setprecision(6) << start_time; @@ -64,11 +69,110 @@ std::string SRTMinorServoCommandLibrary::programTrack(unsigned int servo_id, uns command << "*"; } - command.unsetf(std::ios_base::floatfield); for(unsigned int coordinate = 0; coordinate < coordinates.size(); coordinate++) { - command << "," << coordinates[coordinate]; + command << "," << std::fixed << std::setprecision(6) << coordinates[coordinate]; } - command << std::endl; + command << CLOSER; return command.str(); } + +std::string SRTMinorServoCommandLibrary::offset(const std::string& servo_id, const std::vector& coordinates) +{ + std::stringstream command; + command << "OFFSET=" << servo_id; + for(unsigned int coordinate = 0; coordinate < coordinates.size(); coordinate++) + { + command << "," << std::fixed << std::setprecision(6) << coordinates[coordinate]; + } + command << CLOSER; + return command.str(); +} + +SRTMinorServoAnswerMap SRTMinorServoCommandLibrary::parseAnswer(const std::string& original_answer) +{ + // First thing first, standardize the separators and remove the newline/carriage return characters + std::string answer(original_answer); + std::replace(answer.begin(), answer.end(), ':', '='); + std::replace(answer.begin(), answer.end(), '|', ','); + answer.erase(std::remove(answer.begin(), answer.end(), '\n'), answer.end()); + answer.erase(std::remove(answer.begin(), answer.end(), '\r'), answer.end()); + + // Create the dictionary + SRTMinorServoAnswerMap args; + + std::stringstream ss(answer); + std::string token; + + try + { + // Loop through the tokens + while(std::getline(ss, token, ',')) + { + std::stringstream sss(token); + std::string key, value; + std::getline(sss, key, '='); + std::getline(sss, value); + + // No value, could be the timestamp + if(value.empty()) + { + if(args.contains("TIMESTAMP")) // Timestamp already found, some other value is missing + { + throw std::invalid_argument(std::string("Missing key for value " + value)); + } + + value = key; + key = "TIMESTAMP"; + } + + if(key == "OUTPUT") + { + if(value != "GOOD" && value != "BAD") + { + throw std::invalid_argument(std::string("Unrecognized OUTPUT value: " + value)); + } + + args.put(key, value); + } + else if(key == "TIMESTAMP") + { + size_t last_char; + args.put(key, std::stod(value, &last_char)); + if(last_char != value.size()) + { + throw std::invalid_argument(std::string("Wrong TIMESTAMP value: " + value)); + } + } + else + { + size_t last_char; + args.put(key, std::stol(value, &last_char)); + if(last_char != value.size()) + { + args.put(key, std::stod(value)); + } + } + } + + if(!args.contains("OUTPUT")) + { + throw std::invalid_argument(std::string("Missing OUTPUT value!")); + } + else if(args.checkOutput() && !args.contains("TIMESTAMP")) + { + throw std::invalid_argument(std::string("Missing TIMESTAMP value!")); + } + } + catch(const std::invalid_argument& ex) + { + // If we are not able to convert any of the values to the correct type, + // or if OUTPUT and/or TIMESTAMP is missing, we send back an empty dictionary. + // It will be the caller's duty to understand that something was wrong with the answer. + args.clear(); + } + + args.put("PLAIN_ANSWER", original_answer); + + return args; +} diff --git a/SRT/Libraries/SRTMinorServoLibrary/src/SRTMinorServoCommandLibrary/README.md b/SRT/Libraries/SRTMinorServoLibrary/src/SRTMinorServoCommandLibrary/README.md new file mode 100644 index 0000000000000000000000000000000000000000..7eb57a028c2942f8b6cc5ea7261debe58a274541 --- /dev/null +++ b/SRT/Libraries/SRTMinorServoLibrary/src/SRTMinorServoCommandLibrary/README.md @@ -0,0 +1,19 @@ +# SRTMinorServoCommandLibrary Python wrapper + +In order to use the SRTMinorServoCommandLibrary python wrapper, it is sufficient to import the SRTCommandLibrary package like this: + +`import SRTMinorServoCommandLibrary` + +The all the functions of the library can be used in the following fashion: + +`SRTMinorServoCommandLibrary.status()` +`SRTMinorServoCommandLibrary.status('PFP')` +`SRTMinorServoCommandLibrary.setup('KKG')` +`SRTMinorServoCommandLibrary.stow('PFP')` +`SRTMinorServoCommandLibrary.stow('PFP', 2)` +`SRTMinorServoCommandLibrary.stop('SRP')` +`SRTMinorServoCommandLibrary.preset('SRP', [0, 1, 2, 3, 4, 5])` +`SRTMinorServoCommandLibrary.programTrack('PFP', 0, 0, [0, 1, 2, 3, 4, 5], )` +`SRTMinorServoCommandLibrary.programTrack('PFP', 0, 1, [0, 1, 2, 3, 4, 5])` +`SRTMinorServoCommandLibrary.offset('SRP', [6, 7, 8, 9, 10, 11])` +`SRTMinorServoCommandLibrary.parseAnswer('OUTPUT:GOOD,1665757400.123456,PFP_ENABLED=5|PFP_STATUS=44|PFP_BLOCK=7|PFP_WARNING=47|PFP_ROTARY_AXIS_ENABLE=52|PFP_COORD_1=94')` diff --git a/SRT/Libraries/SRTMinorServoLibrary/src/SRTMinorServoCommandLibrary/__init__.py b/SRT/Libraries/SRTMinorServoLibrary/src/SRTMinorServoCommandLibrary/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..f07ae391eae2abae663875a0960cbac8d3bc385f --- /dev/null +++ b/SRT/Libraries/SRTMinorServoLibrary/src/SRTMinorServoCommandLibrary/__init__.py @@ -0,0 +1,17 @@ +# The following lines will look for the libPySRTMinorServoCommandLibrary in the +# $INTROOT/lib path instead of looking in the $INTROOT/lib/python/site-packages +# path. This is a workaround needed since the said library is a C++ shared +# library and it does not get automatically installed in the correct folder. +# By adding this workaround we're also able to rename the library with the +# module name SRTMinorServoCommandLibrary, trimming the 'libPy' header. I +# suggest to use this approach whenever a Python wrapper is needed. +import os +import sys +# Temporarily add the $INTROOT/lib path to Python libraries path +sys.path.append(os.path.join(os.environ['INTROOT'], 'lib')) +# Import the functions we need +from libPySRTMinorServoCommandLibrary import status, setup, stop, stow, preset, programTrack, offset, parseAnswer +# Remove the added path and unused imported modules +sys.path.remove(os.path.join(os.environ['INTROOT'], 'lib')) +del os +del sys diff --git a/SRT/Libraries/SRTMinorServoLibrary/src/SRTMinorServoSocket.cpp b/SRT/Libraries/SRTMinorServoLibrary/src/SRTMinorServoSocket.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f0606fe29a38105aebcf34348a15c2b1deb52516 --- /dev/null +++ b/SRT/Libraries/SRTMinorServoLibrary/src/SRTMinorServoSocket.cpp @@ -0,0 +1,237 @@ +#include "SRTMinorServoSocket.h" + +using namespace MinorServo; + +std::mutex SRTMinorServoSocket::c_mutex; + +SRTMinorServoSocket& SRTMinorServoSocket::getInstance(std::string ip_address, int port, double timeout) +{ + std::lock_guard guard(SRTMinorServoSocket::c_mutex); + + if(m_instance != nullptr) + { + if(m_instance->m_ip_address != ip_address || m_instance->m_port != port) + { + _EXCPT(MinorServoErrors::CommunicationErrorExImpl, impl, "SRTMinorServoSocket::getInstance(std::string, int)"); + impl.setReason(("Socket already open on '" + m_instance->m_ip_address + ":" + std::to_string(m_instance->m_port) + "' . Use getInstance() (no arguments) to retrieve the object.").c_str()); + throw impl.getMinorServoErrorsEx(); + } + } + else + { + m_instance = new SRTMinorServoSocket(ip_address, port, timeout); + } + return *m_instance; +} + +SRTMinorServoSocket& SRTMinorServoSocket::getInstance() +{ + std::lock_guard guard(SRTMinorServoSocket::c_mutex); + + if(m_instance == nullptr) + { + _EXCPT(MinorServoErrors::CommunicationErrorExImpl, impl, "SRTMinorServoSocket::getInstance()"); + impl.setReason("Socket not yet initialized. Use getInstance(std::string ip_address, int port) to initialize it and retrieve the object."); + throw impl.getMinorServoErrorsEx(); + } + return *m_instance; +} + +SRTMinorServoSocket::SRTMinorServoSocket(std::string ip_address, int port, double timeout) : m_ip_address(ip_address), m_port(port), m_timeout(timeout), m_socket_status(NOTREADY) +{ + try + { + connect(); + } + catch(...) + { + // Not yet connected, we catch the exception in order to go on + } +} + +SRTMinorServoSocket::~SRTMinorServoSocket() +{ + std::lock_guard guard(m_mutex); + Close(m_error); +} + +void SRTMinorServoSocket::connect() +{ + if(isConnected()) + { + return; + } + + std::lock_guard guard(m_mutex); + + Close(m_error); + m_error.Reset(); + if(Create(m_error, STREAM) == FAIL) + { + Close(m_error); + _EXCPT(MinorServoErrors::CommunicationErrorExImpl, impl, "SRTMinorServoSocket::SRTMinorServoSocket()"); + impl.setReason("Cannot create the socket."); + throw impl.getMinorServoErrorsEx(); + } + + if(Connect(m_error, m_ip_address.c_str(), m_port) == FAIL) + { + m_socket_status = TIMEOUT; + Close(m_error); + _EXCPT(MinorServoErrors::CommunicationErrorExImpl, impl, "SRTMinorServoSocket::SRTMinorServoSocket()"); + impl.setReason("Cannot connect the socket."); + throw impl.getMinorServoErrorsEx(); + } + + if(setSockMode(m_error, NONBLOCKING) != SUCCESS) + { + m_socket_status = NOTREADY; + Close(m_error); + _EXCPT(MinorServoErrors::CommunicationErrorExImpl, impl, "SRTMinorServoSocket::SRTMinorServoSocket()"); + impl.setReason("Cannot set the socket to non-blocking."); + throw impl.getMinorServoErrorsEx(); + } + + m_socket_status = READY; +} + +const bool SRTMinorServoSocket::isConnected() const +{ + return m_socket_status == READY ? true : false; +} + +SRTMinorServoAnswerMap SRTMinorServoSocket::sendCommand(std::string command, std::optional> map) +{ + std::lock_guard guard(m_mutex); + + connect(); + + double start_time = IRA::CIRATools::getUNIXEpoch(); + size_t sent_bytes = 0; + + while(sent_bytes < command.size()) + { + size_t sent_now; + + try + { + sent_now = Send(m_error, command.substr(sent_bytes, command.size() - sent_bytes).c_str(), command.size() - sent_bytes); + sent_bytes += sent_now; + } + catch(...) + { + m_socket_status = NOTREADY; + Close(m_error); + _EXCPT(MinorServoErrors::CommunicationErrorExImpl, impl, "SRTMinorServoSocker::sendCommand()"); + impl.setReason("Something went wrong while sending some bytes."); + throw impl.getMinorServoErrorsEx(); + } + + if(sent_now > 0) + { + // Reset the timer + start_time = IRA::CIRATools::getUNIXEpoch(); + } + else if(IRA::CIRATools::getUNIXEpoch() - start_time >= m_timeout) + { + m_socket_status = TIMEOUT; + Close(m_error); + _EXCPT(MinorServoErrors::CommunicationErrorExImpl, impl, "SRTMinorServoSocket::sendCommand()"); + impl.setReason("Timeout when sending command."); + throw impl.getMinorServoErrorsEx(); + } + } + + start_time = IRA::CIRATools::getUNIXEpoch(); + std::string answer; + + while(answer.size() < 2 || !(answer.rfind(CLOSER) == answer.size() - CLOSER.size())) + { + char buf; + try + { + if(Receive(m_error, &buf, 1) == 1) + { + answer += buf; + + // Reset the timer + start_time = IRA::CIRATools::getUNIXEpoch(); + } + else if(IRA::CIRATools::getUNIXEpoch() - start_time >= m_timeout) + { + m_socket_status = TIMEOUT; + Close(m_error); + _EXCPT(MinorServoErrors::CommunicationErrorExImpl, impl, "SRTMinorServoSocket::sendCommand()"); + impl.setReason("Timeout when receiving answer."); + throw impl.getMinorServoErrorsEx(); + } + } + catch(...) + { + m_socket_status = NOTREADY; + Close(m_error); + _EXCPT(MinorServoErrors::CommunicationErrorExImpl, impl, "SRTMinorServoSocker::sendCommand()"); + impl.setReason(("Something went wrong while receiving some bytes. Command: " + command).c_str()); + throw impl.getMinorServoErrorsEx(); + } + } + + SRTMinorServoAnswerMap map_answer = SRTMinorServoCommandLibrary::parseAnswer(answer); + map_answer.put("PLAIN_COMMAND", command); + if(map) + { + map->get() = map_answer; + } + + return map_answer; +} + +SRTMinorServoSocketConfiguration& SRTMinorServoSocketConfiguration::getInstance(maci::ContainerServices* containerServices) +{ + if(m_instance == nullptr) + { + m_instance = new SRTMinorServoSocketConfiguration(containerServices); + } + + return *m_instance; +} + +SRTMinorServoSocketConfiguration::SRTMinorServoSocketConfiguration(maci::ContainerServices* containerServices) +{ + AUTO_TRACE("SRTMinorServoSocketConfiguration::SRTMinorServoSocketConfiguration()"); + + IRA::CString _ip_address; + if(!IRA::CIRATools::getDBValue(containerServices, "IPAddress", _ip_address, CONFIG_DOMAIN, CONFIG_DIRNAME)) + { + _EXCPT(ComponentErrors::CDBAccessExImpl, impl, "SRTMinorServoSocketConfiguration::SRTMinorServoSocketConfiguration()"); + impl.setFieldName("IPAddress"); + throw impl.getComponentErrorsEx(); + } + m_ip_address = (std::string)_ip_address; + + DWORD port; + if(!IRA::CIRATools::getDBValue(containerServices, "Port", port, CONFIG_DOMAIN, CONFIG_DIRNAME)) + { + _EXCPT(ComponentErrors::CDBAccessExImpl, impl, "SRTMinorServoSocketConfiguration::SRTMinorServoSocketConfiguration()"); + impl.setFieldName("Port"); + throw impl.getComponentErrorsEx(); + } + else + { + m_port = port; + } + + if(!IRA::CIRATools::getDBValue(containerServices, "SocketTimeout", m_timeout, CONFIG_DOMAIN, CONFIG_DIRNAME)) + { + _EXCPT(ComponentErrors::CDBAccessExImpl, impl, "SRTMinorServoSocketConfiguration::SRTMinorServoSocketConfiguration()"); + impl.setFieldName("SocketTimeout"); + throw impl.getComponentErrorsEx(); + } +} + +SRTMinorServoSocketConfiguration::~SRTMinorServoSocketConfiguration() +{ + AUTO_TRACE("SRTMinorServoSocketConfiguration::~SRTMinorServoSocketConfiguration()"); + + delete m_instance; +} diff --git a/SRT/Libraries/SRTMinorServoLibrary/tests/.discos b/SRT/Libraries/SRTMinorServoLibrary/tests/.discos new file mode 100644 index 0000000000000000000000000000000000000000..1ada672a94fe0bc0155145468f693b3e4180735b --- /dev/null +++ b/SRT/Libraries/SRTMinorServoLibrary/tests/.discos @@ -0,0 +1,5 @@ +This file is here to differentiate between ACS style test directory and discos-style test. + +This is a discos test directory + +DO NOT REMOVE THIS FILE diff --git a/SRT/Libraries/SRTMinorServoLibrary/tests/Makefile b/SRT/Libraries/SRTMinorServoLibrary/tests/Makefile new file mode 100644 index 0000000000000000000000000000000000000000..517c0d13a03f1ac26736769ddf3368af0c3dffc5 --- /dev/null +++ b/SRT/Libraries/SRTMinorServoLibrary/tests/Makefile @@ -0,0 +1,97 @@ +# CPP UNIT TESTING SETUP +#-------------- +GTEST_HOME=/usr/local/include/gtest +GMOCK_HOME=/usr/local/include/gmock +GTEST_LIBS=gtest gtest_main + +USER_INC=-I$(GTEST_HOME) -I$(GMOCK_HOME) +# END OF CPP UNIT TESTING SETUP +#--------------------- + +# DEFINE YOUR CPP UNIT TEST EXECUTABLES HERE as: +# +# EXECTUABLES_L = unittest +# SRTMinorServoCommandLibraryTest_OBJECTS = unittest +# SRTMinorServoCommandLibraryTest_LIBS = $(GTEST_LIBS) + +EXECUTABLES_L = SRTMinorServoCommandLibraryTest SRTMinorServoSocketTest + +SRTMinorServoCommandLibraryTest_OBJECTS = SRTMinorServoCommandLibraryTest +SRTMinorServoCommandLibraryTest_CFLAGS = -std=c++17 -fconcepts +SRTMinorServoCommandLibraryTest_LIBS = $(GTEST_LIBS) SRTMinorServoCommandLibrary IRALibrary +SRTMinorServoCommandLibraryTest_LDFLAGS = -lstdc++ -lpthread + +SRTMinorServoSocketTest_OBJECTS = SRTMinorServoSocketTest +SRTMinorServoSocketTest_CFLAGS = -std=c++17 -fconcepts +SRTMinorServoSocketTest_LIBS = $(GTEST_LIBS) SRTMinorServoSocketLibrary SRTMinorServoCommandLibrary IRALibrary +SRTMinorServoSocketTest_LDFLAGS = -lstdc++ -lpthread + +# END OF CUSTOMIZATION +# do not edit below this line +#---------------------------- + +CSOURCENAMES = \ + $(foreach exe, $(EXECUTABLES) $(EXECUTABLES_L), $($(exe)_OBJECTS)) \ + $(foreach rtos, $(RTAI_MODULES) , $($(rtos)_OBJECTS)) \ + $(foreach lib, $(LIBRARIES) $(LIBRARIES_L), $($(lib)_OBJECTS)) + +MAKEDIRTMP := $(shell searchFile include/acsMakefile) +ifneq ($(MAKEDIRTMP),\#error\#) + MAKEDIR := $(MAKEDIRTMP)/include + include $(MAKEDIR)/acsMakefile +endif + +# TEST TARGETS +#TODO: unittest(2) discover pyunit + +do_unit: all + @echo "running cpp unit tests" + ../bin/unittest --gtest_output=xml:results/cppunittest.xml + +do_pyunit: + @echo "running python unit tests" + python -m unittest pyunit + +do_functional: + @echo "running python functional tests" + python -m unittest functional + +do_external: + @echo "running python external tests" + python -m unittest external + +clean_test: + rm -f results/*.xml + rm -f functional/*.pyc + rm -f pyunit/*.pyc + rm -f external/*.pyc + +unit: do_unit + @echo " . . . 'unit' done" + +pyunit: do_pyunit + @echo " . . . 'pyunit' done" + +functional: do_functional + @echo " . . . 'functional' done" + +external: do_external + @echo " . . . 'external' done" + +# TARGETS +# ------- +all: do_all + @echo " . . . 'all' done" + +clean : clean_all clean_test + @echo " . . . clean done" + +clean_dist : clean_all clean_dist_all clean_test + @echo " . . . clean_dist done" + +man : do_man + @echo " . . . man page(s) done" + +install : install_all + @echo " . . . installation done" + diff --git a/SRT/Libraries/SRTMinorServoLibrary/tests/SRTMinorServoCommandLibraryTest.cpp b/SRT/Libraries/SRTMinorServoLibrary/tests/SRTMinorServoCommandLibraryTest.cpp new file mode 100644 index 0000000000000000000000000000000000000000..b0fb317e20b1a40e5e8e6edd1997653edf5111c6 --- /dev/null +++ b/SRT/Libraries/SRTMinorServoLibrary/tests/SRTMinorServoCommandLibraryTest.cpp @@ -0,0 +1,108 @@ +#include "gtest/gtest.h" +#include +#include "SRTMinorServoCommandLibrary.h" + +using namespace MinorServo; + +TEST(SRTMinorServoCommandLibraryTest, status) +{ + EXPECT_EQ(SRTMinorServoCommandLibrary::status(), "STATUS\r\n"); + EXPECT_EQ(SRTMinorServoCommandLibrary::status("PFP"), "STATUS=PFP\r\n"); +} + +TEST(SRTMinorServoCommandLibraryTest, setup) +{ + EXPECT_EQ(SRTMinorServoCommandLibrary::setup("CCB"), "SETUP=CCB\r\n"); +} + +TEST(SRTMinorServoCommandLibraryTest, stow) +{ + EXPECT_EQ(SRTMinorServoCommandLibrary::stow("PFP"), "STOW=PFP,1\r\n"); + EXPECT_EQ(SRTMinorServoCommandLibrary::stow("PFP", 2), "STOW=PFP,2\r\n"); +} + +TEST(SRTMinorServoCommandLibraryTest, stop) +{ + EXPECT_EQ(SRTMinorServoCommandLibrary::stop("PFP"), "STOP=PFP\r\n"); +} + +TEST(SRTMinorServoCommandLibraryTest, preset) +{ + EXPECT_EQ(SRTMinorServoCommandLibrary::preset("PFP", std::vector{0.,1.,2.,3.,4.,5.}), "PRESET=PFP,0.000000,1.000000,2.000000,3.000000,4.000000,5.000000\r\n"); +} + +TEST(SRTMinorServoCommandLibraryTest, programTrack) +{ + double start_time = IRA::CIRATools::getUNIXEpoch() + 3; + unsigned long int trajectory_id = (unsigned long int)start_time; + std::stringstream expected_answer; + expected_answer << std::fixed << std::setprecision(6); + expected_answer << "PROGRAMTRACK=PFP," << trajectory_id << ",0," << start_time << ",0.000000,1.000000,2.000000,3.000000,4.000000,5.000000\r\n"; + EXPECT_EQ(SRTMinorServoCommandLibrary::programTrack("PFP", trajectory_id, 0, std::vector{0.,1.,2.,3.,4.,5.}, start_time), expected_answer.str()); + + for(unsigned int i = 1; i < 10; i++) + { + expected_answer.str(std::string()); + expected_answer << "PROGRAMTRACK=PFP," << trajectory_id << "," << i << ",*,0.000000,1.000000,2.000000,3.000000,4.000000,5.000000\r\n"; + EXPECT_EQ(SRTMinorServoCommandLibrary::programTrack("PFP", trajectory_id, i, std::vector{0.,1.,2.,3.,4.,5.}), expected_answer.str()); + } +} + +TEST(SRTMinorServoCommandLibraryTest, offset) +{ + EXPECT_EQ(SRTMinorServoCommandLibrary::offset("PFP", std::vector{0.,1.,2.,3.,4.,5.}), "OFFSET=PFP,0.000000,1.000000,2.000000,3.000000,4.000000,5.000000\r\n"); +} + +TEST(SRTMinorServoCommandLibraryTest, parseAnswer) +{ + // Minimal correct answer + std::string answer = "OUTPUT:GOOD,1665743366.654321\r\n"; + SRTMinorServoAnswerMap args; + args.put("OUTPUT", "GOOD"); + args.put("TIMESTAMP", 1665743366.654321); + EXPECT_EQ(SRTMinorServoCommandLibrary::parseAnswer(answer), args); + EXPECT_EQ(args.getTimestamp(), 138850361666543210); + EXPECT_TRUE(args.checkOutput()); + SRTMinorServoAnswerMap other; + other.put("OTHER", 123456); + args += other; + EXPECT_EQ(args.get("OTHER"), 123456); + EXPECT_EQ(args.getTimestamp(), 138850361666543210); + EXPECT_TRUE(args.checkOutput()); + + // Complete correct answer + answer = "OUTPUT:GOOD,1665743366.123456,CURRENT_CONFIG=21|SIMULATION_ENABLED=34|PLC_TIME=78|PLC_VERSION=69|CONTROL=14|POWER=38|EMERGENCY=69|ENABLED=51|OPERATIVE_MODE=94\r\n"; + args.clear(); + args.put("OUTPUT", "GOOD"); + args.put("TIMESTAMP", 1665743366.123456); + args.put("CURRENT_CONFIG", 21); + args.put("SIMULATION_ENABLED", 34); + args.put("PLC_TIME", 78); + args.put("PLC_VERSION", 69); + args.put("CONTROL", 14); + args.put("POWER", 38); + args.put("EMERGENCY", 69); + args.put("ENABLED", 51); + args.put("OPERATIVE_MODE", 94); + EXPECT_EQ(SRTMinorServoCommandLibrary::parseAnswer(answer), args); + + // Missing timestamp + answer = "OUTPUT:GOOD,CURRENT_CONFIG=21|SIMULATION_ENABLED=34|PLC_TIME=78|PLC_VERSION=69|CONTROL=14|POWER=38|EMERGENCY=69|ENABLED=51|OPERATIVE_MODE=94\r\n"; + args.clear(); + EXPECT_EQ(SRTMinorServoCommandLibrary::parseAnswer(answer), args); + + // Wrong OUTPUT field + answer = "OUTPUT:123456\r\n"; + args.clear(); + EXPECT_EQ(SRTMinorServoCommandLibrary::parseAnswer(answer), args); + + // Multiple values without key, cannot find the correct timestamp + answer = "OUTPUT:GOOD,12345,67890\r\n"; + args.clear(); + EXPECT_EQ(SRTMinorServoCommandLibrary::parseAnswer(answer), args); + + // Wrong timestamp format + answer = "OUTPUT:GOOD,12345.ABCD\r\n"; + args.clear(); + EXPECT_EQ(SRTMinorServoCommandLibrary::parseAnswer(answer), args); +} diff --git a/SRT/Libraries/SRTMinorServoLibrary/tests/SRTMinorServoSocketTest.cpp b/SRT/Libraries/SRTMinorServoLibrary/tests/SRTMinorServoSocketTest.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8fe3ba017fa349014719adc7f35c629737950ae7 --- /dev/null +++ b/SRT/Libraries/SRTMinorServoLibrary/tests/SRTMinorServoSocketTest.cpp @@ -0,0 +1,214 @@ +/* These tests require a running simulator and were designed * + * to test the thread-safeness and singleton design pattern of the * + * SRTMinorServoSocket class. */ +#include "gtest/gtest.h" +#include +#include +#include +#include "SRTMinorServoUtils.h" +#include "SRTMinorServoCommandLibrary.h" +#include "SRTMinorServoTestingSocket.h" + +// This address and port are the ones set in the simulator +// In order for the test to properly be executed, the simulator should be launched with the following command: +// discos-simulator -s minor_servo start & +#define ADDRESS std::string("127.0.0.1") +#define PORT 12800 +//#define ADDRESS std::string("192.168.200.13") +//#define PORT 4758 + +using namespace MinorServo; + +class SRTMinorServoSocketTest : public ::testing::Test +{ +protected: + std::vector commands; + std::vector threads; + + void SetUp() override + { + // The following commands yield an articulated answer in return + commands.push_back(SRTMinorServoCommandLibrary::status()); + commands.push_back(SRTMinorServoCommandLibrary::status("PFP")); + commands.push_back(SRTMinorServoCommandLibrary::status("SRP")); + commands.push_back(SRTMinorServoCommandLibrary::status("DerotatoreGFR1")); + commands.push_back(SRTMinorServoCommandLibrary::status("DerotatoreGFR2")); + commands.push_back(SRTMinorServoCommandLibrary::status("DerotatoreGFR3")); + commands.push_back(SRTMinorServoCommandLibrary::status("DerotatorePFP")); + commands.push_back(SRTMinorServoCommandLibrary::status("M3R")); + commands.push_back(SRTMinorServoCommandLibrary::status("GFR")); + } + + void TearDown() override + { + SRTMinorServoTestingSocket::destroyInstance(); + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } +}; + +// This test passes the already created instance to some threads +TEST_F(SRTMinorServoSocketTest, instance_passed_to_threads) +{ + SRTMinorServoSocket& socket = SRTMinorServoTestingSocket::getInstance(ADDRESS, PORT); + + if(!socket.isConnected()) + { + FAIL() << "Socket failed to connect. Check if the simulator or the hardware can be reached."; + } + + for(auto command : this->commands) + { + this->threads.push_back(std::thread([command, &socket]() + { + auto args = socket.sendCommand(command); + // By testing if the command was received correctly we also test if the socket is working properly + // and if the answer was received correctly without being interleaved with the answer from another thread + EXPECT_TRUE(args.checkOutput()); + })); + } + + std::for_each(this->threads.begin(), this->threads.end(), [](std::thread &t) + { + t.join(); + }); +} + +// This test spawns some threads, each one retrieves the instance. The first thread which tries to retrieve the instance will also generate it +TEST_F(SRTMinorServoSocketTest, instance_retrieved_in_threads) +{ + std::string error = ""; + + for(auto command : this->commands) + { + std::mutex mutex; + + this->threads.push_back(std::thread([command, &error, &mutex]() + { + try + { + auto args = SRTMinorServoTestingSocket::getInstance(ADDRESS, PORT).sendCommand(command); + // By testing if the command was received correctly we also test if the socket is working properly + // and if the answer was received correctly without being interleaved with the answer from another thread + EXPECT_TRUE(args.checkOutput()); + } + catch(MinorServoErrors::MinorServoErrorsEx& ex) + { + std::lock_guard guard(mutex); + + try + { + if(std::string(getReasonFromEx(ex)) == "Cannot connect the socket.") + { + error = "Socket failed to connect. Check if the simulator or the hardware can be reached."; + return; + } + } + catch(...) + { + } + + error = "Unexpected failure."; + } + })); + } + + std::for_each(this->threads.begin(), this->threads.end(), [](std::thread &t) + { + t.join(); + }); + + if(error != "") + { + FAIL() << error; + } +} + +// This test generates an instance on the given address and port, then tries to generate another instance with different address and port and fails +TEST_F(SRTMinorServoSocketTest, open_with_args_retrieve_without) +{ + try + { + // First let's open the socket with the chosen ADDRESS and PORT + SRTMinorServoTestingSocket::getInstance(ADDRESS, PORT); + + // Let's try to instance another socket on a different port + SRTMinorServoTestingSocket::getInstance(ADDRESS, PORT + 1); + } + catch(MinorServoErrors::MinorServoErrorsEx& ex) + { + try + { + std::string reason(getReasonFromEx(ex)); + + if(reason == "Cannot connect the socket.") + { + FAIL() << "Socket failed to connect. Check if the simulator or the hardware can be reached."; + return; + } + else + { + // Check if we got the correct exception + EXPECT_EQ(reason, "Socket already open on '" + ADDRESS + ":" + std::to_string(PORT) + "' . Use getInstance() (no arguments) to retrieve the object."); + } + } + catch(...) + { + FAIL() << "Unexpected failure."; + } + } +} + +// This test tries to retrieve an instance which has not been generated yet, failing +TEST_F(SRTMinorServoSocketTest, try_open_without_args) +{ + try + { + SRTMinorServoTestingSocket::getInstance(); + } + catch(MinorServoErrors::MinorServoErrorsEx& ex) + { + try + { + std::string reason(getReasonFromEx(ex)); + + if(reason == "Cannot connect the socket.") + { + FAIL() << "Socket failed to connect. Check if the simulator or the hardware can be reached."; + return; + } + else + { + // Check if we got the correct exception + EXPECT_EQ(reason, "Socket not yet initialized. Use getInstance(std::string ip_address, int port) to initialize it and retrieve the object."); + } + } + catch(...) + { + FAIL() << "Unexpected failure."; + } + } +} + +// This test tries to generate an instance using a pair of address and port on which the socket fails to open +TEST_F(SRTMinorServoSocketTest, try_open_on_wrong_address) +{ + try + { + // The exception is raised only if the given port is wrong + SRTMinorServoTestingSocket::getInstance(ADDRESS, 0); + } + catch(MinorServoErrors::MinorServoErrorsEx& ex) + { + try + { + std::string reason(getReasonFromEx(ex)); + + // Check if we got the correct exception + EXPECT_EQ(reason, "Cannot connect the socket."); + } + catch(...) + { + FAIL() << "Unexpected failure."; + } + } +} diff --git a/SRT/Libraries/SRTMinorServoLibrary/tests/external/__init__.py b/SRT/Libraries/SRTMinorServoLibrary/tests/external/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/SRT/Libraries/SRTMinorServoLibrary/tests/functional/__init__.py b/SRT/Libraries/SRTMinorServoLibrary/tests/functional/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/SRT/Libraries/SRTMinorServoLibrary/tests/pyunit/__init__.py b/SRT/Libraries/SRTMinorServoLibrary/tests/pyunit/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e62ae691a7613591cb1eefcea8723c0a912af120 --- /dev/null +++ b/SRT/Libraries/SRTMinorServoLibrary/tests/pyunit/__init__.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python +import unittest +import time +import SRTMinorServoCommandLibrary + +class TestPySRTMinorServoCommandLibrary(unittest.TestCase): + + def test_status(self): + command = SRTMinorServoCommandLibrary.status() + expected_command = b'STATUS\r\n' + self.assertEqual(command, expected_command) + + def test_status_servo(self): + command = SRTMinorServoCommandLibrary.status('PFP') + expected_command = b'STATUS=PFP\r\n' + self.assertEqual(command, expected_command) + + def test_setup(self): + command = SRTMinorServoCommandLibrary.setup('KKG') + expected_command = b'SETUP=KKG\r\n' + self.assertEqual(command, expected_command) + + def test_stow(self): + command = SRTMinorServoCommandLibrary.stow('SRP') + expected_command = b'STOW=SRP,1\r\n' + self.assertEqual(command, expected_command) + + def test_stow_position(self): + command = SRTMinorServoCommandLibrary.stow('SRP', 2) + expected_command = b'STOW=SRP,2\r\n' + self.assertEqual(command, expected_command) + + def test_stop(self): + command = SRTMinorServoCommandLibrary.stop('SRP') + expected_command = b'STOP=SRP\r\n' + self.assertEqual(command, expected_command) + + def test_preset(self): + command = SRTMinorServoCommandLibrary.preset('PFP', [0.1, 1.1, 2.1]) + expected_command = b'PRESET=PFP,0.100000,1.100000,2.100000\r\n' + self.assertEqual(command, expected_command) + + def test_programTrack(self): + now = time.time() + command = SRTMinorServoCommandLibrary.programTrack('PFP', 0, 0, [0.1, 1.1, 2.1], now) + expected_command = bytes(f'PROGRAMTRACK=PFP,0,0,{now:.6f},0.100000,1.100000,2.100000\r\n', encoding='latin-1') + self.assertEqual(command, expected_command) + for i in range(1, 10): + command = SRTMinorServoCommandLibrary.programTrack('PFP', 0, i, [0.1, 1.1, 2.1]) + expected_command = bytes(f'PROGRAMTRACK=PFP,0,{i},*,0.100000,1.100000,2.100000\r\n', encoding='latin-1') + self.assertEqual(command, expected_command) + + def test_offset(self): + command = SRTMinorServoCommandLibrary.offset('PFP', [0.1, 1.1, 2.1]) + expected_command = b'OFFSET=PFP,0.100000,1.100000,2.100000\r\n' + self.assertEqual(command, expected_command) + + def test_parseAnswer(self): + answer = 'OUTPUT:GOOD,1665743366.123456,CURRENT_CONFIG=21|SIMULATION_ENABLED=34|PLC_TIME=78|PLC_VERSION=69|CONTROL=14|POWER=38|EMERGENCY=69|ENABLED=51|OPERATIVE_MODE=94' + args = { + 'OUTPUT': 'GOOD', + 'TIMESTAMP': 1665743366.123456, + 'CURRENT_CONFIG': 21, + 'SIMULATION_ENABLED': 34, + 'PLC_TIME': 78, + 'PLC_VERSION': 69, + 'CONTROL': 14, + 'POWER': 38, + 'EMERGENCY': 69, + 'ENABLED': 51, + 'OPERATIVE_MODE': 94 + } + self.assertEqual( + SRTMinorServoCommandLibrary.parseAnswer(answer), + args + ) + + answer = "OUTPUT:GOOD,1665743366.654321" + args = { + "OUTPUT": "GOOD", + "TIMESTAMP": 1665743366.654321 + } + self.assertEqual( + SRTMinorServoCommandLibrary.parseAnswer(answer), + args + ) + + answer = b"OUTPUT:GOOD,CURRENT_CONFIG=21|SIMULATION_ENABLED=34|PLC_TIME=78|PLC_VERSION=69|CONTROL=14|POWER=38|EMERGENCY=69|ENABLED=51|OPERATIVE_MODE=94" + self.assertEqual(SRTMinorServoCommandLibrary.parseAnswer(answer), {}) + + answer = b"OUTPUT:123456" + self.assertEqual(SRTMinorServoCommandLibrary.parseAnswer(answer), {}) + + answer = b"OUTPUT:GOOD,12345,67890" + self.assertEqual(SRTMinorServoCommandLibrary.parseAnswer(answer), {}) + + answer = b"OUTPUT:GOOD,12345.ABCD" + self.assertEqual(SRTMinorServoCommandLibrary.parseAnswer(answer), {}) + + +if __name__ == '__main__': + unittest.main() diff --git a/SRT/Misc/SRTScripts/app-defaults/discosStartup.xml b/SRT/Misc/SRTScripts/app-defaults/discosStartup.xml index fe407ec9bbfc80792dd8e9c4f90e92a12cd5bbc9..29c8a78a6c2c36f4492f8224ffc7a7e55447d82b 100644 --- a/SRT/Misc/SRTScripts/app-defaults/discosStartup.xml +++ b/SRT/Misc/SRTScripts/app-defaults/discosStartup.xml @@ -14,7 +14,7 @@ - + @@ -109,15 +109,6 @@ MASTERHOST discos - - MinorServoBossContainer - cpp - - true - 0 - MASTERHOST - discos - MinorServoContainer cpp diff --git a/SRT/Misc/SRTScripts/app-defaults/simulationStartup.xml b/SRT/Misc/SRTScripts/app-defaults/simulationStartup.xml index 3a6e4fd0bb8ca863a14776ee78b26c55c9e35238..55d8ba5caea2d4b2a04c1c0fad0e7336af19c23e 100644 --- a/SRT/Misc/SRTScripts/app-defaults/simulationStartup.xml +++ b/SRT/Misc/SRTScripts/app-defaults/simulationStartup.xml @@ -109,15 +109,6 @@ MASTERHOST discos - - MinorServoBossContainer - cpp - - true - 0 - MASTERHOST - discos - MinorServoContainer cpp diff --git a/SRT/Servers/SRTMinorServo/config/CDB/schemas/SRTMinorServo.xsd b/SRT/Servers/SRTMinorServo/config/CDB/schemas/SRTMinorServo.xsd new file mode 100644 index 0000000000000000000000000000000000000000..d3f3d966fdfb4e3d67fc5f6537ef945da2848d69 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/config/CDB/schemas/SRTMinorServo.xsd @@ -0,0 +1,88 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/SRT/Servers/SRTMinorServo/config/CDB/schemas/SRTMinorServoBoss.xsd b/SRT/Servers/SRTMinorServo/config/CDB/schemas/SRTMinorServoBoss.xsd new file mode 100644 index 0000000000000000000000000000000000000000..2d87a89a548202705ff653a51a8386456f6d0612 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/config/CDB/schemas/SRTMinorServoBoss.xsd @@ -0,0 +1,83 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/SRT/Servers/SRTMinorServo/config/CDB/schemas/SRTMinorServoLookupTable.xsd b/SRT/Servers/SRTMinorServo/config/CDB/schemas/SRTMinorServoLookupTable.xsd new file mode 100644 index 0000000000000000000000000000000000000000..858936df267329f497190fd6f80b87837e75e7bc --- /dev/null +++ b/SRT/Servers/SRTMinorServo/config/CDB/schemas/SRTMinorServoLookupTable.xsd @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/SRT/Servers/SRTMinorServo/config/CDB/schemas/SRTMinorServoProperties.xsd b/SRT/Servers/SRTMinorServo/config/CDB/schemas/SRTMinorServoProperties.xsd new file mode 100644 index 0000000000000000000000000000000000000000..e1f7c7a5221d1071338130d4b27f1d4da03ac5e2 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/config/CDB/schemas/SRTMinorServoProperties.xsd @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/SRT/Servers/SRTMinorServo/config/CDB/schemas/SRTMinorServoSocketConfiguration.xsd b/SRT/Servers/SRTMinorServo/config/CDB/schemas/SRTMinorServoSocketConfiguration.xsd new file mode 100644 index 0000000000000000000000000000000000000000..b37cfaedd207b6eb8563e340cd7e9a13dbf786cf --- /dev/null +++ b/SRT/Servers/SRTMinorServo/config/CDB/schemas/SRTMinorServoSocketConfiguration.xsd @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/SRT/Servers/SRTMinorServo/include/MSDevIOs.h b/SRT/Servers/SRTMinorServo/include/MSDevIOs.h new file mode 100644 index 0000000000000000000000000000000000000000..330d3905dbf720dbf93ce024a6cda15ba38601fc --- /dev/null +++ b/SRT/Servers/SRTMinorServo/include/MSDevIOs.h @@ -0,0 +1,317 @@ +#ifndef _MSDEVIOS_H +#define _MSDEVIOS_H + +/** + * MSDevIOs.h + * Giuseppe Carboni (giuseppe.carboni@inaf.it) + */ + +#include "SuppressWarnings.h" +#include "SRTMinorServoContainers.h" +#include +#include +#include +#include "SRTMinorServoBossCore.h" + + +namespace MinorServo +{ + /** + * This class acts as a base class for all the SRTMinorServo DevIOs. + */ + template class MSBaseDevIO : public DevIO + { + public: + /** + * Destructor. + */ + ~MSBaseDevIO() + { + } + + /** + * Used to read the property value. + * This is pure virtual and has to be implemented in the derived DevIOs. + * @throw ComponentErrors::PropertyError. + * @param timestamp epoch when the operation completes. + */ + virtual T read(ACS::Time& timestamp); + + /** + * @return true to initialize the property with default value from CDB. + */ + bool initializeValue() + { + return false; + } + + /** + * It writes values into controller. Unused because all the properties are read-only. + */ + void write(const T& value, ACS::Time& timestamp) + { + timestamp = getTimeStamp(); + return; + } + }; + + /** + * This class is used to read the status of the motion of the minor servo system. + */ + class MSMotionInfoDevIO : public MSBaseDevIO + { + public: + /** + * Constructor. + * @param motion_status the atomic status of the motion of the minor servo system. + * @param answer_map a reference to the SRTMinorServoAnswerMap object containing the status of the system. It is used to read the position of the gregorian cover. + * @param scanning a reference to the TBoolean indicating whether the system is scanning or not. + * @param current_scan a reference to the SRTMinorServoScan object containing the parameters for the current scan. It is used to read the servo name and axis involved in the scan. + */ + MSMotionInfoDevIO(const std::atomic& motion_status, const SRTMinorServoGeneralStatus& boss_status, const std::atomic& scanning, const SRTMinorServoScan& current_scan) : + m_motion_status(motion_status), + m_boss_status(boss_status), + m_scanning(scanning), + m_current_scan(current_scan) + {} + + /** + * Returns the property value. + * @param timestamp epoch when the operation completes. + * @return a string containing the information about the motion status of the minor servo system. + */ + ACE_CString read(ACS::Time& timestamp) + { + std::string motion_status; + + switch(m_motion_status.load()) + { + case MOTION_STATUS_UNCONFIGURED: + { + motion_status = "Unknown"; + break; + } + case MOTION_STATUS_STARTING: + { + motion_status = "Setup in progress..."; + break; + } + case MOTION_STATUS_CONFIGURED: + { + motion_status = "Elevation Track Mode Disabled"; + break; + } + case MOTION_STATUS_TRACKING: + { + motion_status = "Elevation Track Mode"; + break; + } + case MOTION_STATUS_PARKING: + { + motion_status = "Parking..."; + break; + } + case MOTION_STATUS_PARKED: + { + motion_status = "Parked"; + + try + { + // If I can read the status of the gregorian cover I will notify the user about it on the GUI + SRTMinorServoGregorianCoverStatus cover_position = m_boss_status.getGregorianCoverPosition(); + + if(cover_position == COVER_STATUS_OPEN) + { + motion_status += ", gregorian cover open"; + } + else if(cover_position == COVER_STATUS_CLOSED) + { + motion_status += ", gregorian cover closed"; + } + } + catch(...) + { + // If I can't, it doesn't matter + } + + break; + } + case MOTION_STATUS_ERROR: + { + motion_status = "Error"; + break; + } + } + + if(m_scanning.load() == Management::MNG_TRUE) + { + motion_status = "Scanning along " + m_current_scan.servo_name + " " + m_current_scan.axis_name + " axis"; + } + + return motion_status.c_str(); + } + private: + /** + * Reference to the motion status object of the Boss. + */ + const std::atomic& m_motion_status; + + /** + * Reference to the SRTMinorServoGeneralStatus object of the Boss. + */ + const SRTMinorServoGeneralStatus& m_boss_status; + + /** + * Reference to the boolean telling if the system is scanning. + */ + const std::atomic& m_scanning; + + /** + * Reference to the SRTMinorServoScan object of the Boss. + */ + const SRTMinorServoScan& m_current_scan; + }; + + /** + * This template class is used to retrieve values from a SRTMinorServoAnswerMap and provide them as properties. + * The templates is specialized for the types listed right below and compilation will fail if the developer attempts to use it for an unknown MSDevIO type. + */ + template + && is_any_v + >> + class MSAnswerMapDevIO : public MSBaseDevIO + { + public: + /** + * Constructor, accepting the SRTMinorServoAnswerMap derived object and a pointer to the method used to retrieve the DevIO value. + * @param map, the SRTMinorServoAnswerMap derived object. + * @param method, the method to call in order to retrieve the return value. + */ + MSAnswerMapDevIO(const std::string& property_name, const Y& map, X (Y::*method)() const) : m_property_name(property_name), m_map(map), m_method(method) {} + + /** + * Used to read the property value. + * @param timestamp epoch when the operation completes. + * @throw ComponentErrors::PropertyError. + * @return the property value as read from the SRTMinorServoAnswerMap object reference. + */ + X read(ACS::Time& timestamp) + { + timestamp = getTimeStamp(); + + try + { + return (m_map.*m_method)(); + } + catch(std::out_of_range& ex) + { + _EXCPT(ComponentErrors::PropertyErrorExImpl, impl, "MSAnswerMapDevIO::read()"); + impl.setPropertyName(m_property_name.c_str()); + impl.setReason("Property is missing from the map!"); + throw impl; + } + catch(std::bad_variant_access& ex) + { + _EXCPT(ComponentErrors::PropertyErrorExImpl, impl, "MSAnswerMapDevIO::read()"); + impl.setPropertyName(m_property_name.c_str()); + impl.setReason("Attempt to access the property with the wrong variant type!"); + throw impl; + } + catch(ACSErr::ACSbaseExImpl& ex) + { + _ADD_BACKTRACE(ComponentErrors::PropertyErrorExImpl, impl, ex, "MSAnswerMapDevIO::read()"); + impl.setPropertyName(m_property_name.c_str()); + impl.setReason("Property could not be read!"); + throw impl; + } + } + + private: + /** + * The name of the property. + */ + const std::string m_property_name; + + /** + * The reference to the SRTMinorServoAnswerMap in which the readings from the PLC appear. This could be either a SRTMinorServoGeneralStatus or a SRTMinorServoStatus. + */ + const Y& m_map; + + /** + * Pointer to the method of the SRTMinorServoAnswerMap to call in order to retrieve the DevIO return value. + */ + X (Y::*m_method)() const; + }; + + /** + * This template class represents a generic Minor Servo DevIO. + * It accepts 2 types, the DevIO type (the return type) and the type of the object reference which stores the original value to be returned by the read method. + * The templates is specialized for the combinations of types listed right below and the compilation will fail if the developer attempts to use it with any other types combination. + */ + template || (std::is_same_v && std::is_same_v>) + >> + class MSGenericDevIO : public MSBaseDevIO + { + public: + /** + * Default constructor. + * @param value a constant reference to the object from which the DevIO will read the value to be returned as property. + */ + MSGenericDevIO(const A& value) : m_value(value) {} + + /** + * Used to read the property value. + * @param timestamp epoch when the operation completes. + * @return the property value read from the original referenced object. + */ + C read(ACS::Time& timestamp) + { + timestamp = getTimeStamp(); //completion time + + if constexpr(std::is_same_v) + { + return m_value.c_str(); + } + else if constexpr(std::is_same_v) + { + ACS::doubleSeq_var sequence = new ACS::doubleSeq; + sequence->length(m_value.size()); + + for(size_t i = 0; i < m_value.size(); i++) + { + sequence[i] = m_value.operator[](i); + } + + return sequence; + } + else if constexpr(is_atomic_v) + { + return m_value.load(); + } + else + { + return m_value; + } + } + + /** + * The reference to the object containing the value to be returned as property. + */ + const A& m_value; + }; +} + +#endif diff --git a/SRT/Servers/SRTMinorServo/include/SRTMinorServoBossCore.h b/SRT/Servers/SRTMinorServo/include/SRTMinorServoBossCore.h new file mode 100644 index 0000000000000000000000000000000000000000..157fe1f23cf1fa23965ce30a8912df7958047c8c --- /dev/null +++ b/SRT/Servers/SRTMinorServo/include/SRTMinorServoBossCore.h @@ -0,0 +1,437 @@ +#ifndef __SRTMINORSERVOBOSSCORE_H__ +#define __SRTMINORSERVOBOSSCORE_H__ + +/** + * SRTMinorServoBossCore.h + * Giuseppe Carboni (giuseppe.carboni@inaf.it) + */ + +#include "SuppressWarnings.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "SRTMinorServoSocket.h" +#include "MSDevIOs.h" +#include "SRTMinorServoBossImpl.h" +#include "SRTMinorServoStatusThread.h" +#include "SRTMinorServoSetupThread.h" +#include "SRTMinorServoParkThread.h" +#include "SRTMinorServoTrackingThread.h" +#include "SRTMinorServoScanThread.h" +#include "SRTMinorServoContainers.h" + + +_IRA_LOGFILTER_IMPORT; + +using namespace MinorServo; + +class SRTMinorServoBossImpl; +class SRTMinorServoStatusThread; +class SRTMinorServoSetupThread; +class SRTMinorServoParkThread; +class SRTMinorServoTrackingThread; +class SRTMinorServoScanThread; + + +/** + * This class implements the core functionalities for the SRTMinorServoBoss. It is constructed during the SRTMinorServoBossImpl component's construction. + * It handles all the threads and the procedures necessary for the minor servo system to work properly. + */ +class SRTMinorServoBossCore +{ + /** + * These classes needs full access to the SRTMinorServoBossCore object methods and attributes in order for the system to work properly. + */ + friend class SRTMinorServoBossImpl; + friend class SRTMinorServoStatusThread; + friend class SRTMinorServoSetupThread; + friend class SRTMinorServoParkThread; + friend class SRTMinorServoTrackingThread; + friend class SRTMinorServoScanThread; + +public: + /** + * Constructor. + * @param component a reference to the component object. Used in order to access the properties. + * @throw ComponentErrors::ComponentErrorsEx when reading configurations from the CDB. + */ + SRTMinorServoBossCore(SRTMinorServoBossImpl& component); + + /** + * Destructor. + */ + virtual ~SRTMinorServoBossCore(); + +private: + /** + * Reads the overall status from the hardware. + * @return true when the status is OK, false otherwise. + */ + bool status(); + + /** + * Performs a setup procedure. + * @param configuration a mnemonic code identifying the desired configuration. + * @throw ManagementErrors::ConfigurationErrorEx when something went wrong while performing the setup procedure or if checkLineStatus throws. + */ + void setup(std::string configuration); + + /** + * Performs a park procedure. + * @throw ManagementErrors::ParkingErrorEx when something went wrong while performing the park procedure or if checkLineStatus throws. + */ + void park(); + + /** + * Enables or disables the elevation tracking. + * @param configuration the desired elevation tracking configuration, allowed values are 'on', 'ON', 'off' and 'OFF'. + * @throw MinorServoErrors::MinorServoErrorsEx when a scan is waiting to be completed or when the passed configuration is unknown. + */ + void setElevationTracking(std::string configuration); + + /** + * Enables or disables the use of ASACTIVE configurations. + * @param configuration the desired active surface configuration, allowed values are 'on', 'ON', 'off' and 'OFF'. + * @throw MinorServoErrors::MinorServoErrorsEx when a scan is waiting to be completed or when the passed configuration is unknown. + */ + void setASConfiguration(std::string configuration); + + /** + * Opens or closes the gregorian cover. + * @param position the desired position for the gregorian cover, allowed values are 'open', 'OPEN', 'closed' or 'CLOSED'. + * @throw MinorServoErrors::MinorServoErrorsEx when the commanded position is unknown, when the system is not parked or parking, + * when anything goes wrong in the lower communication level or if checkLineStatus throws. + */ + void setGregorianCoverPosition(std::string position); + + /** + * Configures the whole minor servo system to a desired position. + * @param elevation the elevation to use for all the minor servo positions calculation. + * @throw MinorServoErrors::MinorServoErrorsEx when a scan is waiting to be completed, when the system was not configured yet, + * when a single minor servo component raises an error or when checkLineStatus throws. + */ + void preset(double elevation); + + /** + * Resets the given servo user offsets to 0. + * @param servo_name the name of the minor servo the user offsets will be reset to 0. + * @throw MinorServoErrors::MinorServoErrorsEx when a scan is waiting to be completed, when the system is not configured yet, when the given servo_name is unknown, + * when the given servo is not used in the current focal configuration or when checkLineStatus throws. + */ + void clearUserOffsets(std::string servo_name); + + /** + * Sets the given axis' user offset. + * @param servo_axis_name the minor servo and axis names, connected by a _ character. + * @param offset the desired user offset to be loaded for the given axis. + * @param log a boolean indicating whether the call comes from the SimpleParser or from outside sources. In case it comes from the SimpleParser, we will log the action, otherwise we won't. + * @throw MinorServoErrors::MinorServoErrorsEx when a scan is waiting to be completed, when the system is not configured yet, when the given servo or axis name are unknown, + * when the given servo is not used in the current focal configuration or when checkLineStatus throws. + */ + void setUserOffset(std::string servo_axis_name, double offset, bool log = false); + + /** + * Retrieves all the current user offsets. + * @throw MinorServoErrors::MinorServoErrorsEx when the system is not configured yet. + * @return a ACS::doubleSeq containing all the axes user offsets. The axes order is the same one retrieved with the getAxesInfo method. + */ + ACS::doubleSeq* getUserOffsets(); + + /** + * Resets the given servo system offsets to 0. + * @param servo_name the name of the minor servo the system offsets will be reset to 0. + * @throw MinorServoErrors::MinorServoErrorsEx when a scan is waiting to be completed, when the system is not configured yet, when the given servo_name is unknown, + * when the given servo is not used in the current focal configuration or when checkLineStatus throws. + */ + void clearSystemOffsets(std::string servo_name); + + /** + * Sets the given axis' system offset. + * @param servo_axis_name the minor servo and axis names, connected by a _ character. + * @param offset the desired system offset to be loaded for the given axis. + * @throw MinorServoErrors::MinorServoErrorsEx when a scan is waiting to be completed, when the system is not configured yet, when the given servo or axis name are unknown, + * when the given servo is not used in the current focal configuration or when checkLineStatus throws. + */ + void setSystemOffset(std::string servo_axis_name, double offset); + + /** + * Retrieves all the current system offsets. + * @throw MinorServoErrors::MinorServoErrorsEx when the system is not configured yet. + * @return a ACS::doubleSeq containing all the axes system offsets. The axes order is the same one retrieved with the getAxesInfo method. + */ + ACS::doubleSeq* getSystemOffsets(); + + /** + * Retrieves all the current axes names and units of measure. + * @param axes_names_out a reference to the sequence in which the method will put all the axes names. + * @param axes_units_out a reference to the sequence in which the method will put all the axes units of measure. + * @throw MinorServoErrors::MinorServoErrorsEx when the system is not configured yet. + */ + void getAxesInfo(ACS::stringSeq_out axes_names_out, ACS::stringSeq_out axes_units_out); + + /** + * Retrieves all the axes positions for a given epoch. + * @param acs_time the ACS::Time the user wants to retrieve all the axes positions for. + * @throw MinorServoErrors::MinorServoErrorsEx when the system is not configured yet. + * @return an ACS::doubleSeq containing all the axes positions for the given time. + */ + ACS::doubleSeq* getAxesPositions(ACS::Time acs_time); + + /** + * This method performs the calculations necessary to check if a scan can be performed starting from the given parameters. + * @param start_time the starting ACS::Time for the requested scan. + * @param scan_info the minor servo scan parameters for the requested scan. + * @param antenna_info the antenna scan parameters for the requested scan. + * @throw MinorServoErrors::MinorServoErrorsEx when there are issues with the given parameters or when the scan could not be performed for any reason. + * @return a SRTMinorServoScan object containing the parameters for a feasible scan. + */ + SRTMinorServoScan checkScanFeasibility(const ACS::Time& start_time, const MinorServoScan& scan_info, const Antenna::TRunTimeParameters& antenna_info); + + /** + * This method checks if a scan with the given parameters is feasible by calling the checkScanFeasibility method. + * @param start_time the starting ACS::Time for the requested scan. + * @param scan_info the minor servo scan parameters for the requested scan. + * @param antenna_info the antenna scan parameters for the requested scan. + * @param ms_parameters a reference to the object containing the calculated parameters for the requested scan. + * @return true if the requested scan is feasible, false otherwise. + */ + bool checkScan(const ACS::Time start_time, const MinorServoScan& scan_info, const Antenna::TRunTimeParameters& antenna_info, TRunTimeParameters_out ms_parameters); + + /** + * This method commands the start of a scan to the minor servo system. It calls the checkScanFeasibility method again to be sure the scan can actually be performed, updating the starting time. + * @param start_time a reference to the ACS::Time object. This value will be written by this method and it will represent the earliest time the minor servo system can perform the requested scan. + * @param scan_info the minor servo scan parameters for the requested scan. + * @param antenna_info the antenna scan parameters for the requested scan. + * @throw MinorServoErrors::MinorServoErrorsEx when there are issues with the line status, with the given parameters or when the scan could not be performed for any reason. + */ + void startScan(ACS::Time& start_time, const MinorServoScan& scan_info, const Antenna::TRunTimeParameters& antenna_info); + + /** + * Requests a stop for the ongoing scan. + * @param close_time the closing time for the ongoing scan. + * @throw MinorServoErrors::MinorServoErrorsEx when there are issues with the line status. + */ + void closeScan(ACS::Time& close_time); + + /** + * Retrieve the antenna elevation from the AntennaBoss component for the given ACS::Time. + * @param acs_time the ACS::Time we want to know the antenna elevation for. + * @throw ComponentErrors::ComponentErrorsEx when the AntennaBoss component could not be retrieved. + * @return the antenna elevation for the given time, expressed in degrees. + */ + double getElevation(const ACS::Time& acs_time); + + /** + * Checks if the socket is connected, if the Leonardo minor servo system is currently controlled by DISCOS and if it is not in emergency status. + * @throw MinorServoErrors::MinorServoErrorsEx when any of the above is false. + */ + void checkLineStatus(); + + /** + * Method that creates and starts an ACS thread object. + * It is a template specialized on the SRTMinorServoStatusThread, SRTMinorServoSetupThread, SRTMinorServoParkThread, SRTMinorServoTrackingThread and SRTMinorServoScanThread classes. + * @param thread pointer to the ACS thread object to be eventually created and started. + * @param sleep_time the optional thread sleep time, defaults to 0, meaning that this value will be ignored and internally the thread sleep time will be set to the default sleep time. + */ + template >> + void startThread(T*& thread, const ACS::TimeInterval& sleep_time = 0); + + /** + * Method that stops an ACS thread object. + * It is a template specialized on the SRTMinorServoStatusThread, SRTMinorServoSetupThread, SRTMinorServoParkThread, SRTMinorServoTrackingThread and SRTMinorServoScanThread classes. + * @param thread pointer to the ACS thread object to be stopped. + */ + template >> + void stopThread(T*& thread); + + /** + * Method that destroys an ACS Thread object. + * It is a template specialized on the SRTMinorServoStatusThread, SRTMinorServoSetupThread, SRTMinorServoParkThread, SRTMinorServoTrackingThread and SRTMinorServoScanThread classes. + * @param thread pointer to the ACS Thread object to be destroyed. + */ + template >> + void destroyThread(T*& thread); + + /** + * Method that sets all the necessary variable to signal a failure of the minor servo system. + */ + void setFailure(); + + /** + * Method used to retrieve a configuration value from the CDB. + * @param configuration the name of the value to be read from the CDB. + */ + Management::TBoolean getCDBConfiguration(std::string configuration); + + /** + * Reference to the component object. + */ + SRTMinorServoBossImpl& m_component; + + /** + * Pointer to the status thread. + */ + SRTMinorServoStatusThread* m_status_thread; + + /** + * Pointer to the setup thread. + */ + SRTMinorServoSetupThread* m_setup_thread; + + /** + * Pointer to the park thread. + */ + SRTMinorServoParkThread* m_park_thread; + + /** + * Pointer to the tracking thread. + */ + SRTMinorServoTrackingThread* m_tracking_thread; + + /** + * Pointer to the scan thread. + */ + SRTMinorServoScanThread* m_scan_thread; + + /** + * Pointer to the AntennaBoss component. + */ + Antenna::AntennaBoss_proxy m_antennaBoss; + + /** + * SRTMinorServoGeneralStatus object containing the status read from the PLC. + */ + SRTMinorServoGeneralStatus m_status; + + /** + * Enumeration indicating the status of the motion of the minor servo system. + */ + std::atomic m_motion_status; + + /** + * String containing the commanded focal configuration. + */ + std::string m_commanded_setup; + + /** + * Enumeration containing the commanded focal configuration. + */ + std::atomic m_commanded_configuration; + + /** + * Enumeration containing the status of the subsystem. + */ + std::atomic m_subsystem_status; + + /** + * String containing the current focal configuration name. + */ + std::string m_actual_setup; + + /** + * Boolean indicating whether the system is ready or not. + */ + std::atomic m_ready; + + /** + * Boolean indicating whether the system is performing a setup procedure or not. + */ + std::atomic m_starting; + + /** + * Boolean indicating whether the system is using ASACTIVE configurations or not. + */ + std::atomic m_as_configuration; + + /** + * Boolean indicating whether the system is currently tracking the elevation. + */ + std::atomic m_elevation_tracking; + + /** + * Boolean indicating whether the tracking of the elevation is enabled. + */ + std::atomic m_elevation_tracking_enabled; + + /** + * Boolean indicating whether the system can perform scans or not. + */ + std::atomic m_scan_active; + + /** + * Boolean indicating whether the system is scanning or not. + */ + std::atomic m_scanning; + + /** + * Boolean indicating whether the system is tracking or not. + */ + std::atomic m_tracking; + + /** + * This boolean will be set to true every time the socket connects. + * When true it will trigger a procedure that will check if the minor servos offsets need to be reloaded because of a discrepancy between the DISCOS offsets (user + system) and the Leonardo offsets. + */ + bool m_reload_servo_offsets; + + /** + * Configuration of the socket object. + */ + const SRTMinorServoSocketConfiguration& m_socket_configuration; + + /** + * Socket object. + */ + SRTMinorServoSocket& m_socket; + + /** + * Boolean indicating whether the socket is connected or not. + */ + std::atomic m_socket_connected; + + /** + * Map containing all the servos in the minor servo system. + */ + const std::map m_servos; + + /** + * Map containing all the tracking servos in the minor servo system. + */ + const std::map m_tracking_servos; + + /** + * Map that will dynamically be updated containing the current configuration's minor servos. + */ + std::map m_current_servos; + + /** + * Map that will dynamically be updated containing the current configuration's tracking minor servos. + */ + std::map m_current_tracking_servos; + + /** + * Current scan parameters. + */ + SRTMinorServoScan m_current_scan; + + /** + * Last scan parameters. + */ + SRTMinorServoScan m_last_scan; +}; + +#endif diff --git a/SRT/Servers/SRTMinorServo/include/SRTMinorServoBossImpl.h b/SRT/Servers/SRTMinorServo/include/SRTMinorServoBossImpl.h new file mode 100644 index 0000000000000000000000000000000000000000..40eb0c828bb51477f6a645542054fd0d413c8a3f --- /dev/null +++ b/SRT/Servers/SRTMinorServo/include/SRTMinorServoBossImpl.h @@ -0,0 +1,548 @@ +#ifndef __SRTMINORSERVOBOSSIMPL_H__ +#define __SRTMINORSERVOBOSSIMPL_H__ + +/** + * SRTMinorServoBossImpl.h + * Giuseppe Carboni (giuseppe.carboni@inaf.it) + */ + +#include "SuppressWarnings.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "MinorServoErrors.h" +#include "SRTMinorServoBossCore.h" +#include "SRTMinorServoStatusThread.h" +#include "MSDevIOs.h" +#include "SRTMinorServoCommon.h" + +using namespace MinorServo; + +// Forward classes definitions +class SRTMinorServoBossCore; +class SRTMinorServoStatusThread; + +class SRTMinorServoBossImpl : public baci::CharacteristicComponentImpl, public virtual POA_MinorServo::SRTMinorServoBoss +{ + friend class SRTMinorServoBossCore; +public: + /** + * Constructor. + * @param component_name the name of the component. + * @param container_services the ACS container services. + */ + SRTMinorServoBossImpl(const ACE_CString& component_name, maci::ContainerServices* container_services); + + /** + * Destructor. + */ + virtual ~SRTMinorServoBossImpl(); + + /** + * Called to give the component time to initialize itself. The component reads in configuration files/parameters, builds up connection. + * Called before execute. It is implemented as a synchronous (blocking) call. + * @throw ComponentErrors::ComponentErrorsEx when there has been a memory allocation issue with the properties pointers. + */ + virtual void initialize(); + + /** + * Called after initialize to tell the component that it has to be ready to accept incoming functional calls any time. + * Must be implemented as a synchronous (blocking) call. In this class the default implementation only logs the COMPSTATE_OPERATIONAL. + */ + virtual void execute(); + + /** + * Called by the container before destroying the server in a normal situation. This function takes charge of releasing all resources. + */ + virtual void cleanUp(); + + /** + * Called by the container in case of error or emergency situation. + * This function tries to free all resources even though there is no warranty that the function is completely executed before the component is destroyed. + */ + virtual void aboutToAbort(); + + /** + * Performs a setup procedure. + * @param configuration a mnemonic code identifying the desired configuration. + * @throw ManagementErrors::ConfigurationErrorEx when something went wrong while calling the SRTMinorServoBossCore method implementation. + */ + virtual void setup(const char* configuration); + + /** + * Performs a park procedure. + * @throw ManagementErrors::ConfigurationErrorEx when something went wrong while calling the SRTMinorServoBossCore method implementation. + */ + virtual void park(); + + /** + * Method that tells if the elevation tracking is enabled. + * @return a CORBA::Boolean indicating if the elevation tracking is enabled or not. + */ + virtual CORBA::Boolean isElevationTrackingEn(); + + /** + * Method that tells if the elevation is being tracked. + * @return a CORBA::Boolean indicating if the system is currently tracking the elevation or not. + */ + virtual CORBA::Boolean isElevationTracking(); + + /** + * Method that tells if the system is currently tracking the commanded position. + * @return a CORBA::Boolean indicating if the system is currently tracking below the given error threshold or not. + */ + virtual CORBA::Boolean isTracking(); + + /** + * Method that tells if the system is currently performing a setup procedure. + * @return a CORBA::Boolean indicating if the system is currently performing a setup procedure or not. + */ + virtual CORBA::Boolean isStarting(); + + /** + * Method that tells if the system is using ASACTIVE lookup tables or not. + * @return a CORBA::Boolean indicating if the system is configured to use ASACTIVE lookup tables or not. + */ + virtual CORBA::Boolean isASConfiguration(); + + /** + * Method that tells if the system is currently performing a parking procedure. + * @return a CORBA::Boolean indicating if the system is currently performing a parking procedure or not. + */ + virtual CORBA::Boolean isParking(); + + /** + * Method that tells if the system was configured correctly. + * @return a CORBA::Boolean indicating if the system is ready to be moved or not. + */ + virtual CORBA::Boolean isReady(); + + /** + * Method that tells if the system is currently performing a scan. + * @return a CORBA::Boolean indicating if the system is currently performing a scan or not. + */ + virtual CORBA::Boolean isScanning(); + + /** + * Method that tells if the system can currently perform a scan or not. + * @return a CORBA::Boolean indicating if the system can perform a scan or not. + */ + virtual CORBA::Boolean isScanActive(); + + /** + * Returns the name of the current focal configuration. + * @return a string containing the current focal configuration name. + */ + virtual char* getActualSetup(); + + /** + * Returns the name of the commanded focal configuration. + * @return a string containing the commanded focal configuration name. + */ + virtual char* getCommandedSetup(); + + /** + * Returns the central position for the axis involved in the current or last scan. + * @return a CORBA::Double containing the central position for the current scan axis. + * @throw MinorServoErrors::MinorServoErrorsEx when no scan has been performed yet. + */ + virtual CORBA::Double getCentralScanPosition(); + + /** + * Clears the user defined offsets from all the minor servos. + * @throw MinorServoErrors::MinorServoErrorsEx when a scan is waiting to be completed, when the system is not configured yet, when the given servo_name is unknown, + * when the given servo is not used in the current focal configuration or when checkLineStatus throws. + */ + virtual void clearOffsets(); + + /** + * Resets the given servo user offsets to 0. + * @param servo_name the name of the minor servo the user offsets will be reset to 0. + * @throw MinorServoErrors::MinorServoErrorsEx when a scan is waiting to be completed, when the system is not configured yet, when the given servo_name is unknown, + * when the given servo is not used in the current focal configuration or when checkLineStatus throws. + */ + virtual void clearUserOffset(const char* servo_name); + + /** + * Sets the given axis' user offset. CORBA IDL implementation. + * @param servo_axis_name the minor servo and axis names, connected by a _ character. + * @param offset the desired user offset to be loaded for the given axis. + * @param log a boolean indicating whether the call comes from the SimpleParser or from outside sources. In case it comes from the SimpleParser, we will log the action, otherwise we won't. + * @throw MinorServoErrors::MinorServoErrorsEx when a scan is waiting to be completed, when the system is not configured yet, when the given servo or axis name are unknown, + * when the given servo is not used in the current focal configuration or when checkLineStatus throws. + */ + virtual void setUserOffset(const char* servo_axis_name, CORBA::Double offset); + + /** + * Sets the given axis' user offset. SimpleParser implementation. + * @param servo_axis_name the minor servo and axis names, connected by a _ character. + * @param offset the desired user offset to be loaded for the given axis. + * @param log a boolean indicating whether the call comes from the SimpleParser or from outside sources. In case it comes from the SimpleParser, we will log the action, otherwise we won't. + * @throw MinorServoErrors::MinorServoErrorsEx when a scan is waiting to be completed, when the system is not configured yet, when the given servo or axis name are unknown, + * when the given servo is not used in the current focal configuration or when checkLineStatus throws. + */ + virtual void setUserOffset(const char* servo_axis_name, const double& offset); + + /** + * Retrieves all the current user offsets. + * @throw MinorServoErrors::MinorServoErrorsEx when the system is not configured yet. + * @return a ACS::doubleSeq containing all the axes user offsets. The axes order is the same one retrieved with the getAxesInfo method. + */ + virtual ACS::doubleSeq* getUserOffset(); + + /** + * Resets the given servo system offsets to 0. + * @param servo_name the name of the minor servo the system offsets will be reset to 0. + * @throw MinorServoErrors::MinorServoErrorsEx when a scan is waiting to be completed, when the system is not configured yet, when the given servo_name is unknown, + * when the given servo is not used in the current focal configuration or when checkLineStatus throws. + */ + virtual void clearSystemOffset(const char* servo_name); + + /** + * Sets the given axis' system offset. + * @param servo_axis_name the minor servo and axis names, connected by a _ character. + * @param offset the desired system offset to be loaded for the given axis. + * @throw MinorServoErrors::MinorServoErrorsEx when a scan is waiting to be completed, when the system is not configured yet, when the given servo or axis name are unknown, + * when the given servo is not used in the current focal configuration or when checkLineStatus throws. + */ + virtual void setSystemOffset(const char* servo_axis_name, CORBA::Double offset); + + /** + * Retrieves all the current system offsets. + * @throw MinorServoErrors::MinorServoErrorsEx when the system is not configured yet. + * @return a ACS::doubleSeq containing all the axes system offsets. The axes order is the same one retrieved with the getAxesInfo method. + */ + virtual ACS::doubleSeq* getSystemOffset(); + + /** + * Retrieves all the current axes names and units of measure. + * @param axes_names_out a reference to the sequence in which the method will put all the axes names. + * @param axes_units_out a reference to the sequence in which the method will put all the axes units of measure. + * @throw MinorServoErrors::MinorServoErrorsEx when the system is not configured yet. + */ + virtual void getAxesInfo(ACS::stringSeq_out axes_names, ACS::stringSeq_out axes_units); + + /** + * Gets the axis involved in the currently or last executed scan. + * @return a string containing the servo name and the axis name, connected by a _ character. + */ + virtual char* getScanAxis(); + + /** + * Retrieves all the axes positions for a given epoch. + * @param acs_time the ACS::Time the user wants to retrieve all the axes positions for. + * @throw MinorServoErrors::MinorServoErrorsEx when the system is not configured yet. + * @return an ACS::doubleSeq containing all the axes positions for the given time. + */ + virtual ACS::doubleSeq* getAxesPosition(ACS::Time acs_time); + + /** + * Enables or disables the elevation tracking. + * @param configuration the desired elevation tracking configuration, allowed values are 'on', 'ON', 'off' and 'OFF'. + * @throw MinorServoErrors::MinorServoErrorsEx when a scan is waiting to be completed or when the passed configuration is unknown. + */ + virtual void setElevationTracking(const char* elevation_tracking); + + /** + * Enables or disables the use of ASACTIVE configurations. + * @param configuration the desired active surface configuration, allowed values are 'on', 'ON', 'off' and 'OFF'. + * @throw MinorServoErrors::MinorServoErrorsEx when a scan is waiting to be completed or when the passed configuration is unknown. + */ + virtual void setASConfiguration(const char* as_configuration); + + /** + * Opens or closes the gregorian cover. + * @param position the desired position for the gregorian cover, allowed values are 'open', 'OPEN', 'closed' or 'CLOSED'. + * @throw MinorServoErrors::MinorServoErrorsEx when the commanded position is unknown, when the system is not parked or parking, + * when anything goes wrong in the lower communication level or if checkLineStatus throws. + */ + virtual void setGregorianCoverPosition(const char* position); + + /** + * This method checks if a scan with the given parameters is feasible. + * @param start_time the starting ACS::Time for the requested scan. + * @param scan_info the minor servo scan parameters for the requested scan. + * @param antenna_info the antenna scan parameters for the requested scan. + * @param ms_parameters a reference to the object containing the calculated parameters for the requested scan. + * @return true if the requested scan is feasible, false otherwise. + */ + virtual CORBA::Boolean checkScan(const ACS::Time start_time, const MinorServoScan& scan_info, const Antenna::TRunTimeParameters& antenna_info, TRunTimeParameters_out ms_parameters); + + /** + * This method commands the start of a scan to the minor servo system. + * @param start_time a reference to the ACS::Time object. This value will be written by this method and it will represent the earliest time the minor servo system can perform the requested scan. + * @param scan_info the minor servo scan parameters for the requested scan. + * @param antenna_info the antenna scan parameters for the requested scan. + * @throw MinorServoErrors::MinorServoErrorsEx when there are issues with the line status, with the given parameters or when the scan could not be performed for any reason. + */ + virtual void startScan(ACS::Time& start_time, const MinorServoScan& scan_info, const Antenna::TRunTimeParameters& antenna_info); + + /** + * Requests a stop for the ongoing scan. + * @param close_time the closing time for the ongoing scan. + * @throw MinorServoErrors::MinorServoErrorsEx when there are issues with the line status. + */ + virtual void closeScan(ACS::Time& close_time); + + /** + * Configures the whole minor servo system to a desired position. + * @param elevation the elevation to use for all the minor servo positions calculation. + * @throw MinorServoErrors::MinorServoErrorsEx when a scan is waiting to be completed, when the system was not configured yet, + * when a single minor servo component raises an error or when checkLineStatus throws. + */ + virtual void preset(double elevation); + + /** + * Parser method. It executes the desired command. + */ + virtual CORBA::Boolean command(const char*, CORBA::String_out); + + /** + * Returns a reference to the status property implementation of the IDL interface. + * @return pointer to read-only TSystemStatus property status. + */ + virtual Management::ROTSystemStatus_ptr status(); + + /** + * Returns a reference to the ready property implementation of the IDL interface. + * @return pointer to read-only TBoolean property ready. + */ + virtual Management::ROTBoolean_ptr ready(); + + /** + * Returns a reference to the actualSetup property implementation of the IDL interface. + * @return pointer to read-only string property actualSetup. + */ + virtual ACS::ROstring_ptr actualSetup(); + + /** + * Returns a reference to the motionInfo property implementation of the IDL interface. + * @return pointer to read-only string property motionInfo. + */ + virtual ACS::ROstring_ptr motionInfo(); + + /** + * Returns a reference to the starting property implementation of the IDL interface. + * @return pointer to read-only TBoolean property starting. + */ + virtual Management::ROTBoolean_ptr starting(); + + /** + * Returns a reference to the asConfiguration property implementation of the IDL interface. + * @return pointer to read-only TBoolean property asConfiguration. + */ + virtual Management::ROTBoolean_ptr asConfiguration(); + + /** + * Returns a reference to the elevationTrack property implementation of the IDL interface. + * @return pointer to read-only TBoolean property elevationTrack. + */ + virtual Management::ROTBoolean_ptr elevationTrack(); + + /** + * Returns a reference to the scanActive property implementation of the IDL interface. + * @return pointer to read-only TBoolean property scanActive. + */ + virtual Management::ROTBoolean_ptr scanActive(); + + /** + * Returns a reference to the scanning property implementation of the IDL interface. + * @return pointer to read-only TBoolean property scanning. + */ + virtual Management::ROTBoolean_ptr scanning(); + + /** + * Returns a reference to the tracking property implementation of the IDL interface. + * @return pointer to read-only TBoolean property tracking. + */ + virtual Management::ROTBoolean_ptr tracking(); + + /** + * Returns a reference to the connected property implementation of the IDL interface. + * @return pointer to read-only TBoolean property connected. + */ + virtual Management::ROTBoolean_ptr connected(); + + /** + * Returns a reference to the current_configuration property implementation of the IDL interface. + * @return pointer to read-only SRTMinorServoFocalConfiguration property current_configuration. + */ + virtual ROSRTMinorServoFocalConfiguration_ptr current_configuration(); + + /** + * Returns a reference to the simulation_enabled property implementation of the IDL interface. + * @return pointer to read-only TBoolean property simulation_enabled. + */ + virtual Management::ROTBoolean_ptr simulation_enabled(); + + /** + * Returns a reference to the plc_time property implementation of the IDL interface. + * @return pointer to read-only double property plc_time. + */ + virtual ACS::ROdouble_ptr plc_time(); + + /** + * Returns a reference to the plc_version property implementation of the IDL interface. + * @return pointer to read-only string property plc_version. + */ + virtual ACS::ROstring_ptr plc_version(); + + /** + * Returns a reference to the control property implementation of the IDL interface. + * @return pointer to read-only SRTMinorServoControlStatus property control. + */ + virtual ROSRTMinorServoControlStatus_ptr control(); + + /** + * Returns a reference to the power property implementation of the IDL interface. + * @return pointer to read-only TBoolean property power. + */ + virtual Management::ROTBoolean_ptr power(); + + /** + * Returns a reference to the emergency property implementation of the IDL interface. + * @return pointer to read-only TBoolean property emergency. + */ + virtual Management::ROTBoolean_ptr emergency(); + + /** + * Returns a reference to the gregorian_cover property implementation of the IDL interface. + * @return pointer to read-only SRTMinorServoGregorianCoverStatus property gregorian_cover. + */ + virtual ROSRTMinorServoGregorianCoverStatus_ptr gregorian_cover(); + + /** + * Returns a reference to the last_executed_command property implementation of the IDL interface. + * @return pointer to read-only double property last_executed_command. + */ + virtual ACS::ROdouble_ptr last_executed_command(); + +private: + /** + * Component name. + */ + const std::string m_component_name; + + /** + * SRTMinorServoBossCore object pointer. No delete is needed since it is handled by the shared_ptr logic. + */ + const std::shared_ptr m_core_ptr; + + /** + * SRTMinorServoBossCore object reference. + */ + SRTMinorServoBossCore& m_core; + + /** + * Command line parser object. + */ + SimpleParser::CParser m_parser; + + /** + * Pointer to the connected property. + */ + baci::SmartPropertyPointer> m_connected_ptr; + + /** + * Pointer to the status property. + */ + baci::SmartPropertyPointer> m_status_ptr; + + /** + * Pointer to the ready property. + */ + baci::SmartPropertyPointer> m_ready_ptr; + + /** + * Pointer to the actual_setup property. + */ + baci::SmartPropertyPointer m_actual_setup_ptr; + + /** + * Pointer to the motion_info property. + */ + baci::SmartPropertyPointer m_motion_info_ptr; + + /** + * Pointer to the starting property. + */ + baci::SmartPropertyPointer> m_starting_ptr; + + /** + * Pointer to the as_configuration property. + */ + baci::SmartPropertyPointer> m_as_configuration_ptr; + + /** + * Pointer to the elevation_tracking property. + */ + baci::SmartPropertyPointer> m_elevation_tracking_ptr; + + /** + * Pointer to the scan_active property. + */ + baci::SmartPropertyPointer> m_scan_active_ptr; + + /** + * Pointer to the scanning property. + */ + baci::SmartPropertyPointer> m_scanning_ptr; + + /** + * Pointer to the tracking property. + */ + baci::SmartPropertyPointer> m_tracking_ptr; + + /** + * Pointer to the current_configuration property. + */ + baci::SmartPropertyPointer> m_current_configuration_ptr; + + /** + * Pointer to the simulation_enabled property. + */ + baci::SmartPropertyPointer> m_simulation_enabled_ptr; + + /** + * Pointer to the plc_time property. + */ + baci::SmartPropertyPointer m_plc_time_ptr; + + /** + * Pointer to the plc_version property. + */ + baci::SmartPropertyPointer m_plc_version_ptr; + + /** + * Pointer to the control property. + */ + baci::SmartPropertyPointer> m_control_ptr; + + /** + * Pointer to the power property. + */ + baci::SmartPropertyPointer> m_power_ptr; + + /** + * Pointer to the emergency property. + */ + baci::SmartPropertyPointer> m_emergency_ptr; + + /** + * Pointer to the gregorian_cover property. + */ + baci::SmartPropertyPointer> m_gregorian_cover_ptr; + + /** + * Pointer to the last_executed_command property. + */ + baci::SmartPropertyPointer m_last_executed_command_ptr; +}; + +#endif diff --git a/SRT/Servers/SRTMinorServo/include/SRTMinorServoCommon.h b/SRT/Servers/SRTMinorServo/include/SRTMinorServoCommon.h new file mode 100644 index 0000000000000000000000000000000000000000..2048dbdf7fc680a5132838670c5f9ccc6201b2cb --- /dev/null +++ b/SRT/Servers/SRTMinorServo/include/SRTMinorServoCommon.h @@ -0,0 +1,38 @@ +#ifndef __SRTMINORSERVOCOMMON_H__ +#define __SRTMINORSERVOCOMMON_H__ + +/** + * SRTMinorServoCommon.h + * Giuseppe Carboni (giuseppe.carboni@inaf.it) + */ + +/** + * programTrack constants. The first indicates the time between two consecutive points, the second indicates the time we send each point in advance. + */ +#define PROGRAM_TRACK_TIMEGAP 2000000 //200 milliseconds, time between each programTrack point +#define PROGRAM_TRACK_FUTURE_TIME 26000000 //2.6 seconds, we send points this amount of time before their actual timestamp + +/** + * Macro used to link the properties pointers to their methods. + */ +#define GET_PROPERTY_REFERENCE(TYPE, CLASSNAME, PROPERTY, PROPERTYNAME) TYPE##_ptr CLASSNAME::PROPERTYNAME() \ +{ \ + if (PROPERTY==0) return TYPE::_nil(); \ + TYPE##_var tmp=TYPE::_narrow(PROPERTY->getCORBAReference()); \ + return tmp._retn(); \ +} + +/** + * Macro used to show the error on the operatorInput and on the jlog since the parser does not log Ex type exceptions + */ +#define LOG_EX(EXTYPE) \ +{ \ + EXTYPE##Impl impl(ex); \ + std::string _command(cmd); \ + std::string error = _command.substr(0, _command.find('=')) + "?"; \ + SP::CFormatter::exceptionToUser(impl, out); \ + error += out; \ + out = error.c_str(); \ +} + +#endif diff --git a/SRT/Servers/SRTMinorServo/include/SRTMinorServoImpl.h b/SRT/Servers/SRTMinorServo/include/SRTMinorServoImpl.h new file mode 100644 index 0000000000000000000000000000000000000000..2a43122df0d598c11415572b8b2ad8bc77be3fc8 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/include/SRTMinorServoImpl.h @@ -0,0 +1,745 @@ +#ifndef __SRTMINORSERVOIMPL_H__ +#define __SRTMINORSERVOIMPL_H__ + +/** + * SRTMinorServoImpl.h + * Giuseppe Carboni (giuseppe.carboni@inaf.it) + */ + +#include "SuppressWarnings.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "ManagementErrors.h" +#include "MinorServoErrors.h" +#include "SRTMinorServoSocket.h" +#include "MSDevIOs.h" +#include "SRTMinorServoContainers.h" +#include "SRTMinorServoCommon.h" + + +using namespace MinorServo; + +/** + * This class implements the base ACS::CharacteristicComponent CORBA interface for a SRTMinorServo component. + * It is inherited by the other classes declared below. + */ +class SRTBaseMinorServoImpl : public baci::CharacteristicComponentImpl +{ +public: + /** + * Constructor. + * @param component_name component's name. This is also the name that will be used to find the configuration data for the component in the Configuration DataBase. + * @param container_services pointer to the class that exposes all services offered by container. + * @throw ComponentErrors::ComponentErrorsEx when there has been an issue reading some value from the CDB. + */ + SRTBaseMinorServoImpl(const ACE_CString& component_name, maci::ContainerServices* container_services); + + /** + * Destructor. + */ + virtual ~SRTBaseMinorServoImpl(); + + /** + * Called to give the component time to initialize itself. The component reads in configuration files/parameters, builds up connection. + * Called before execute. It is implemented as a synchronous (blocking) call. + * @throw ComponentErrors::ComponentErrorsEx when there has been a memory allocation issue with the properties pointers. + */ + void initialize(); + + /** + * Called after initialize to tell the component that it has to be ready to accept incoming functional calls any time. + * Must be implemented as a synchronous (blocking) call. In this class the default implementation only logs the COMPSTATE_OPERATIONAL. + */ + void execute(); + + /** + * Called by the container before destroying the server in a normal situation. This function takes charge of releasing all resources. + */ + void cleanUp(); + + /** + * Called by the container in case of error or emergency situation. + * This function tries to free all resources even though there is no warranty that the function is completely executed before the component is destroyed. + */ + void aboutToAbort(); + + /** + * Asks the hardware the status of the servo system and updates the component properties. + * @return true if the communication succeeded, false otherwise. + * @throw MinorServoErrors::MinorServoErrorsEx when trying to reset the offsets when they don't match the ones loaded into the hardware. + */ + bool status(); + + /** + * Asks the servo system to perform a STOW operation. + * @param stow_position the position to get stowed into. + * @throw MinorServoErrors::MinorServoErrorsEx if there has been a communication error or if the command was not accepted. + */ + void stow(CORBA::Long stow_position = 1); + + /** + * Asks the servo system to perform a STOP operation. + * @throw MinorServoErrors::MinorServoErrorsEx if there has been a communication error or if the command was not accepted. + */ + void stop(); + + /** + * Asks the servo system to perform a PRESET operation. + * @param coordinates the sequence of coordinates to get into position to, the sequence length must be equal to the number of virtual axes of the servo system. + * @throw MinorServoErrors::MinorServoErrorsEx if the length of the coordinates sequence doesn't match the number of virtual axes of the servo, + * if the resulting position summing the offsets would go outside the accepted range of the servo, + * if there has been a communication error or if the command was not accepted. + */ + void preset(const ACS::doubleSeq& coordinates); + + /** + * Asks the servo system to load the commanded configuration table. + * @param configuration_name the configuration the servo system should assume. + * @throw ComponentErrors::ComponentErrorsEx when there is an error while trying to load the table for the given configuration. + * @return true if the servo is in use with the current configuration, false otherwise + */ + bool setup(const char* configuration_name = ""); + + /** + * Asks the component to calculate the servo system position starting from the given elevation. + * @param elevation the elevation we want to use to calculate and retrieve the servo system coordinates, expressed in degrees. + * @throw MinorServoErrors::MinorServoErrorsEx when the servo has not been configured yet and has not loaded any coefficient for the position calculation. + * @return a pointer to the double sequence object containing the calculated coordinates of the servo system. + */ + ACS::doubleSeq* calcCoordinates(CORBA::Double elevation); + + /** + * Asks the component the virtual axes user offsets. + * @return a pointer to the double sequence object containing the current virtual axes user offsets of the servo system. + */ + ACS::doubleSeq* getUserOffsets(); + + /** + * Load a single virtual axis user offset to the component and to the servo system. + * @param axis_name the name of the axis to load the given offset to. + * @param offset the desired user offset, expressed in millimeters or degrees, depending if the axis is a translation axis or a rotation one. + * @throw MinorServoErrors::MinorServoErrorsEx when the given axis_name is unknown, when the sum of user and system offsets for the given axis are out of range, + * when there has been a communication error or when the command was not accepted. + */ + void setUserOffset(const char* axis_name, CORBA::Double offset); + + /** + * Resets the virtual axes user offsets to 0. + * @throw MinorServoErrors::MinorServoErrorsEx when there has been a communication error or when the command was not accepted. + */ + void clearUserOffsets(); + + /** + * Asks the component the virtual axes system offsets. + * @return a pointer to the double sequence object containing the current virtual axes system offsets of the servo system. + */ + ACS::doubleSeq* getSystemOffsets(); + + /** + * Load a single virtual axis system offset to the component and to the servo system. + * @param axis_name the name of the axis to load the given offset to. + * @param offset the desired system offset, expressed in millimeters or degrees, depending if the axis is a translation axis or a rotation one. + * @throw MinorServoErrors::MinorServoErrorsEx when the given axis is unknown, when the sum of user and system offsets for the given axis are out of range, + * when there has been a communication error or when the command was not accepted. + */ + void setSystemOffset(const char* axis_name, CORBA::Double offset); + + /** + * Resets the virtual axes system offsets to 0. + * @throw MinorServoErrors::MinorServoErrorsEx when there has been a communication error or when the command was not accepted. + */ + void clearSystemOffsets(); + + /** + * Reload the user and the system offsets to the minor servo when the Leonardo offsets do not correspond to the sum of the DISCOS user and system offsets. + * @throw MinorServoErrors::MinorServoErrorsEx when there has been a communication error or when the command was not accepted. + */ + void reloadOffsets(); + + /** + * Returns the name and the unit of each virtual axes of the servo system, as reference arguments. + * @param axes_names_out a string sequence object containing the names of the virtual axes of the servo system. + * @param axes_units_out a string sequence object containing the units of the virtual axes of the servo system. + */ + void getAxesInfo(ACS::stringSeq_out axes_names_out, ACS::stringSeq_out axes_units_out); + + /** + * Returns the virtual axes positions to where the servo system was at the given time acs_time. + * @param acs_time the epoch we want to retrieve the axes virtual positions of the servo system. + * @throw MinorServoErrors::MinorServoErrorsEx when the position history is empty. + * @return a pointer to the double sequence object containing the virtual axes positions at the given epoch. + */ + ACS::doubleSeq* getAxesPositions(ACS::Time acs_time); + + /** + * Returns the maximum travel time to get from a starting position to a destination position. + * @param start a double sequence containing the starting position. If the provided sequence is empty, the current axes positions are used for the calculation. + * @param dest a double sequence containing the destination position. + * @throw MinorServoErrors::MinorServoErrorsEx when receiving a starting position sequence of length different from zero or the number of virtual axes of the servo, + * when receiving a destination position sequence of lenght different from the number of virtual axes of the servo + * @return an ACS::TimeInterval object representing the total duration of the movement from the starting position to the destination position + */ + ACS::TimeInterval getTravelTime(const ACS::doubleSeq& start, const ACS::doubleSeq& dest); + + /** + * Returns the minimum and maximum range of the virtual axes of the servo system, as reference arguments. + * @param min_ranges_out a double sequence object containing the minimum ranges of the virtual axes of the servo system. + * @param max_ranges_out a double sequence object containing the maximum ranges of the virtual axes of the servo system. + */ + void getAxesRanges(ACS::doubleSeq_out min_ranges_out, ACS::doubleSeq_out max_ranges_out); + + /** + * Returns a reference to the enabled property implementation of the IDL interface. + * @return pointer to the read-only boolean property enabled. + */ + virtual Management::ROTBoolean_ptr enabled(); + + /** + * Returns a reference to the drive_cabinet_status property implementation of the IDL interface. + * @return pointer to the read-only SRTMinorServoCabinerStatus (enumeration) property drive_cabinet_status. + */ + virtual ROSRTMinorServoCabinetStatus_ptr drive_cabinet_status(); + + /** + * Returns a reference to the block property implementation of the IDL interface. + * @return pointer to the read-only boolean property block. + */ + virtual Management::ROTBoolean_ptr block(); + + /** + * Returns a reference to the operative_mode property implementation of the IDL interface. + * @return pointer to the read-only SRTMinorServoOperativeMode (enumeration) property operative_mode. + */ + virtual ROSRTMinorServoOperativeMode_ptr operative_mode(); + + /** + * Returns a reference to the physical_axes_enabled property implementation of the IDL interface. + * @return pointer to the read-only boolean sequence property physical_axes_enabled. + */ + virtual ACS::RObooleanSeq_ptr physical_axes_enabled(); + + /** + * Returns a reference to the physical_positions property implementation of the IDL interface. + * @return pointer to the read-only boolean sequence property physical_positions. + */ + virtual ACS::ROdoubleSeq_ptr physical_positions(); + + /** + * Returns a reference to the virtual_axes property implementation of the IDL interface. + * @return pointer to the read-only long property virtual_axes. + */ + virtual ACS::ROlong_ptr virtual_axes(); + + /** + * Returns a reference to the plain_virtual_positions property implementation of the IDL interface. + * @return pointer to the read-only double sequence property plain_virtual_positions. + */ + virtual ACS::ROdoubleSeq_ptr plain_virtual_positions(); + + /** + * Returns a reference to the virtual_positions property implementation of the IDL interface. + * @return pointer to the read-only double sequence property virtual_positions. + */ + virtual ACS::ROdoubleSeq_ptr virtual_positions(); + + /** + * Returns a reference to the virtual_offsets property implementation of the IDL interface. + * @return pointer to the read-only double sequence property virtual_offsets. + */ + virtual ACS::ROdoubleSeq_ptr virtual_offsets(); + + /** + * Returns a reference to the virtual_user_offsets property implementation of the IDL interface. + * @return pointer to the read-only double sequence property virtual_user_offsets. + */ + virtual ACS::ROdoubleSeq_ptr virtual_user_offsets(); + + /** + * Returns a reference to the virtual_system_offsets property implementation of the IDL interface. + * @return pointer to the read-only double sequence property virtual_system_offsets. + */ + virtual ACS::ROdoubleSeq_ptr virtual_system_offsets(); + + /** + * Returns a reference to the in_use property implementation of the IDL interface. + * @return pointer to the read-only boolean property in_use. + */ + virtual Management::ROTBoolean_ptr in_use(); + + /** + * Returns a reference to the current_setup property implementation of the IDL interface. + * @return pointer to the read-only string property current_setup. + */ + virtual ACS::ROstring_ptr current_setup(); + +protected: + /** + * Checks if the socket is connected and if the minor servo system is in a good state. + * @throw MinorServoErrors::MinorServoErrorsEx when the socket is not connected or when the minor servo system is blocked or the drive cabinet is in error state. + */ + void checkLineStatus(); + + /** + * Static function used to retrieve some constants from the component CDB xml schema. + * @param object the instance of this class, used inside the function logic. + * @param constant the name of the constants we want to retrieve from the CDB. + * @throw ComponentErrors::ComponentErrorsEx when the requested value cannot be read from the CDB or when it has a non meaningful value. + * @return a vector of doubles containing the retrieved constants, its length is the same as the number of virtual axes of the servo system. + */ + static std::vector getMotionConstant(SRTBaseMinorServoImpl& object, const std::string& constant); + +private: + /** + * Static function used to retrieve a table from the CDB DataBlock directory. Used inside the initialization list. + * @param object the instance of this class, used inside the function logic. + * @param properties_name the name of the block to retrieve as written inside the DataBlock file. + * @throw ComponentErrors::ComponentErrorsEx when the requested value cannot be read from the CDB. + * @return a vector of strings containing the retrieved table fields. + */ + static std::vector getPropertiesTable(SRTBaseMinorServoImpl& object, const std::string& properties_name); + + /** + * Attributes. + * Keep the same order for the initialization list. + */ +protected: + /** + * Name of the component. + */ + const std::string m_component_name; + + /** + * Name of the servo system. + */ + const std::string m_servo_name; + + /** + * Number of virtual axes of the servo system. + */ + const size_t m_virtual_axes; +private: + /** + * Number of physical axes of the servo system. + */ + const size_t m_physical_axes; + + /** + * Name of the virtual axes of the servo system. + */ + const std::vector m_virtual_axes_names; + + /** + * Units of the virtual axes of the servo system. + */ + const std::vector m_virtual_axes_units; +protected: + /** + * Dictionary containing the last status retrieved form the servo system. + */ + SRTMinorServoStatus m_status; + + /** + * Commanded user offsets for each axis of the servo system. + */ + std::vector m_user_offsets; + + /** + * Commanded system offsets for each axis of the servo system. + */ + std::vector m_system_offsets; + + /** + * Queue of positions assumed by the servo system in time. + */ + SRTMinorServoPositionsQueue m_positions_queue; + + /** + * Minimum ranges of the axes of the servo system. + */ + const std::vector m_min; + + /** + * Maximum ranges of the axes of the servo system. + */ + const std::vector m_max; +private: + /** + * Maximum speeds of the axes of the servo system. + */ + const std::vector m_m_s; + + /** + * Accelerations of the axes of the servo system. + */ + const std::vector m_a; + + /** + * Times to perform a full acceleration ramp from 0 to maximum speed, for each axis. + */ + const std::vector m_r_t; + + /** + * Distances covered by a full acceleration ramp from 0 to maximum speed, for each axis. + */ + const std::vector m_r_d; + + /** + * Current speed of the axes of the servo system. + */ + std::vector m_c_s; + + /** + * Boolean indicating whether the servo system is used in the current focal configuration. + */ + std::atomic m_in_use; + + /** + * Current active setup name. + */ + std::string m_current_setup; + + /** + * Pointer to the enabled property. + */ + baci::SmartPropertyPointer> m_enabled_ptr; + + /** + * Pointer to the drive_cabinet_status property. + */ + baci::SmartPropertyPointer> m_drive_cabinet_status_ptr; + + /** + * Pointer to the block property. + */ + baci::SmartPropertyPointer> m_block_ptr; + + /** + * Pointer to the operative_mode property. + */ + baci::SmartPropertyPointer> m_operative_mode_ptr; + + /** + * Pointer to the physical_axes_enabled property. + */ + baci::SmartPropertyPointer m_physical_axes_enabled_ptr; + + /** + * Pointer to the physical_positions property. + */ + baci::SmartPropertyPointer m_physical_positions_ptr; + + /** + * Pointer to the virtual_axes property. + */ + baci::SmartPropertyPointer m_virtual_axes_ptr; + + /** + * Pointer to the plain_virtual_positions property. + */ + baci::SmartPropertyPointer m_plain_virtual_positions_ptr; + + /** + * Pointer to the virtual_positions property. + */ + baci::SmartPropertyPointer m_virtual_positions_ptr; + + /** + * Pointer to the virtual_offsets property. + */ + baci::SmartPropertyPointer m_virtual_offsets_ptr; + + /** + * Pointer to the virtual_user_offsets property. + */ + baci::SmartPropertyPointer m_virtual_user_offsets_ptr; + + /** + * Pointer to the virtual_system_offsets property. + */ + baci::SmartPropertyPointer m_virtual_system_offsets_ptr; + + /** + * Pointer to the in_use property. + */ + baci::SmartPropertyPointer> m_in_use_ptr; + + /** + * Pointer to the current_setup property. + */ + baci::SmartPropertyPointer m_current_setup_ptr; + + /** + * Table containing the coefficients for the positions calculations. + */ + SRTMinorServoLookupTable m_current_lookup_table; + + /** + * Configuration of the socket object. + */ + const SRTMinorServoSocketConfiguration& m_socket_configuration; +protected: + /** + * Socket object. + */ + SRTMinorServoSocket& m_socket; +}; + +/** + * MACRO definition of child classes methods. + * This simplifies the declaration since these methods' implementations are the same for both SRTGenericMinorServo and SRTProgramTrackMinorServo classes. + * This was necessary since these are pure virtual methods in POA_MinorServo::SRTGenericMinorServo and POA_MinorServo::SRTProgramTrackMinorServo, + * and even if they are inherited from the SRTBaseMinorServo class they are not seen by the compiler and they must be declared inside the respective classes. + * Take a look to the SRTBaseMinorServoImpl class for more information for each of these methods. + */ +#define METHODS_DECLARATION \ + void stow(CORBA::Long stow_position = 1) { SRTBaseMinorServoImpl::stow(stow_position); }\ + void stop() { SRTBaseMinorServoImpl::stop(); }\ + void preset(const ACS::doubleSeq& coordinates) { SRTBaseMinorServoImpl::preset(coordinates); }\ + ACS::doubleSeq* calcCoordinates(CORBA::Double elevation) { return SRTBaseMinorServoImpl::calcCoordinates(elevation); }\ + ACS::doubleSeq* getUserOffsets() { return SRTBaseMinorServoImpl::getUserOffsets(); }\ + void setUserOffset(const char* axis_name, CORBA::Double offset) { SRTBaseMinorServoImpl::setUserOffset(axis_name, offset); }\ + void clearUserOffsets() { SRTBaseMinorServoImpl::clearUserOffsets(); }\ + ACS::doubleSeq* getSystemOffsets() { return SRTBaseMinorServoImpl::getSystemOffsets(); }\ + void setSystemOffset(const char* axis_name, CORBA::Double offset) { SRTBaseMinorServoImpl::setSystemOffset(axis_name, offset); }\ + void clearSystemOffsets() { SRTBaseMinorServoImpl::clearSystemOffsets(); }\ + void reloadOffsets() { SRTBaseMinorServoImpl::reloadOffsets(); }\ + void getAxesInfo(ACS::stringSeq_out axes_names_out, ACS::stringSeq_out axes_units_out) { SRTBaseMinorServoImpl::getAxesInfo(axes_names_out, axes_units_out); }\ + ACS::doubleSeq* getAxesPositions(ACS::Time acs_time) { return SRTBaseMinorServoImpl::getAxesPositions(acs_time); }\ + long getTravelTime(const ACS::doubleSeq& start, const ACS::doubleSeq& dest) { return SRTBaseMinorServoImpl::getTravelTime(start, dest); }\ + void getAxesRanges(ACS::doubleSeq_out min_ranges_out, ACS::doubleSeq_out max_ranges_out) { SRTBaseMinorServoImpl::getAxesRanges(min_ranges_out, max_ranges_out); } + +/** + * MACRO definition of child classes properties methods. + * This simplifies the declaration since these properties' methods implementations are the same for both SRTGenericMinorServo and SRTProgramTrackMinorServo classes. + * This was necessary since these are pure virtual methods in POA_MinorServo::SRTGenericMinorServo and POA_MinorServo::SRTProgramTrackMinorServo, + * and even if they are inherited from the SRTBaseMinorServo class they are not seen by the compiler and they must be declared inside the respective classes. + * Take a look to the SRTBaseMinorServoImpl class for more information for each of these properties methods. + */ +#define PROPERTIES_DECLARATION \ + virtual Management::ROTBoolean_ptr enabled() { return SRTBaseMinorServoImpl::enabled(); }\ + virtual ROSRTMinorServoCabinetStatus_ptr drive_cabinet_status() { return SRTBaseMinorServoImpl::drive_cabinet_status(); }\ + virtual Management::ROTBoolean_ptr block() { return SRTBaseMinorServoImpl::block(); }\ + virtual ROSRTMinorServoOperativeMode_ptr operative_mode() { return SRTBaseMinorServoImpl::operative_mode(); }\ + virtual ACS::RObooleanSeq_ptr physical_axes_enabled() { return SRTBaseMinorServoImpl::physical_axes_enabled(); }\ + virtual ACS::ROdoubleSeq_ptr physical_positions() { return SRTBaseMinorServoImpl::physical_positions(); }\ + virtual ACS::ROlong_ptr virtual_axes() { return SRTBaseMinorServoImpl::virtual_axes(); }\ + virtual ACS::ROdoubleSeq_ptr plain_virtual_positions() { return SRTBaseMinorServoImpl::plain_virtual_positions(); }\ + virtual ACS::ROdoubleSeq_ptr virtual_positions() { return SRTBaseMinorServoImpl::virtual_positions(); }\ + virtual ACS::ROdoubleSeq_ptr virtual_offsets() { return SRTBaseMinorServoImpl::virtual_offsets(); }\ + virtual ACS::ROdoubleSeq_ptr virtual_user_offsets() { return SRTBaseMinorServoImpl::virtual_user_offsets(); }\ + virtual ACS::ROdoubleSeq_ptr virtual_system_offsets() { return SRTBaseMinorServoImpl::virtual_system_offsets(); }\ + virtual Management::ROTBoolean_ptr in_use() { return SRTBaseMinorServoImpl::in_use(); }\ + virtual ACS::ROstring_ptr current_setup() { return SRTBaseMinorServoImpl::current_setup(); } + +/** + * This class implements the SRTGenericMinorServoImpl CORBA interface for a generic SRTMinorServo component. + * It inherits from the SRTBaseMinorServoImpl class. + * A SRTGenericMinorServo component does not need to implement any tracking feature. + */ +class SRTGenericMinorServoImpl: public SRTBaseMinorServoImpl, public virtual POA_MinorServo::SRTGenericMinorServo +{ +public: + /** + * Constructor + * @param component_name component's name. This is also the name that will be used to find the configuration data for the component in the Configuration DataBase. + * @param container_services pointer to the class that exposes all services offered by container. + */ + SRTGenericMinorServoImpl(const ACE_CString &component_name, maci::ContainerServices *container_services); + + /** + * Destructor. + */ + ~SRTGenericMinorServoImpl(); + + /** + * Status method definition. It simply calls and returns the SRTBaseMinorServoImpl method. + */ + bool status() { return SRTBaseMinorServoImpl::status(); } + + /** + * Setup method definition. It simply calls the SRTBaseMinorServoImpl method. + */ + bool setup(const char* configuration_name = "") { return SRTBaseMinorServoImpl::setup(configuration_name); } + + /** + * Declaration of all the other inherited methods. + */ + METHODS_DECLARATION; + + /** + * Declaration of all the other inherited properties methods. + */ + PROPERTIES_DECLARATION; +}; + +/** + * This class implements the SRTProgramTrackMinorServoImpl CORBA interface for a tracking-capable SRTMinorServo component. + * It inherits from the SRTBaseMinorServoImpl class. + * A SRTProgramTrackMinorServo component is capable of commanding a trajectory to track to its related minor servo system. + */ +class SRTProgramTrackMinorServoImpl: public SRTBaseMinorServoImpl, public virtual POA_MinorServo::SRTProgramTrackMinorServo +{ +public: + /** + * Constructor. + * @param component_name component's name. This is also the name that will be used to find the configuration data for the component in the Configuration DataBase. + * @param container_services pointer to the class that exposes all services offered by container. + * @throw ComponentErrors::ComponentErrorsEx when there has been an issue reading some value from the CDB. + */ + SRTProgramTrackMinorServoImpl(const ACE_CString &component_name, maci::ContainerServices *container_services); + + /** + * Destructor. + */ + ~SRTProgramTrackMinorServoImpl(); + + /** + * Overloaded initialize method. It calls the SRTBaseMinorServoImpl initialize method and performs some other initialization routines. + * @throw ComponentErrors::ComponentErrorsEx when there has been a memory allocation issue with the properties pointers. + */ + void initialize(); + + /** + * Overloaded status method. It calls the SRTBaseMinorServoImpl status method and performs some other routines. + * @throw MinorServoErrors::MinorServoErrorsEx when trying to reset the offsets when they don't match the ones loaded into the hardware. + */ + bool status(); + + /** + * Overloaded setup method. It calls the SRTBaseMinorServoImpl status method and performs some other routines. + * @param configuration_name the configuration the servo system should assume. + * @throw ComponentErrors::ComponentErrorsEx when there is an error while trying to load the table for the given configuration. + */ + bool setup(const char* configuration_name = ""); + + /** + * Declaration of all the other inherited methods. + */ + METHODS_DECLARATION; + + /** + * Loads a set of tracking coordinates to the minor servo system. + * @param trajectory_id the ID of the trajectory. This defaults to UNIX Epoch * 1000 (milliseconds precision) of the starting time of the trajectory. + * @param point_id the ID of the point inside the current trajectory. + * @param point_time the timestamp associated with the given set of tracking coordinates. + * @coordinates the given set of tracking coordinates. + * @throw MinorServoErrors::MinorServoErrorsEx if the length of the coordinates sequence doesn't match the number of virtual axes of the servo, + * if the resulting position summing the offsets would go outside the accepted range of the servo, + * if there has been a communication error or if the command was not accepted. + */ + void programTrack(CORBA::Long trajectory_id, CORBA::Long point_id, ACS::Time point_time, const ACS::doubleSeq& coordinates); + + /** + * Returns the tracking status of the minor servo. + * @return true if the minor servo is tracking within the tracking error, false otherwise + */ + bool isTracking() { return m_tracking.load() == Management::MNG_TRUE; } + + /** + * Declaration of all the other inherited properties methods. + */ + PROPERTIES_DECLARATION; + + /** + * Returns a reference to the tracking property implementation of the IDL interface. + * @return pointer to the read-only boolean property tracking. + */ + virtual Management::ROTBoolean_ptr tracking(); + + /** + * Returns a reference to the trajectory_id property implementation of the IDL interface. + * @return pointer to the read-only long property trajectory_id. + */ + virtual ACS::ROlong_ptr trajectory_id(); + + /** + * Returns a reference to the total_trajectory_points property implementation of the IDL interface. + * @return pointer to the read-only long property total_trajectory_points. + */ + virtual ACS::ROlong_ptr total_trajectory_points(); + + /** + * Returns a reference to the remaining_trajectory_points property implementation of the IDL interface. + * @return pointer to the read-only long property remaining_trajectory_points. + */ + virtual ACS::ROlong_ptr remaining_trajectory_points(); + + /** + * Returns a reference to the tracking_error property implementation of the IDL interface. + * @return pointer to the read-only double sequence property tracking_error. + */ + virtual ACS::ROdoubleSeq_ptr tracking_error(); + +private: + /** + * Queue of positions to be assumed by the servo system when tracking a trajectory. + */ + SRTMinorServoPositionsQueue m_tracking_queue; + + /** + * Tracking delta values for all minor servo system virtual axes. + */ + const std::vector m_tracking_delta; + + /** + * Tracking error values for all minor servo system virtual axes. + */ + std::vector m_tracking_error; + + /** + * Indicates if the servo system is tracking or not. It is tracking when the position error is lower than the tracking delta for all the virtual axes. + */ + std::atomic m_tracking; + + /** + * Current trajectory ID. + */ + std::atomic m_trajectory_id; + + /** + * Total trajectory points of the current trajectory. + */ + std::atomic m_total_trajectory_points; + + /** + * Remaining trajectory points of the current trajectory. + */ + std::atomic m_remaining_trajectory_points; + + /** + * Pointer to the tracking property. + */ + baci::SmartPropertyPointer> m_tracking_ptr; + + /** + * Pointer to the trajectory_id property. + */ + baci::SmartPropertyPointer m_trajectory_id_ptr; + + /** + * Pointer to the total_trajectory_points property. + */ + baci::SmartPropertyPointer m_total_trajectory_points_ptr; + + /** + * Pointer to the remaining_trajectory_points property. + */ + baci::SmartPropertyPointer m_remaining_trajectory_points_ptr; + + /** + * Pointer to the tracking_error property. + */ + baci::SmartPropertyPointer m_tracking_error_ptr; +}; + +#endif diff --git a/SRT/Servers/SRTMinorServo/include/SRTMinorServoParkThread.h b/SRT/Servers/SRTMinorServo/include/SRTMinorServoParkThread.h new file mode 100644 index 0000000000000000000000000000000000000000..031e058a80dd22ca93a427b2800ac0ad68e5c5d7 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/include/SRTMinorServoParkThread.h @@ -0,0 +1,78 @@ +#ifndef _SRTMINORSERVOPARKTHREAD_H_ +#define _SRTMINORSERVOPARKTHREAD_H_ + +/** + * SRTMinorServoParkThread.h + * Giuseppe Carboni (giuseppe.carboni@inaf.it) + */ + +#include "SuppressWarnings.h" +#include +#include +#include "SRTMinorServoBossCore.h" + +// Same timeout as the one defined for the SETUP procedure +#define PARK_TIMEOUT 120 + +class SRTMinorServoBossCore; + + +/** + * This class implements a parking thread. This thread is in charge of checking the status of the minor servos parking procedure. + */ +class SRTMinorServoParkThread : public ACS::Thread +{ +public: + /** + * Constructor. + * @param name thread name + * @param core a reference to the SRTMinorServoBossCore object. + * @param response_time thread's heartbeat response time in 100ns unit. Default value is 1s. + * @param sleep_time thread's sleep time in 100ns unit. Default value is 100ms. + */ + SRTMinorServoParkThread(const ACE_CString& name, SRTMinorServoBossCore& core, const ACS::TimeInterval& response_time=ThreadBase::defaultResponseTime, const ACS::TimeInterval& sleep_time=ThreadBase::defaultSleepTime); + + /** + * Destructor. + */ + ~SRTMinorServoParkThread(); + + /** + * This method is executed once when the thread starts. + */ + virtual void onStart(); + + /** + * This method is executed once when the thread stops. + */ + virtual void onStop(); + + /** + * This method overrides the thread implementation class. + * The thread can be exited by calling ACS::ThreadBase::stop or ACS::ThreadBase::exit command. + */ + virtual void runLoop(); + + /** + * The name of this class of threads. Since a single instance of this thread class can only run it is ok to reuse the same name. + */ + static constexpr const char* c_thread_name = "SRTMinorServoParkThread"; + +private: + /** + * The reference to the SRTMinorServoBossCore object. + */ + SRTMinorServoBossCore& m_core; + + /** + * The current status of the finite-state machine. + */ + unsigned int m_status; + + /** + * The starting time of the park procedure. Used to check if we ran out of time. + */ + double m_start_time; +}; + +#endif /*_SRTMINORSERVOSETUPTHREAD_H_*/ diff --git a/SRT/Servers/SRTMinorServo/include/SRTMinorServoScanThread.h b/SRT/Servers/SRTMinorServo/include/SRTMinorServoScanThread.h new file mode 100644 index 0000000000000000000000000000000000000000..e5c26e67d2c7bf7aaf6ee5e648a9b713db9d9ad0 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/include/SRTMinorServoScanThread.h @@ -0,0 +1,96 @@ +#ifndef _SRTMINORSERVOSCANTHREAD_H_ +#define _SRTMINORSERVOSCANTHREAD_H_ + +/** + * SRTMinorServoScanThread.h + * Giuseppe Carboni (giuseppe.carboni@inaf.it) + */ + +#include "SuppressWarnings.h" +#include +#include +#include "SRTMinorServoBossCore.h" +#include "SRTMinorServoContainers.h" + + +class SRTMinorServoBossCore; + + +/** + * This class implements a scan thread. This thread is in charge of positioning the minor servos during a scan operation. + */ +class SRTMinorServoScanThread : public ACS::Thread +{ +public: + /** + * Constructor. + * @param name thread name + * @param response_time thread's heartbeat response time in 100ns unit. Default value is 1s. + * @param sleep_time thread's sleep time in 100ns unit. Default value is 100ms. + */ + SRTMinorServoScanThread(const ACE_CString& name, SRTMinorServoBossCore& core, const ACS::TimeInterval& response_time=ThreadBase::defaultResponseTime, const ACS::TimeInterval& sleep_time=ThreadBase::defaultSleepTime); + + /** + * Destructor. + */ + ~SRTMinorServoScanThread(); + + /** + * This method is executed once when the thread starts. + */ + virtual void onStart(); + + /** + * This method is executed once when the thread stops. + */ + virtual void onStop(); + + /** + * This method overrides the thread implementation class. + * The thread can be exited by calling ACS::ThreadBase::stop or ACS::ThreadBase::exit command. + */ + virtual void runLoop(); + + /** + * The name of this class of threads. Since a single instance of this thread class can only run it is ok to reuse the same name. + */ + static constexpr const char* c_thread_name = "SRTMinorServoScanThread"; + +private: + /** + * The reference to the SRTMinorServoBossCore object. + */ + SRTMinorServoBossCore& m_core; + + /** + * The current trajectory ID. + */ + unsigned int m_trajectory_id; + + /** + * The current trajectory point ID. + */ + unsigned int m_point_id; + + /** + * The current trajectory point time. + */ + ACS::Time m_point_time; + + /** + * Boolean which indicates if the scan thread exited with an error or not. + */ + bool m_error; + + /** + * The queue of offsets to be added to the original tracking coordinates. + */ + MinorServo::SRTMinorServoPositionsQueue m_scan_offsets; + + /** + * The coordinates to which the minor servo involved in the scan was positioned before starting the scan itself. + */ + ACS::doubleSeq m_starting_coordinates; +}; + +#endif /*_SRTMINORSERVOSCANTHREAD_H_*/ diff --git a/SRT/Servers/SRTMinorServo/include/SRTMinorServoSetupThread.h b/SRT/Servers/SRTMinorServo/include/SRTMinorServoSetupThread.h new file mode 100644 index 0000000000000000000000000000000000000000..2a1f4297ffa8eabec08cdd314a65e103511e4302 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/include/SRTMinorServoSetupThread.h @@ -0,0 +1,87 @@ +#ifndef _SRTMINORSERVOSETUPTHREAD_H_ +#define _SRTMINORSERVOSETUPTHREAD_H_ + +/** + * SRTMinorServoSetupThread.h + * Giuseppe Carboni (giuseppe.carboni@inaf.it) + */ + +#include "SuppressWarnings.h" +#include +#include +#include "SRTMinorServoBossCore.h" + +// GFR from -160 to 160 takes approximately 110 seconds +#define SETUP_TIMEOUT 120 + +class SRTMinorServoBossCore; + + +/** + * This class implements a setup thread. This thread is in charge of checking the status of the minor servos setup procedure. + */ +class SRTMinorServoSetupThread : public ACS::Thread +{ +public: + /** + * Constructor. + * @param name thread name + * @param response_time thread's heartbeat response time in 100ns unit. Default value is 1s. + * @param sleep_time thread's sleep time in 100ns unit. Default value is 100ms. + */ + SRTMinorServoSetupThread(const ACE_CString& name, SRTMinorServoBossCore& core, const ACS::TimeInterval& response_time=ThreadBase::defaultResponseTime, const ACS::TimeInterval& sleep_time=ThreadBase::defaultSleepTime); + + /** + * Destructor. + */ + ~SRTMinorServoSetupThread(); + + /** + * This method is executed once when the thread starts. + */ + virtual void onStart(); + + /** + * This method is executed once when the thread stops. + */ + virtual void onStop(); + + /** + * This method overrides the thread implementation class. + * The thread can be exited by calling ACS::ThreadBase::stop or ACS::ThreadBase::exit command. + */ + virtual void runLoop(); + + /** + * The name of this class of threads. Since a single instance of this thread class can only run it is ok to reuse the same name. + */ + static constexpr const char* c_thread_name = "SRTMinorServoSetupThread"; + +private: + /** + * The reference to the SRTMinorServoBossCore object. + */ + SRTMinorServoBossCore& m_core; + + /** + * The current status of the finite-state machine. + */ + unsigned int m_status; + + /** + * The starting time of the park procedure. Used to check if we ran out of time. + */ + double m_start_time; + + /** + * The requested Leonardo minor servo focal configuration. + */ + std::string m_LDO_configuration; + + /** + * The gregorian cover position to be reached for the requested focal configuration. + */ + MinorServo::SRTMinorServoGregorianCoverStatus m_gregorian_cover_position; +}; + +#endif /*_SRTMINORSERVOSETUPTHREAD_H_*/ diff --git a/SRT/Servers/SRTMinorServo/include/SRTMinorServoStatusThread.h b/SRT/Servers/SRTMinorServo/include/SRTMinorServoStatusThread.h new file mode 100644 index 0000000000000000000000000000000000000000..fbe34f5ad8b6a953befddf09e34d95fc493f26ff --- /dev/null +++ b/SRT/Servers/SRTMinorServo/include/SRTMinorServoStatusThread.h @@ -0,0 +1,87 @@ +#ifndef _SRTMINORSERVOSTATUSTHREAD_H_ +#define _SRTMINORSERVOSTATUSTHREAD_H_ + +/** + * SRTMinorServoStatusThread.h + * Giuseppe Carboni (giuseppe.carboni@inaf.it) + */ + +#include "SuppressWarnings.h" +#include +#include +#include "SRTMinorServoSocket.h" +#include "SRTMinorServoBossCore.h" +#include + +class SRTMinorServoBossCore; + + +/** + * This class implements a status thread. This thread is in charge of updating the status of the minor servo components. + */ +class SRTMinorServoStatusThread : public ACS::Thread +{ +public: + /** + * Constructor. + * @param name thread name + * @param response_time thread's heartbeat response time in 100ns unit. Default value is 1s. + * @param sleep_time thread's sleep time in 100ns unit. Default value is 100ms. + */ + SRTMinorServoStatusThread(const ACE_CString& name, SRTMinorServoBossCore& core, const ACS::TimeInterval& response_time=ThreadBase::defaultResponseTime, const ACS::TimeInterval& sleep_time=ThreadBase::defaultSleepTime); + + /** + * Destructor. + */ + ~SRTMinorServoStatusThread(); + + /** + * This method is executed once when the thread starts. + */ + virtual void onStart(); + + /** + * This method is executed once when the thread stops. + */ + virtual void onStop(); + + /** + * This method overrides the thread implementation class. + * The thread can be exited by calling ACS::ThreadBase::stop or ACS::ThreadBase::exit command. + */ + virtual void runLoop(); + + /** + * The name of this class of threads. Since a single instance of this thread class can only run it is ok to reuse the same name. + */ + static constexpr const char* c_thread_name = "SRTMinorServoStatusThread"; + +private: + /** + * Method that publishes the status onto the ACS notification channel. + */ + void publish(); + + /** + * The reference to the SRTMinorServoBossCore object. + */ + SRTMinorServoBossCore& m_core; + + /** + * The current status of the finite-state machine. + */ + unsigned int m_status; + + /** + * The sleeping time of the thread. + * The thread should be cycling at a constant rate, therefore the inner sleeping time is always updated taking into account this and the thread execution time. + */ + ACS::TimeInterval m_sleep_time; + + /** + * The ACS notification channel simple supplier object. + */ + nc::SimpleSupplier* m_notification_channel; +}; + +#endif /*_SRTMINORSERVOSTATUSTHREAD_H_*/ diff --git a/SRT/Servers/SRTMinorServo/include/SRTMinorServoTrackingThread.h b/SRT/Servers/SRTMinorServo/include/SRTMinorServoTrackingThread.h new file mode 100644 index 0000000000000000000000000000000000000000..32ae15c9e3bc44d4757d0330e7e0c8cd4158ece5 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/include/SRTMinorServoTrackingThread.h @@ -0,0 +1,85 @@ +#ifndef _SRTMINORSERVOTRACKINGTHREAD_H_ +#define _SRTMINORSERVOTRACKINGTHREAD_H_ + +/** + * SRTMinorServoTrackingThread.h + * Giuseppe Carboni (giuseppe.carboni@inaf.it) + */ + +#include "SuppressWarnings.h" +#include +#include +#include "SRTMinorServoBossCore.h" + + +class SRTMinorServoBossCore; + + +/** + * This class implements a tracking thread. This thread is in charge of positioning the minor servos in time. + */ +class SRTMinorServoTrackingThread : public ACS::Thread +{ +public: + /** + * Constructor. + * @param name thread name + * @param response_time thread's heartbeat response time in 100ns unit. Default value is 1s. + * @param sleep_time thread's sleep time in 100ns unit. Default value is 100ms. + */ + SRTMinorServoTrackingThread(const ACE_CString& name, SRTMinorServoBossCore& core, const ACS::TimeInterval& response_time=ThreadBase::defaultResponseTime, const ACS::TimeInterval& sleep_time=ThreadBase::defaultSleepTime); + + /** + * Destructor. + */ + ~SRTMinorServoTrackingThread(); + + /** + * This method is executed once when the thread starts. + */ + virtual void onStart(); + + /** + * This method is executed once when the thread stops. + */ + virtual void onStop(); + + /** + * This method overrides the thread implementation class. + * The thread can be exited by calling ACS::ThreadBase::stop or ACS::ThreadBase::exit command. + */ + virtual void runLoop(); + + /** + * The name of this class of threads. Since a single instance of this thread class can only run it is ok to reuse the same name. + */ + static constexpr const char* c_thread_name = "SRTMinorServoTrackingThread"; + +private: + /** + * The reference to the SRTMinorServoBossCore object. + */ + SRTMinorServoBossCore& m_core; + + /** + * The current trajectory ID. + */ + unsigned int m_trajectory_id; + + /** + * The current trajectory point ID. + */ + unsigned int m_point_id; + + /** + * The current trajectory point time. + */ + ACS::Time m_point_time; + + /** + * Boolean which indicates if the tracking thread exited with an error or not. + */ + bool m_error; +}; + +#endif /*_SRTMINORSERVOTRACKINGTHREAD_H_*/ diff --git a/SRT/Servers/SRTMinorServo/include/SuppressWarnings.h b/SRT/Servers/SRTMinorServo/include/SuppressWarnings.h new file mode 100644 index 0000000000000000000000000000000000000000..0dc0dd252623770b5bc284cf6f8216e3f467a26a --- /dev/null +++ b/SRT/Servers/SRTMinorServo/include/SuppressWarnings.h @@ -0,0 +1,13 @@ +#ifndef __SUPPRESSWARNINGS_H__ +#define __SUPPRESSWARNINGS_H__ + +/** + * SuppressWarnings.h + * Giuseppe Carboni (giuseppe.carboni@inaf.it) + */ + +#include +C11_IGNORE_WARNING_PUSH +C11_IGNORE_WARNING("-Wsequence-point") +C11_IGNORE_WARNING("-Wdeprecated-declarations") +#endif diff --git a/SRT/Servers/SRTMinorServo/src/Makefile b/SRT/Servers/SRTMinorServo/src/Makefile index 6c47bef427c984badc375e47f541d6ed1cb212d2..c11de16457adbe6a4dd2507bce1217c729f0725f 100644 --- a/SRT/Servers/SRTMinorServo/src/Makefile +++ b/SRT/Servers/SRTMinorServo/src/Makefile @@ -1,35 +1,46 @@ -#***************************************** -#----------------------------------------- -# Marco Buttu -#----------------------------------------- -#***************************************** +#******************************************** +#-------------------------------------------- +# Giuseppe Carboni +#-------------------------------------------- +#******************************************** +# +# C programs (public and local) +# ----------------------------- +EXECUTABLES = +EXECUTABLES_L = -PY_PACKAGES = -PY_SCRIPTS = +PY_SCRIPTS = #_cover # On-Line Database Files # ---------------------- -CDB_SCHEMAS = MinorServo +CDB_SCHEMAS = SRTMinorServoBoss SRTMinorServo SRTMinorServoSocketConfiguration SRTMinorServoProperties SRTMinorServoLookupTable # ---------------------------- # Libraries (public and local) # ---------------------------- -LIBRARIES = WPServoImpl MinorServoBossImpl - -WPServoImpl_OBJECTS = WPServoImpl WPServoSocket WPServoTalker RequestDispatcher \ - WPStatusUpdater SocketListener utils libCom WPUtils -WPServoImpl_LIBS = MinorServoStubs IRALibrary ComponentErrors \ - SRTMinorServoLibrary MinorServoErrors - -MinorServoBossImpl_OBJECTS = MinorServoBossImpl utils SetupThread ParkThread \ - TrackingThread ScanThread MSBossPublisher libCom \ - MSBossConfiguration -MinorServoBossImpl_LIBS = MinorServoStubs MinorServoBossStubs SRTMinorServoLibrary \ - ManagmentDefinitionsStubs ManagementErrors \ - IRALibrary ComponentErrors MinorServoErrors acsnc \ - ParserErrors DiscosVersion AntennaDefinitionsStubs MountStubs \ - AntennaBossStubs AntennaErrors ActiveSurfaceBossStubs \ - SRTActiveSurfaceBossStubs MinorServoDefinitionsStubs +LIBRARIES = SRTGenericMinorServoImpl SRTProgramTrackMinorServoImpl SRTMinorServoBossImpl + +USER_CFLAGS = + +SRTMinorServoBossImpl_OBJECTS = SRTMinorServoBossCore SRTMinorServoSetupThread SRTMinorServoParkThread SRTMinorServoTrackingThread SRTMinorServoScanThread SRTMinorServoBossImpl SRTMinorServoStatusThread +SRTMinorServoBossImpl_LIBS = IRALibrary SRTMinorServoBossStubs SRTMinorServoStubs ComponentErrors MinorServoErrors ManagementErrors MinorServoDefinitionsStubs SRTMinorServoCommandLibrary AntennaBossStubs SRTMinorServoSocketLibrary ParserErrors DiscosVersion acsnc +SRTMinorServoBossImpl_CFLAGS = -std=c++17 -fconcepts +SRTMinorServoBossCore_CFLAGS = -std=c++17 -fconcepts +SRTMinorServoStatusThread_CFLAGS = -std=c++17 -fconcepts +SRTMinorServoSetupThread_CFLAGS = -std=c++17 -fconcepts +SRTMinorServoParkThread_CFLAGS = -std=c++17 -fconcepts +SRTMinorServoTrackingThread_CFLAGS = -std=c++17 -fconcepts +SRTMinorServoScanThread_CFLAGS = -std=c++17 -fconcepts + +SRTBaseMinorServoImpl_CFLAGS = -std=c++17 -fconcepts +SRTGenericMinorServoImpl_OBJECTS = SRTGenericMinorServoImpl SRTBaseMinorServoImpl +SRTGenericMinorServoImpl_LIBS = IRALibrary SRTMinorServoStubs MinorServoErrors MinorServoDefinitionsStubs SRTMinorServoCommandLibrary SRTMinorServoSocketLibrary DiscosVersion +SRTGenericMinorServoImpl_CFLAGS = -std=c++17 -fconcepts +SRTProgramTrackMinorServoImpl_OBJECTS = SRTProgramTrackMinorServoImpl SRTBaseMinorServoImpl +SRTProgramTrackMinorServoImpl_LIBS = IRALibrary SRTMinorServoStubs MinorServoErrors MinorServoDefinitionsStubs SRTMinorServoCommandLibrary SRTMinorServoSocketLibrary DiscosVersion +SRTProgramTrackMinorServoImpl_CFLAGS = -std=c++17 -fconcepts + + # ---------------------------------------------------------------------- # List of all possible C-sources (used to create automatic dependencies) # ---------------------------------------------------------------------- @@ -54,19 +65,12 @@ all: do_all @echo " . . . 'all' done" clean : clean_all - $(RM) *~ ../include/*~ ../idl/*~ ../*~ ../../*~ core - $(RM) ../doc/html - $(RM) tmp.txt acsexmplbeans.jar ../doc/abeans.log - $(RM) ../lib/python/site-packages/* - $(RM) $(INTROOT)/lib/python/site-packages/SRTMinorServoTest - $(RM) $(INTROOT)/bin/mscu-runserver @echo " . . . clean done" clean_dist : clean clean_dist_all @echo " . . . clean_dist done" man : do_man - # cp ../doc/html/group__ACSEXMPLDOC.html ../doc/html/main.html @echo " . . . man page(s) done" install : install_all diff --git a/SRT/Servers/SRTMinorServo/src/SRTBaseMinorServoImpl.cpp b/SRT/Servers/SRTMinorServo/src/SRTBaseMinorServoImpl.cpp new file mode 100644 index 0000000000000000000000000000000000000000..df2e50b72f7e14f113ab71271326250e89b0ca16 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/src/SRTBaseMinorServoImpl.cpp @@ -0,0 +1,811 @@ +#include "SRTMinorServoImpl.h" + +using namespace MinorServo; + +SRTBaseMinorServoImpl::SRTBaseMinorServoImpl(const ACE_CString& component_name, maci::ContainerServices* container_services): + CharacteristicComponentImpl(component_name, container_services), + m_component_name(std::string(component_name.c_str())), + m_servo_name(std::string(strchr(component_name.c_str(), '/') + 1)), + m_virtual_axes(getCDBValue(container_services, "virtual_axes")), + m_physical_axes(getCDBValue(container_services, "physical_axes")), + m_virtual_axes_names(SRTBaseMinorServoImpl::getPropertiesTable(*this, "virtual_positions")), + m_virtual_axes_units(SRTBaseMinorServoImpl::getPropertiesTable(*this, "virtual_axes_units")), + m_status( + m_servo_name, + SRTBaseMinorServoImpl::getPropertiesTable(*this, "physical_axes_enabled"), + SRTBaseMinorServoImpl::getPropertiesTable(*this, "physical_positions"), + m_virtual_axes_names, + SRTBaseMinorServoImpl::getPropertiesTable(*this, "virtual_offsets") + ), + m_user_offsets(m_virtual_axes, 0.0), + m_system_offsets(m_virtual_axes, 0.0), + m_positions_queue(5 * 60 * int(1 / getCDBValue(container_services, "status_thread_period", "/MINORSERVO/Boss")), m_virtual_axes), + m_min(SRTBaseMinorServoImpl::getMotionConstant(*this, "min_range")), + m_max(SRTBaseMinorServoImpl::getMotionConstant(*this, "max_range")), + m_m_s(SRTBaseMinorServoImpl::getMotionConstant(*this, "max_speed")), + m_a(SRTBaseMinorServoImpl::getMotionConstant(*this, "acceleration")), + m_r_t(SRTBaseMinorServoImpl::getMotionConstant(*this, "ramp_times")), + m_r_d(SRTBaseMinorServoImpl::getMotionConstant(*this, "ramp_distances")), + m_c_s(m_virtual_axes, 0.0), + m_in_use(Management::MNG_FALSE), + m_current_setup(""), + m_enabled_ptr(this), + m_drive_cabinet_status_ptr(this), + m_block_ptr(this), + m_operative_mode_ptr(this), + m_physical_axes_enabled_ptr(this), + m_physical_positions_ptr(this), + m_virtual_axes_ptr(this), + m_plain_virtual_positions_ptr(this), + m_virtual_positions_ptr(this), + m_virtual_offsets_ptr(this), + m_virtual_user_offsets_ptr(this), + m_virtual_system_offsets_ptr(this), + m_in_use_ptr(this), + m_current_setup_ptr(this), + m_current_lookup_table(), + m_socket_configuration(SRTMinorServoSocketConfiguration::getInstance(container_services)), + m_socket(SRTMinorServoSocket::getInstance(m_socket_configuration.m_ip_address, m_socket_configuration.m_port, m_socket_configuration.m_timeout)) +{ + AUTO_TRACE(m_servo_name + "::SRTBaseMinorServoImpl()"); +} + +SRTBaseMinorServoImpl::~SRTBaseMinorServoImpl() +{ + AUTO_TRACE(m_servo_name + "::~SRTBaseMinorServoImpl()"); +} + +void SRTBaseMinorServoImpl::initialize() +{ + AUTO_TRACE(m_servo_name + "::initialize()"); + + try + { + m_enabled_ptr = new ROEnumImpl((m_component_name + ":enabled").c_str(), getComponent(), + new MSAnswerMapDevIO("enabled", m_status, &SRTMinorServoStatus::isEnabled), true); + m_drive_cabinet_status_ptr = new ROEnumImpl((m_component_name + ":drive_cabinet_status").c_str(), getComponent(), + new MSAnswerMapDevIO("drive_cabinet_status", m_status, &SRTMinorServoStatus::getDriveCabinetStatus), true); + m_block_ptr = new ROEnumImpl((m_component_name + ":block").c_str(), getComponent(), + new MSAnswerMapDevIO("block", m_status, &SRTMinorServoStatus::isBlocked), true); + m_operative_mode_ptr = new ROEnumImpl((m_component_name + ":operative_mode").c_str(), getComponent(), + new MSAnswerMapDevIO("operative_mode", m_status, &SRTMinorServoStatus::getOperativeMode), true); + m_physical_axes_enabled_ptr = new baci::RObooleanSeq((m_component_name + ":physical_axes_enabled").c_str(), getComponent(), + new MSAnswerMapDevIO("physical_axes_enabled", m_status, &SRTMinorServoStatus::getPhysicalAxesEnabled), true); + m_physical_positions_ptr = new baci::ROdoubleSeq((m_component_name + ":physical_positions").c_str(), getComponent(), + new MSAnswerMapDevIO("physical_positions", m_status, &SRTMinorServoStatus::getPhysicalPositions), true); + m_virtual_axes_ptr = new baci::ROlong((m_component_name + ":virtual_axes").c_str(), getComponent(), + new MSGenericDevIO(m_virtual_axes), true); + m_plain_virtual_positions_ptr = new baci::ROdoubleSeq((m_component_name + ":plain_virtual_positions").c_str(), getComponent(), + new MSAnswerMapDevIO("plain_virtual_positions", m_status, &SRTMinorServoStatus::getPlainVirtualPositions), true); + m_virtual_positions_ptr = new baci::ROdoubleSeq((m_component_name + ":virtual_positions").c_str(), getComponent(), + new MSAnswerMapDevIO("virtual_positions", m_status, &SRTMinorServoStatus::getVirtualPositions), true); + m_virtual_offsets_ptr = new baci::ROdoubleSeq((m_component_name + ":virtual_offsets").c_str(), getComponent(), + new MSAnswerMapDevIO("virtual_offsets", m_status, &SRTMinorServoStatus::getVirtualOffsets), true); + m_virtual_user_offsets_ptr = new baci::ROdoubleSeq((m_component_name + ":virtual_user_offsets").c_str(), getComponent(), + new MSGenericDevIO>(m_user_offsets), true); + m_virtual_system_offsets_ptr = new baci::ROdoubleSeq((m_component_name + ":virtual_system_offsets").c_str(), getComponent(), + new MSGenericDevIO>(m_system_offsets), true); + m_in_use_ptr = new ROEnumImpl((m_component_name + ":in_use").c_str(), getComponent(), + new MSGenericDevIO>(m_in_use), true); + m_current_setup_ptr = new baci::ROstring((m_component_name + ":current_setup").c_str(), getComponent(), + new MSGenericDevIO(m_current_setup), true); + } + catch(std::bad_alloc& ba) + { + _EXCPT(ComponentErrors::MemoryAllocationExImpl, ex, (m_servo_name + "::initialize()").c_str()); + ex.log(LM_DEBUG); + throw ex.getComponentErrorsEx(); + } + + // Try to read the current status of the servo + try + { + status(); + } + catch(...) + { + // This block is necessary since the socket might not be connected yet. If the Leonardo system is not reachable the status(); call will fail, but we want to instantiate the component anyway + // A non connected socket will try to connect every time a new command is sent, therefore the status thread will establish the connection as soon as possible. + } +} + +void SRTBaseMinorServoImpl::execute() +{ + AUTO_TRACE(m_servo_name + "::execute()"); +} + +void SRTBaseMinorServoImpl::cleanUp() +{ + AUTO_TRACE(m_servo_name + "::cleanUp()"); +} + +void SRTBaseMinorServoImpl::aboutToAbort() +{ + AUTO_TRACE(m_servo_name + "::aboutToAbort()"); +} + +/////////////////// PUBLIC methods +bool SRTBaseMinorServoImpl::status() +{ + AUTO_TRACE(m_servo_name + "::status()"); + + // We don't check if the socket is connected here since a status command will try to reconnect it automatically + try + { + m_socket.sendCommand(SRTMinorServoCommandLibrary::status(m_servo_name), m_status); + + ACS::doubleSeq current_point = m_status.getVirtualPositions(); + + // Calculate the current speed of the axes + try + { + std::pair> previous_point = m_positions_queue.get(m_status.getTimestamp()); + for(size_t i = 0; i < m_virtual_axes; i++) + { + m_c_s[i] = (current_point[i] - previous_point.second[i]) * ((double(m_status.getTimestamp() - previous_point.first)) / 10000000); + } + } + catch(...) + { + // Empty queue, first reading, skip the speed calculation + } + + m_positions_queue.put(m_status.getTimestamp(), current_point); + } + catch(...) + { + return false; + } + + return true; +} + +void SRTBaseMinorServoImpl::stow(CORBA::Long stow_position) +{ + AUTO_TRACE(m_servo_name + "::stow()"); + + checkLineStatus(); + + if(!m_socket.sendCommand(SRTMinorServoCommandLibrary::stow(m_servo_name, (unsigned int)stow_position)).checkOutput()) + { + _EXCPT(MinorServoErrors::CommunicationErrorExImpl, ex, (m_servo_name + "::stow()").c_str()); + ex.setReason("Received NAK in response to a STOW command."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } +} + +void SRTBaseMinorServoImpl::stop() +{ + AUTO_TRACE(m_servo_name + "::stop()"); + + checkLineStatus(); + + if(!m_socket.sendCommand(SRTMinorServoCommandLibrary::stop(m_servo_name)).checkOutput()) + { + _EXCPT(MinorServoErrors::CommunicationErrorExImpl, ex, (m_servo_name + "::stop()").c_str()); + ex.setReason("Received NAK in response to a STOP command."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } +} + +void SRTBaseMinorServoImpl::preset(const ACS::doubleSeq& virtual_coordinates) +{ + AUTO_TRACE(m_servo_name + "::preset()"); + + checkLineStatus(); + + if(virtual_coordinates.length() != m_virtual_axes) + { + _EXCPT(MinorServoErrors::PositioningErrorExImpl, ex, (m_servo_name + "::preset()").c_str()); + ex.addData("Reason", "Wrong number of values for this servo system."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + std::vector coordinates(virtual_coordinates.get_buffer(), virtual_coordinates.get_buffer() + virtual_coordinates.length()); + ACS::doubleSeq offsets = m_status.getVirtualOffsets(); + + for(size_t i = 0; i < m_virtual_axes; i++) + { + double coordinate = coordinates[i] + offsets[i]; + if(coordinate < m_min[i] || coordinate > m_max[i]) + { + _EXCPT(MinorServoErrors::PositioningErrorExImpl, ex, (m_servo_name + "::preset()").c_str()); + ex.addData("Reason", "Resulting position out of range, check the offsets."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + } + + if(!m_socket.sendCommand(SRTMinorServoCommandLibrary::preset(m_servo_name, coordinates)).checkOutput()) + { + _EXCPT(MinorServoErrors::CommunicationErrorExImpl, ex, (m_servo_name + "::preset()").c_str()); + ex.setReason("Received NAK in response to a PRESET command."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } +} + +bool SRTBaseMinorServoImpl::setup(const char* configuration_name) +{ + AUTO_TRACE(m_servo_name + "::setup()"); + m_in_use.store(Management::MNG_FALSE); + m_current_lookup_table.clear(); + + m_current_setup = ""; + std::string setup_name(configuration_name); + std::transform(setup_name.begin(), setup_name.end(), setup_name.begin(), ::toupper); + + if(setup_name.empty()) + { + return false; + } + + IRA::CDBTable table(getContainerServices(), setup_name.c_str(), std::string("DataBlock/MinorServo/" + m_servo_name).c_str()); + IRA::CError error; + error.Reset(); + + if(!table.addField(error, "axis", IRA::CDataField::STRING)) + { + error.setExtra("Error adding field axis", 0); + } + if(!table.addField(error, "coefficients", IRA::CDataField::STRING)) + { + error.setExtra("Error adding field coefficients", 0); + } + if(!error.isNoError()) + { + _EXCPT_FROM_ERROR(ComponentErrors::IRALibraryResourceExImpl, ex, error); + ex.setCode(error.getErrorCode()); + ex.setDescription((const char *)error.getDescription()); + ex.log(LM_DEBUG); + throw ex.getComponentErrorsEx(); + } + if(!table.openTable(error)) + { + _EXCPT_FROM_ERROR(ComponentErrors::CDBAccessExImpl, ex, error); + ex.log(LM_DEBUG); + throw ex.getComponentErrorsEx(); + } + + table.First(); + for(unsigned int i = 0; i < table.recordCount(); i++, table.Next()) + { + std::string axis = std::string(table["axis"]->asString()); + + std::vector coefficients; + + std::string coefficient_str; + std::stringstream stream(std::string(table["coefficients"]->asString())); + + while(std::getline(stream, coefficient_str, ',')) + { + coefficients.push_back(std::stod(std::regex_replace(coefficient_str, std::regex("\\s+"), ""))); + } + + m_current_lookup_table[axis] = coefficients; + } + table.closeTable(); + + if(m_current_lookup_table.size() > 0) + { + m_current_setup = setup_name; + clearUserOffsets(); + clearSystemOffsets(); + m_in_use.store(Management::MNG_TRUE); + } + + return true; +} + +ACS::doubleSeq* SRTBaseMinorServoImpl::calcCoordinates(double elevation) +{ + AUTO_TRACE(m_servo_name + "::calcCoordinates()"); + + if(m_in_use.load() == Management::MNG_TRUE) + { + ACS::doubleSeq_var coordinates = new ACS::doubleSeq; + coordinates->length(m_virtual_axes); + + for(size_t axis = 0; axis < m_virtual_axes; axis++) + { + std::vector coefficients = m_current_lookup_table.at(m_virtual_axes_names[axis]); + + double coordinate = 0; + + for(size_t index = 0; index < coefficients.size(); index++) + { + coordinate += coefficients[index] * pow(elevation, index); + } + + coordinates[axis] = coordinate; + } + + return coordinates._retn(); + } + else + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, (m_servo_name + "::calcCoordinates()").c_str()); + ex.setReason("Unable to calculate the coordinates since the servo system has not been configured yet."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } +} + +ACS::doubleSeq* SRTBaseMinorServoImpl::getUserOffsets() +{ + AUTO_TRACE(m_servo_name + "::getUserOffsets()"); + + ACS::doubleSeq_var offsets = new ACS::doubleSeq; + offsets->length(m_user_offsets.size()); + std::copy(m_user_offsets.begin(), m_user_offsets.end(), offsets->begin()); + return offsets._retn(); +} + +void SRTBaseMinorServoImpl::setUserOffset(const char* axis_name, CORBA::Double offset) +{ + AUTO_TRACE(m_servo_name + "::setUserOffset()"); + + checkLineStatus(); + + std::string axis(axis_name); + std::transform(axis.begin(), axis.end(), axis.begin(), ::toupper); + + if(axis == "RZ" && m_virtual_axes == 1) + { + axis = "ROTATION"; + } + + unsigned int index = std::distance(m_virtual_axes_names.begin(), std::find(m_virtual_axes_names.begin(), m_virtual_axes_names.end(), axis)); + + if(index == m_virtual_axes) + { + _EXCPT(MinorServoErrors::OffsetErrorExImpl, ex, (m_servo_name + "::setUserOffset()").c_str()); + ex.addData("Reason", ("Unknown axis '" + axis + "'.").c_str()); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + // Calculate the new offsets + std::vector offsets(m_virtual_axes, 0); + std::transform(m_user_offsets.begin(), m_user_offsets.end(), m_system_offsets.begin(), offsets.begin(), std::plus()); + offsets[index] = m_system_offsets[index] + offset; + + if(offsets[index] < m_min[index] || offsets[index] > m_max[index]) + { + _EXCPT(MinorServoErrors::OffsetErrorExImpl, ex, (m_servo_name + "::setUserOffset()").c_str()); + ex.addData("Reason", "Sum of user and system offsets out of range."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + if(!m_socket.sendCommand(SRTMinorServoCommandLibrary::offset(m_servo_name, offsets)).checkOutput()) + { + _EXCPT(MinorServoErrors::CommunicationErrorExImpl, ex, (m_servo_name + "::setUserOffset()").c_str()); + ex.setReason("Received NAK in response to an OFFSET command."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + m_user_offsets[index] = offset; +} + +void SRTBaseMinorServoImpl::clearUserOffsets() +{ + AUTO_TRACE(m_servo_name + "::clearUserOffsets()"); + + checkLineStatus(); + + m_user_offsets = std::vector(m_virtual_axes, 0.0); + + std::vector offsets(m_virtual_axes, 0); + std::transform(m_user_offsets.begin(), m_user_offsets.end(), m_system_offsets.begin(), offsets.begin(), std::plus()); + + if(!m_socket.sendCommand(SRTMinorServoCommandLibrary::offset(m_servo_name, offsets)).checkOutput()) + { + _EXCPT(MinorServoErrors::CommunicationErrorExImpl, ex, (m_servo_name + "::clearUserOffset()").c_str()); + ex.setReason("Received NAK in response to an OFFSET command."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } +} + +ACS::doubleSeq* SRTBaseMinorServoImpl::getSystemOffsets() +{ + AUTO_TRACE(m_servo_name + "::getSystemOffsets()"); + + ACS::doubleSeq_var offsets = new ACS::doubleSeq; + offsets->length(m_system_offsets.size()); + std::copy(m_system_offsets.begin(), m_system_offsets.end(), offsets->begin()); + return offsets._retn(); +} + +void SRTBaseMinorServoImpl::setSystemOffset(const char* axis_name, CORBA::Double offset) +{ + AUTO_TRACE(m_servo_name + "::setSystemOffset()"); + + checkLineStatus(); + + std::string axis(axis_name); + std::transform(axis.begin(), axis.end(), axis.begin(), ::toupper); + + if(axis == "RZ" && m_virtual_axes == 1) + { + axis = "ROTATION"; + } + + unsigned int index = std::distance(m_virtual_axes_names.begin(), std::find(m_virtual_axes_names.begin(), m_virtual_axes_names.end(), axis)); + + if(index == m_virtual_axes) + { + _EXCPT(MinorServoErrors::OffsetErrorExImpl, ex, (m_servo_name + "::setSystemOffset()").c_str()); + ex.addData("Reason", ("Unknown axis '" + axis + "'.").c_str()); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + // Calculate the new offsets + std::vector offsets(m_virtual_axes, 0); + std::transform(m_user_offsets.begin(), m_user_offsets.end(), m_system_offsets.begin(), offsets.begin(), std::plus()); + offsets[index] = m_user_offsets[index] + offset; + + if(offsets[index] < m_min[index] || offsets[index] > m_max[index]) + { + _EXCPT(MinorServoErrors::OffsetErrorExImpl, ex, (m_servo_name + "::setUserOffset()").c_str()); + ex.addData("Reason", "Sum of user and system offsets out of range."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + if(!m_socket.sendCommand(SRTMinorServoCommandLibrary::offset(m_servo_name, offsets)).checkOutput()) + { + _EXCPT(MinorServoErrors::CommunicationErrorExImpl, ex, (m_servo_name + "::setSystemOffset()").c_str()); + ex.setReason("Received NAK in response to an OFFSET command."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + m_system_offsets[index] = offset; +} + +void SRTBaseMinorServoImpl::clearSystemOffsets() +{ + AUTO_TRACE(m_servo_name + "::clearSystemOffsets()"); + + checkLineStatus(); + + m_system_offsets = std::vector(m_virtual_axes, 0.0); + + // Update the offsets + std::vector offsets(m_virtual_axes, 0); + std::transform(m_user_offsets.begin(), m_user_offsets.end(), m_system_offsets.begin(), offsets.begin(), std::plus()); + + if(!m_socket.sendCommand(SRTMinorServoCommandLibrary::offset(m_servo_name, offsets)).checkOutput()) + { + _EXCPT(MinorServoErrors::CommunicationErrorExImpl, ex, (m_servo_name + "::clearSystemOffset()").c_str()); + ex.setReason("Received NAK in response to an OFFSET command."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } +} + +void SRTBaseMinorServoImpl::reloadOffsets() +{ + AUTO_TRACE(m_servo_name + "::reloadOffsets()"); + + // Sum the user and system DISCOS offsets to check whether they correspond to the Leonardo offsets + std::vector DISCOS_offsets(m_virtual_axes, 0.0); + std::transform(m_user_offsets.begin(), m_user_offsets.end(), m_system_offsets.begin(), DISCOS_offsets.begin(), std::plus()); + + // Read the Leonardo offsets + ACS::doubleSeq sequence = m_status.getVirtualOffsets(); + std::vector LEONARDO_offsets(sequence.get_buffer(), sequence.get_buffer() + sequence.length()); + + // Check if the offsets correspond or not + if(!std::equal(DISCOS_offsets.begin(), DISCOS_offsets.end(), LEONARDO_offsets.begin())) + { + // Offsets do not correspond, should reset them by sending a offset command + if(!m_socket.sendCommand(SRTMinorServoCommandLibrary::offset(m_servo_name, DISCOS_offsets)).checkOutput()) + { + _EXCPT(MinorServoErrors::CommunicationErrorExImpl, ex, (m_servo_name + "::reloadOffsets()").c_str()); + ex.setReason("Received NAK in response to an OFFSET command."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + ACS_LOG(LM_FULL_INFO, m_servo_name + "::reloadOffsets()", (LM_NOTICE, "Offsets discrepancy, reloading them")); + } +} + +void SRTBaseMinorServoImpl::getAxesInfo(ACS::stringSeq_out axes_names_out, ACS::stringSeq_out axes_units_out) +{ + AUTO_TRACE("SRTBaseMinorServoImpl::getAxesInfo()"); + + ACS::stringSeq_var axes_names = new ACS::stringSeq; + ACS::stringSeq_var axes_units = new ACS::stringSeq; + axes_names->length(m_virtual_axes); + axes_units->length(m_virtual_axes); + + for(size_t i = 0; i < m_virtual_axes; i++) + { + axes_names[i] = (m_servo_name + "_" + m_virtual_axes_names[i] == "ROTATION" ? "RZ" : m_virtual_axes_names[i]).c_str(); + axes_units[i] = m_virtual_axes_units[i].c_str(); + } + + axes_names_out = axes_names._retn(); + axes_units_out = axes_units._retn(); +} + +ACS::doubleSeq* SRTBaseMinorServoImpl::getAxesPositions(ACS::Time acs_time) +{ + AUTO_TRACE("SRTBaseMinorServoImpl::getAxesPositions()"); + + // Get the latest position + if(acs_time == 0) + { + acs_time = getTimeStamp(); + } + + try + { + std::vector p = m_positions_queue.get(acs_time).second; + + ACS::doubleSeq_var positions = new ACS::doubleSeq; + positions->length(m_virtual_axes); + std::copy(p.begin(), p.end(), positions->begin()); + return positions._retn(); + } + catch(std::logic_error& le) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, (m_servo_name + "::getAxesPositions()").c_str()); + ex.setReason("Positions history is empty!"); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } +} + +ACS::TimeInterval SRTBaseMinorServoImpl::getTravelTime(const ACS::doubleSeq& _s_p, const ACS::doubleSeq& d_p) +{ + AUTO_TRACE(m_servo_name + "::getTravelTime()"); + + std::vector c_s = m_c_s; // Current speed + + ACS::doubleSeq s_p(_s_p); + + // No starting coordinates, it means we have to start from the current position taking into account the current speed + if(_s_p.length() == 0) + { + s_p = *getAxesPositions(0); + } + else if(_s_p.length() != m_virtual_axes) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, (m_servo_name + "getTravelTime()").c_str()); + ex.setReason("Wrong number of axes for starting_position."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + if(d_p.length() != m_virtual_axes) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, (m_servo_name + "getTravelTime()").c_str()); + ex.setReason("Wrong number of axes for destination_position."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + // d = delta/distance + std::vector d(m_virtual_axes); + for(size_t i = 0; i < m_virtual_axes; i++) + { + d[i] = d_p[i] - s_p[i]; + + // If we sent a starting position, we are just checking the maximum time to get from the start to the desired position + // We take into account the worst case scenario for the speed, we assume we are moving away from the desired position at maximum speed for each axis + if(_s_p.length() != 0) + { + c_s[i] = m_m_s[i] * (d[i] < 0 ? 1 : (d[i] > 0 ? -1 : 0)); + } + } + + double total_time = 0; + + // Calculate the distance and time taken to get to the maximum speed towards the desired position + for(size_t i = 0; i < m_virtual_axes; i++) + { + double inversion_time = 0; + double inversion_distance = 0; + + // We are moving away from our desired position, this is the only case in which we need a deceleration ramp before even starting to move towards our destination + if(std::signbit(c_s[i]) != std::signbit(d[i]) && c_s[i] != 0) + { + inversion_time = std::abs(c_s[i]) / m_a[i]; + inversion_distance = std::abs(c_s[i]) * inversion_time + 0.5 * m_a[i] * std::pow(inversion_time, 2); + // In this case, we can calculate the next acceleration ramp using a starting speed of 0 + c_s = std::vector(c_s.size(), 0.0); + } + + double accel_ramp_time = (m_m_s[i] - std::abs(c_s[i])) / m_a[i]; + double accel_ramp_distance = std::abs(c_s[i]) * accel_ramp_time + 0.5 * m_a[i] * std::pow(accel_ramp_time, 2); + + // Total time = eventual inversion time + calculated acceleration ramp time + full deceleration time + linear movement time + // Linear movement time = linear movement distance / maximum speed + // Linear movement distance = distance + eventual inversion distance - acceleration ramp distance - full deceleration ramp distance + double t = inversion_time + accel_ramp_time + m_r_t[i] + (std::abs(d[i]) + inversion_distance - accel_ramp_distance - m_r_d[i]) / m_m_s[i]; + + total_time = std::max(total_time, t); + + // This does not take into account any dead time but: + // we're going to use this only for PROGRAMTRACK purposes and + // in PROGRAMTRACK mode the SRP servos move faster than the nominal max_speed + // i.e., IIRC, the max physical speed should be around 12mm/s instead of 4mm/s + // Therefore we might not need to add any guard time + } + + return ACS::TimeInterval(total_time * 10000000); +} + +void SRTBaseMinorServoImpl::getAxesRanges(ACS::doubleSeq_out min_ranges_out, ACS::doubleSeq_out max_ranges_out) +{ + AUTO_TRACE("SRTBaseMinorServoImpl::getAxesRanges()"); + + ACS::doubleSeq_var min_ranges = new ACS::doubleSeq; + ACS::doubleSeq_var max_ranges = new ACS::doubleSeq; + min_ranges->length(m_virtual_axes); + max_ranges->length(m_virtual_axes); + std::copy(m_min.begin(), m_min.end(), min_ranges->begin()); + std::copy(m_max.begin(), m_max.end(), max_ranges->begin()); + min_ranges_out = min_ranges._retn(); + max_ranges_out = max_ranges._retn(); +} + +/////////////////// PROTECTED methods +void SRTBaseMinorServoImpl::checkLineStatus() +{ + if(!m_socket.isConnected()) + { + _EXCPT(MinorServoErrors::CommunicationErrorExImpl, ex, (m_servo_name + "checkLineStatus()").c_str()); + ex.setReason("Socket not connected."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + if(m_status.isBlocked() == Management::MNG_TRUE || m_status.getDriveCabinetStatus() == DRIVE_CABINET_ERROR) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, (m_servo_name + "::checkLineStatus()").c_str()); + ex.setReason("Servo system blocked or drive cabinet error."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } +} + +std::vector SRTBaseMinorServoImpl::getMotionConstant(SRTBaseMinorServoImpl& object, const std::string& constant) +{ + AUTO_STATIC_TRACE(object.m_servo_name + "::getMotionConstant()"); + + std::vector values; + + if(constant == "max_speed" || constant == "acceleration") + { + values = getCDBValue>(object.getContainerServices(), constant.c_str()); + if(values.size() != object.m_virtual_axes) + { + _EXCPT(ComponentErrors::CDBAccessExImpl, ex, (object.m_servo_name + "::getMotionConstant()").c_str()); + ex.setFieldName(constant.c_str()); + ex.log(LM_DEBUG); + throw ex.getComponentErrorsEx(); + } + else if(std::any_of(values.begin(), values.end(), [](double value) + { + return value == 0.0; + })) + { + _EXCPT(ComponentErrors::NotAllowedExImpl, ex, (object.m_servo_name + "::getMotionConstant()").c_str()); + ex.setReason(("A" + constant == "acceleration" ? "n " : " " + constant + " equals to 0 is not allowed.").c_str()); + ex.log(LM_DEBUG); + throw ex.getComponentErrorsEx(); + } + } + else if(constant == "min_range" || constant == "max_range" || constant == "tracking_delta") + { + values = getCDBValue>(object.getContainerServices(), constant.c_str()); + if(values.size() != object.m_virtual_axes) + { + _EXCPT(ComponentErrors::CDBAccessExImpl, ex, (object.m_servo_name + "::getMotionConstant()").c_str()); + ex.setFieldName(constant.c_str()); + ex.log(LM_DEBUG); + throw ex.getComponentErrorsEx(); + } + } + else if(constant == "ramp_times") // Acceleration ramp times, 0 to max_speed and vice versa + { + values = std::vector(object.m_virtual_axes); + std::transform(object.m_m_s.begin(), object.m_m_s.end(), object.m_a.begin(), values.begin(), std::divides()); + } + else if(constant == "ramp_distances") // Acceleration ramp distances, 0 to max_speed and vice versa + { + values = std::vector(object.m_virtual_axes); + std::transform(object.m_r_t.begin(), object.m_r_t.end(), object.m_a.begin(), values.begin(), [](double acceleration_ramp_time, double acceleration) + { + return 0.5 * acceleration * std::pow(acceleration_ramp_time, 2); + }); + } + + return values; +} + +/////////////////// PRIVATE methods +std::vector SRTBaseMinorServoImpl::getPropertiesTable(SRTBaseMinorServoImpl& object, const std::string& properties_name) +{ + AUTO_STATIC_TRACE(object.m_servo_name + "::getPropertiesTable()"); + + std::vector properties; + + IRA::CDBTable table(object.getContainerServices(), properties_name.c_str(), std::string("DataBlock/MinorServo/" + object.m_servo_name).c_str()); + IRA::CError error; + error.Reset(); + + if(!table.addField(error, "property_name", IRA::CDataField::STRING)) + { + error.setExtra("Error adding field property_name", 0); + } + if(!error.isNoError()) + { + _EXCPT_FROM_ERROR(ComponentErrors::IRALibraryResourceExImpl, ex, error); + ex.setCode(error.getErrorCode()); + ex.setDescription((const char *)error.getDescription()); + ex.log(LM_DEBUG); + throw ex.getComponentErrorsEx(); + } + if(!table.openTable(error)) + { + _EXCPT_FROM_ERROR(ComponentErrors::CDBAccessExImpl, ex, error); + ex.setFieldName(properties_name.c_str()); + ex.log(LM_DEBUG); + throw ex.getComponentErrorsEx(); + } + + table.First(); + for(unsigned int i = 0; i < table.recordCount(); i++, table.Next()) + { + properties.push_back(std::string(table["property_name"]->asString())); + } + table.closeTable(); + + size_t expected_size = 0; + + if(properties_name.find("virtual_") == 0) + { + expected_size = object.m_virtual_axes; + } + else if(properties_name.find("physical_") == 0) + { + expected_size = object.m_physical_axes; + } + + if(expected_size == 0 || properties.size() != expected_size) + { + _EXCPT(ComponentErrors::CDBAccessExImpl, ex, (object.m_servo_name + "::initialize()").c_str()); + ex.setFieldName(properties_name.c_str()); + ex.log(LM_DEBUG); + throw ex.getComponentErrorsEx(); + } + + return properties; +} + + +GET_PROPERTY_REFERENCE(Management::ROTBoolean, SRTBaseMinorServoImpl, m_enabled_ptr, enabled); +GET_PROPERTY_REFERENCE(ROSRTMinorServoCabinetStatus, SRTBaseMinorServoImpl, m_drive_cabinet_status_ptr, drive_cabinet_status); +GET_PROPERTY_REFERENCE(Management::ROTBoolean, SRTBaseMinorServoImpl, m_block_ptr, block); +GET_PROPERTY_REFERENCE(ROSRTMinorServoOperativeMode, SRTBaseMinorServoImpl, m_operative_mode_ptr, operative_mode); +GET_PROPERTY_REFERENCE(ACS::RObooleanSeq, SRTBaseMinorServoImpl, m_physical_axes_enabled_ptr, physical_axes_enabled); +GET_PROPERTY_REFERENCE(ACS::ROdoubleSeq, SRTBaseMinorServoImpl, m_physical_positions_ptr, physical_positions); +GET_PROPERTY_REFERENCE(ACS::ROlong, SRTBaseMinorServoImpl, m_virtual_axes_ptr, virtual_axes); +GET_PROPERTY_REFERENCE(ACS::ROdoubleSeq, SRTBaseMinorServoImpl, m_plain_virtual_positions_ptr, plain_virtual_positions); +GET_PROPERTY_REFERENCE(ACS::ROdoubleSeq, SRTBaseMinorServoImpl, m_virtual_positions_ptr, virtual_positions); +GET_PROPERTY_REFERENCE(ACS::ROdoubleSeq, SRTBaseMinorServoImpl, m_virtual_offsets_ptr, virtual_offsets); +GET_PROPERTY_REFERENCE(ACS::ROdoubleSeq, SRTBaseMinorServoImpl, m_virtual_user_offsets_ptr, virtual_user_offsets); +GET_PROPERTY_REFERENCE(ACS::ROdoubleSeq, SRTBaseMinorServoImpl, m_virtual_system_offsets_ptr, virtual_system_offsets); +GET_PROPERTY_REFERENCE(Management::ROTBoolean, SRTBaseMinorServoImpl, m_in_use_ptr, in_use); +GET_PROPERTY_REFERENCE(ACS::ROstring, SRTBaseMinorServoImpl, m_current_setup_ptr, current_setup); diff --git a/SRT/Servers/SRTMinorServo/src/SRTGenericMinorServoImpl.cpp b/SRT/Servers/SRTMinorServo/src/SRTGenericMinorServoImpl.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1b3dad9ec79d343c7925cf84dc96ae1cecf7ec5a --- /dev/null +++ b/SRT/Servers/SRTMinorServo/src/SRTGenericMinorServoImpl.cpp @@ -0,0 +1,13 @@ +#include "SRTMinorServoImpl.h" + +SRTGenericMinorServoImpl::SRTGenericMinorServoImpl(const ACE_CString &componentName, maci::ContainerServices *containerServices) : SRTBaseMinorServoImpl(componentName, containerServices) +{ + AUTO_TRACE(m_servo_name + "::SRTGenericMinorServoImpl()"); +} + +SRTGenericMinorServoImpl::~SRTGenericMinorServoImpl() +{ + AUTO_TRACE(m_servo_name + "::~SRTGenericMinorServoImpl()"); +} + +MACI_DLL_SUPPORT_FUNCTIONS(SRTGenericMinorServoImpl) diff --git a/SRT/Servers/SRTMinorServo/src/SRTMinorServoBossCore.cpp b/SRT/Servers/SRTMinorServo/src/SRTMinorServoBossCore.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7c405282252b98cbdc509638d5763eb263eb3a35 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/src/SRTMinorServoBossCore.cpp @@ -0,0 +1,1316 @@ +#include "SRTMinorServoBossCore.h" + +using namespace MinorServo; + +SRTMinorServoBossCore::SRTMinorServoBossCore(SRTMinorServoBossImpl& component) : + m_component(component), + m_status_thread(nullptr), + m_setup_thread(nullptr), + m_park_thread(nullptr), + m_tracking_thread(nullptr), + m_scan_thread(nullptr), + m_antennaBoss("IDL:alma/Antenna/AntennaBoss:1.0", m_component.getContainerServices()), + m_status(), + m_motion_status(MOTION_STATUS_UNCONFIGURED), + m_commanded_setup("Unknown"), + m_commanded_configuration(CONFIGURATION_UNKNOWN), + m_subsystem_status(Management::MNG_WARNING), + m_actual_setup("Unknown"), + m_ready(Management::MNG_FALSE), + m_starting(Management::MNG_FALSE), + m_as_configuration(getCDBConfiguration("active_surface_configuration")), + m_elevation_tracking(Management::MNG_FALSE), + m_elevation_tracking_enabled(getCDBConfiguration("elevation_tracking_enabled")), + m_scan_active(Management::MNG_FALSE), + m_scanning(Management::MNG_FALSE), + m_tracking(Management::MNG_FALSE), + m_reload_servo_offsets(true), + m_socket_configuration(SRTMinorServoSocketConfiguration::getInstance(m_component.getContainerServices())), + m_socket(SRTMinorServoSocket::getInstance(m_socket_configuration.m_ip_address, m_socket_configuration.m_port, m_socket_configuration.m_timeout)), + m_socket_connected(m_socket.isConnected() ? Management::MNG_TRUE : Management::MNG_FALSE), + m_servos{ + //{ "PFP", m_component.getContainerServices()->getComponent("MINORSERVO/PFP") }, + { "SRP", m_component.getContainerServices()->getComponent("MINORSERVO/SRP") }, + { "GFR", m_component.getContainerServices()->getComponent("MINORSERVO/GFR") } + //{ "M3R", m_component.getContainerServices()->getComponent("MINORSERVO/M3R") } + }, + m_tracking_servos{ + //{ "PFP", m_component.getContainerServices()->getComponent("MINORSERVO/PFP") }, + { "SRP", m_component.getContainerServices()->getComponent("MINORSERVO/SRP") } + } +{ + AUTO_TRACE("SRTMinorServoBossCore::SRTMinorServoBossCore()"); + + startThread(m_status_thread, (ACS::TimeInterval)(getCDBValue(m_component.getContainerServices(), "status_thread_period") * 10000000)); +} + +SRTMinorServoBossCore::~SRTMinorServoBossCore() +{ + AUTO_TRACE("SRTMinorServoBossCore::~SRTMinorServoBossCore()"); + + destroyThread(m_setup_thread); + destroyThread(m_park_thread); + destroyThread(m_tracking_thread); + destroyThread(m_scan_thread); + destroyThread(m_status_thread); +} + +bool SRTMinorServoBossCore::status() +{ + AUTO_TRACE("SRTMinorServoBossCore::status()"); + + try + { + // Attempt communication anyway + m_socket.sendCommand(SRTMinorServoCommandLibrary::status(), m_status); + + if(m_socket_connected.load() == Management::MNG_FALSE) + { + ACS_LOG(LM_FULL_INFO, "SRTMinorServoBossCore::status()", (LM_NOTICE, "Socket connected.")); + m_socket_connected.store(Management::MNG_TRUE); + m_subsystem_status.store(Management::MNG_WARNING); + } + } + catch(MinorServoErrors::MinorServoErrorsEx& ex) + { + if(m_socket_connected.load() == Management::MNG_TRUE) + { + ACS_LOG(LM_FULL_INFO, "SRTMinorServoBossCore::status()", (LM_CRITICAL, (std::string("Socket disconnected: ") + getReasonFromEx(ex)).c_str())); + m_socket_connected.store(Management::MNG_FALSE); + + stopThread(m_setup_thread); + stopThread(m_park_thread); + stopThread(m_tracking_thread); + stopThread(m_scan_thread); + setFailure(); + + m_reload_servo_offsets = true; + } + + return false; + } + + try + { + checkLineStatus(); + } + catch(MinorServoErrors::MinorServoErrorsEx& ex) + { + _IRA_LOGFILTER_LOG(LM_ERROR, "SRTMinorServoBossCore::status()", getReasonFromEx(ex)); + setFailure(); + return false; + } + + SRTMinorServoMotionStatus motion_status = m_motion_status.load(); + /*if(motion_status == MOTION_STATUS_TRACKING || motion_status == MOTION_STATUS_CONFIGURED) + { + // We only get here if the system is configured, therefore we check the correct position of the gregorian cover + SRTMinorServoGregorianCoverStatus commanded_gregorian_cover_position = m_status.getFocalConfiguration() == CONFIGURATION_PRIMARY ? COVER_STATUS_CLOSED : COVER_STATUS_OPEN; + if(m_status.getGregorianCoverPosition() != commanded_gregorian_cover_position) + { + ACS_LOG(LM_FULL_INFO, "SRTMinorServoBossCore::status()", (LM_CRITICAL, "Gregorian cover in wrong position.")); + setFailure(); + return false; + } + }*/ + + for(const auto& [name, servo] : m_servos) + { + if(!servo->status()) + { + ACS_LOG(LM_FULL_INFO, "SRTMinorServoBossCore::status()", (LM_CRITICAL, ("Error checking " + name + " status.").c_str())); + setFailure(); + return false; + } + + if(m_reload_servo_offsets) + { + servo->reloadOffsets(); + } + } + + m_reload_servo_offsets = false; + + if(motion_status == MOTION_STATUS_TRACKING) + { + if(std::all_of(m_current_tracking_servos.begin(), m_current_tracking_servos.end(), [](const std::pair& servo) -> bool + { + ACSErr::Completion_var comp; + return servo.second->isTracking(); + })) + { + m_tracking.store(Management::MNG_TRUE); + } + else + { + m_tracking.store(Management::MNG_FALSE); + } + } + else if(motion_status == MOTION_STATUS_CONFIGURED) + { + m_tracking.store(Management::MNG_TRUE); + } + else + { + m_tracking.store(Management::MNG_FALSE); + } + + return true; +} + +void SRTMinorServoBossCore::setup(std::string commanded_setup) +{ + AUTO_TRACE("SRTMinorServoBossCore::setup()"); + + try + { + checkLineStatus(); + } + catch(MinorServoErrors::MinorServoErrorsEx& mse) + { + _ADD_BACKTRACE(ManagementErrors::ConfigurationErrorExImpl, ex, mse, "SRTMinorServoBossCore::setup()"); + ex.setSubsystem("MinorServo"); + ex.setReason(getReasonFromEx(mse)); + ex.log(LM_DEBUG); + throw ex.getConfigurationErrorEx(); + } + + if(m_scan_active.load() == Management::MNG_TRUE) + { + ACS_LOG(LM_FULL_INFO, "servoSetup", (LM_NOTICE, "THE SYSTEM IS PERFORMING A SCAN, CANNOT SETUP NOW")); + _EXCPT(ManagementErrors::ConfigurationErrorExImpl, ex, "SRTMinorServoBossCore::setup()"); + ex.setSubsystem("MinorServo"); + ex.setReason("The system is waiting for a scan to be completed."); + ex.log(LM_DEBUG); + throw ex.getConfigurationErrorEx(); + } + + std::transform(commanded_setup.begin(), commanded_setup.end(), commanded_setup.begin(), ::toupper); + + std::pair cmd_configuration; + SRTMinorServoFocalConfiguration commanded_configuration; + + try + { + cmd_configuration = DiscosConfigurationNameTable.at(commanded_setup); + commanded_configuration = cmd_configuration.first; + if(m_as_configuration.load() == Management::MNG_TRUE && cmd_configuration.second) + { + commanded_setup += "_ASACTIVE"; + } + } + catch(std::out_of_range& oor) + { + ACS_LOG(LM_FULL_INFO, "servoSetup", (LM_NOTICE, ("UNKNOWN CONFIGURATION '" + commanded_setup + "'").c_str())); + _EXCPT(ManagementErrors::ConfigurationErrorExImpl, ex, "SRTMinorServoBossCore::setup()"); + ex.setSubsystem("MinorServo"); + ex.setReason(("Unknown configuration '" + commanded_setup + "'.").c_str()); + ex.log(LM_DEBUG); + throw ex.getConfigurationErrorEx(); + } + + // Exit if commanded setup is already in place or is about to be + if(commanded_configuration == m_commanded_configuration.load() && commanded_setup == m_commanded_setup) + { + SRTMinorServoMotionStatus motion_status = m_motion_status.load(); + if(motion_status == MOTION_STATUS_STARTING) + { + ACS_LOG(LM_FULL_INFO, "servoSetup", (LM_NOTICE, ("ALREADY SETTING UP '" + commanded_setup + "' CONFIGURATION").c_str())); + return; + } + else if(motion_status == MOTION_STATUS_CONFIGURED || motion_status == MOTION_STATUS_TRACKING) + { + ACS_LOG(LM_FULL_INFO, "servoSetup", (LM_NOTICE, ("CONFIGURATION '" + commanded_setup + "' ALREADY IN PLACE").c_str())); + return; + } + } + + ACS_LOG(LM_FULL_INFO, "servoSetup", (LM_NOTICE, ("SETTING UP '" + commanded_setup + "' CONFIGURATION").c_str())); + + m_commanded_configuration.store(commanded_configuration); + m_commanded_setup = commanded_setup; + + // Stop the setup, park and tracking threads if running + stopThread(m_setup_thread); + stopThread(m_park_thread); + stopThread(m_tracking_thread); + + m_subsystem_status.store(Management::MNG_WARNING); + m_actual_setup = ""; + m_ready.store(Management::MNG_FALSE); + m_starting.store(Management::MNG_TRUE); + m_scan_active.store(Management::MNG_FALSE); + m_scanning.store(Management::MNG_FALSE); + m_tracking.store(Management::MNG_FALSE); + m_motion_status.store(MOTION_STATUS_STARTING); + m_current_servos.clear(); + m_current_tracking_servos.clear(); + + // Send the STOP command to all the servos + for(const auto& [servo_name, servo] : m_servos) + { + try + { + servo->stop(); + } + catch(MinorServoErrors::MinorServoErrorsEx& mse) + { + _ADD_BACKTRACE(ManagementErrors::ConfigurationErrorExImpl, ex, mse, "SRTMinorServoBossCore::setup()"); + ex.setSubsystem("MinorServo"); + ex.setReason(("Error while sending the STOP command to " + servo_name + ".").c_str()); + ex.log(LM_DEBUG); + throw ex.getConfigurationErrorEx(); + } + } + + // Start the setup thread + try + { + startThread(m_setup_thread); + } + catch(ComponentErrors::ComponentErrorsEx& ce) + { + _ADD_BACKTRACE(ManagementErrors::ConfigurationErrorExImpl, ex, ce, "SRTMinorServoBossCore::setup()"); + ex.setSubsystem("MinorServo"); + ex.setReason("Error while trying to start the setup_thread"); + ex.log(LM_DEBUG); + throw ex.getConfigurationErrorEx(); + } +} + +void SRTMinorServoBossCore::park() +{ + AUTO_TRACE("SRTMinorServoBossCore::park()"); + + try + { + checkLineStatus(); + } + catch(MinorServoErrors::MinorServoErrorsEx& mse) + { + _ADD_BACKTRACE(ManagementErrors::ParkingErrorExImpl, ex, mse, "SRTMinorServoBossCore::park()"); + ex.setSubsystem("MinorServo"); + ex.setReason(getReasonFromEx(mse)); + ex.log(LM_DEBUG); + throw ex.getParkingErrorEx(); + } + + if(m_scan_active.load() == Management::MNG_TRUE) + { + ACS_LOG(LM_FULL_INFO, "servoPark", (LM_NOTICE, "THE SYSTEM IS PERFORMING A SCAN, CANNOT PARK NOW")); + _EXCPT(ManagementErrors::ParkingErrorExImpl, ex, "SRTMinorServoBossCore::park()"); + ex.setSubsystem("MinorServo"); + ex.setReason("The system is waiting for a scan to be completed."); + ex.log(LM_DEBUG); + throw ex.getParkingErrorEx(); + } + + SRTMinorServoMotionStatus motion_status = m_motion_status.load(); + if(motion_status == MOTION_STATUS_PARKING) + { + ACS_LOG(LM_FULL_INFO, "servoPark", (LM_NOTICE, "MINOR SERVOS ALREADY PARKING")); + return; + } + else if (motion_status == MOTION_STATUS_PARKED) + { + ACS_LOG(LM_FULL_INFO, "servoPark", (LM_NOTICE, "MINOR SERVOS ALREADY PARKED")); + return; + } + + ACS_LOG(LM_FULL_INFO, "servoPark", (LM_NOTICE, "PARKING")); + + m_commanded_configuration.store(CONFIGURATION_PARK); + m_commanded_setup = "Park"; + + // Stop the setup, park and tracking threads if running + stopThread(m_setup_thread); + stopThread(m_park_thread); + stopThread(m_tracking_thread); + + m_subsystem_status.store(Management::MNG_WARNING); + m_actual_setup = ""; + m_ready.store(Management::MNG_FALSE); + m_starting.store(Management::MNG_TRUE); + m_scan_active.store(Management::MNG_FALSE); + m_scanning.store(Management::MNG_FALSE); + m_tracking.store(Management::MNG_FALSE); + m_motion_status.store(MOTION_STATUS_PARKING); + m_current_servos.clear(); + m_current_tracking_servos.clear(); + + /*try + { + // Send the STOW command to close the gregorian cover + if(!m_socket.sendCommand(SRTMinorServoCommandLibrary::stow("Gregoriano", COVER_STATUS_CLOSED)).checkOutput()) + { + _EXCPT(ManagementErrors::ParkingErrorExImpl, ex, "SRTMinorServoBossCore::park()"); + ex.setSubsystem("MinorServo"); + ex.setReason("Error while sending a STOW command to the gregorian cover."); + ex.log(LM_DEBUG); + throw ex.getParkingErrorEx(); + } + } + catch(MinorServoErrors::MinorServoErrorsEx& mse) + { + _ADD_BACKTRACE(ManagementErrors::ParkingErrorExImpl, ex, mse, "SRTMinorServoBossCore::park()"); + ex.setSubsystem("MinorServo"); + ex.setReason("Error while sending the STOW command to the gregorian cover."); + ex.log(LM_DEBUG); + throw ex.getParkingErrorEx(); + }*/ + + // Send the STOP command to all the servos + for(const auto& [servo_name, servo] : m_servos) + { + try + { + servo->stop(); + } + catch(MinorServoErrors::MinorServoErrorsEx& mse) + { + _ADD_BACKTRACE(ManagementErrors::ParkingErrorExImpl, ex, mse, "SRTMinorServoBossCore::park()"); + ex.setSubsystem("MinorServo"); + ex.setReason(("Error while sending the STOP command to " + servo_name + ".").c_str()); + ex.log(LM_DEBUG); + throw ex.getParkingErrorEx(); + } + } + + // Start the park thread + try + { + startThread(m_park_thread); + } + catch(ComponentErrors::ComponentErrorsEx& ce) + { + _ADD_BACKTRACE(ManagementErrors::ParkingErrorExImpl, ex, ce, "SRTMinorServoBossCore::park()"); + ex.setSubsystem("MinorServo"); + ex.setReason("Error while trying to start the park_thread"); + ex.log(LM_DEBUG); + throw ex.getParkingErrorEx(); + } +} + +void SRTMinorServoBossCore::setElevationTracking(std::string configuration) +{ + AUTO_TRACE("SRTMinorServoBossCore::setElevationTracking()"); + + if(m_scan_active.load() == Management::MNG_TRUE) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::setElevationTracking()"); + ex.setReason("The system is waiting for a scan to be completed."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + std::transform(configuration.begin(), configuration.end(), configuration.begin(), ::toupper); + + if(configuration != "ON" && configuration != "OFF") + { + _EXCPT(MinorServoErrors::ConfigurationErrorExImpl, ex, "SRTMinorServoBossCore::setElevationTracking()"); + ex.addData("Reason", ("Unknown setElevationTracking configuration '" + configuration + "'.").c_str()); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + ACS_LOG(LM_FULL_INFO, "setServoElevationTracking", (LM_NOTICE, ("SETTING ELEVATION TRACKING TO " + configuration).c_str())); + + if(configuration == "ON" && m_elevation_tracking_enabled.load() != Management::MNG_TRUE) + { + m_elevation_tracking_enabled.store(Management::MNG_TRUE); + + if(m_motion_status.load() == MOTION_STATUS_CONFIGURED) + { + m_motion_status.store(MOTION_STATUS_TRACKING); + startThread(m_tracking_thread); + } + } + else if(configuration == "OFF" && m_elevation_tracking_enabled.load() != Management::MNG_FALSE) + { + stopThread(m_tracking_thread); + m_elevation_tracking_enabled.store(Management::MNG_FALSE); + + if(m_motion_status.load() == MOTION_STATUS_TRACKING) + { + m_motion_status.store(MOTION_STATUS_CONFIGURED); + } + } +} + +void SRTMinorServoBossCore::setASConfiguration(std::string configuration) +{ + AUTO_TRACE("SRTMinorServoBossCore::setASConfiguration()"); + + if(m_scan_active.load() == Management::MNG_TRUE) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::setASConfiguration()"); + ex.setReason("The system is waiting for a scan to be completed."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + std::transform(configuration.begin(), configuration.end(), configuration.begin(), ::toupper); + + if(configuration != "ON" && configuration != "OFF") + { + _EXCPT(MinorServoErrors::ConfigurationErrorExImpl, ex, "SRTMinorServoBossCore::setASConfiguration()"); + ex.addData("Reason", ("Unknown setASConfiguration configuration '" + configuration + "'.").c_str()); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + ACS_LOG(LM_FULL_INFO, "setServoASConfiguration", (LM_NOTICE, ("SETTING ACTIVE SURFACE CONFIGURATION TO " + configuration).c_str())); + + if(configuration == "ON" && m_as_configuration.load() != Management::MNG_TRUE) + { + m_as_configuration.store(Management::MNG_TRUE); + } + else if(configuration == "OFF" && m_as_configuration.load() != Management::MNG_FALSE) + { + m_as_configuration.store(Management::MNG_FALSE); + } + else + { + // We asked for the same configuration, nothing else to do + return; + } + + // Should reload the correct setup if the system was already configured or was about to be + if(!m_commanded_setup.empty()) + { + configuration = m_commanded_setup.substr(0, m_commanded_setup.find("_")); + try + { + setup(configuration); + } + catch(ManagementErrors::ConfigurationErrorEx& ce) + { + _ADD_BACKTRACE(MinorServoErrors::ConfigurationErrorExImpl, ex, ce, "SRTMinorServoBossCore::setASConfiguration()"); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + } +} + +void SRTMinorServoBossCore::setGregorianCoverPosition(std::string position) +{ + AUTO_TRACE("SRTMinorServoBossCore::setGregorianCoverPosition()"); + + checkLineStatus(); + + std::transform(position.begin(), position.end(), position.begin(), ::toupper); + + if(position != "OPEN" && position != "CLOSED") + { + _EXCPT(MinorServoErrors::StowErrorExImpl, ex, "SRTMinorServoBossCore::setGregorianCoverPosition()"); + ex.addData("Reason", ("Unknown position '" + position + "'.").c_str()); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + SRTMinorServoMotionStatus motion_status = m_motion_status.load(); + if(motion_status != MOTION_STATUS_PARKING && motion_status != MOTION_STATUS_PARKED) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::setGregorianCoverPosition()"); + ex.setReason("You can set the gregorian cover position only when the system is parked or is parking."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + ACS_LOG(LM_FULL_INFO, "setGregorianCoverPosition", (LM_NOTICE, ("SETTING GREGORIAN COVER POSITION TO " + position).c_str())); + + if(!m_socket.sendCommand(SRTMinorServoCommandLibrary::stow("Gregoriano", position == "OPEN" ? COVER_STATUS_OPEN : COVER_STATUS_CLOSED)).checkOutput()) + { + _EXCPT(MinorServoErrors::StowErrorExImpl, ex, "SRTMinorServoBossCore::setGregorianCoverPosition()"); + ex.addData("Reason", "Error while sending a STOW command to the gregorian cover."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } +} + +void SRTMinorServoBossCore::preset(double elevation) +{ + // Elevation is expressed in degrees + AUTO_TRACE("SRTMinorServoBossCore::preset()"); + + checkLineStatus(); + + if(m_scan_active.load() == Management::MNG_TRUE) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::preset()"); + ex.setReason("The system is waiting for a scan to be completed."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + if(m_motion_status.load() != MOTION_STATUS_CONFIGURED) + { + _EXCPT(MinorServoErrors::PositioningErrorExImpl, ex, "SRTMinorServoBossCore::preset()"); + ex.addData("Reason", "Minor servos are tracking, cannot send a global preset command now."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + for(const auto& [servo_name, servo] : m_current_servos) + { + servo->preset(*servo->calcCoordinates(elevation)); + } +} + +void SRTMinorServoBossCore::clearUserOffsets(std::string servo_name) +{ + AUTO_TRACE("SRTMinorServoBossCore::clearUserOffsets()"); + + checkLineStatus(); + + if(m_scan_active.load() == Management::MNG_TRUE) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::clearUserOffsets()"); + ex.setReason("The system is waiting for a scan to be completed."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + SRTMinorServoMotionStatus motion_status = m_motion_status.load(); + if(motion_status != MOTION_STATUS_CONFIGURED && motion_status != MOTION_STATUS_TRACKING) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::clearUserOffsets()"); + ex.setReason("The system is not configured yet."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + std::transform(servo_name.begin(), servo_name.end(), servo_name.begin(), ::toupper); + + if(servo_name == "ALL") + { + for(const auto& [servo_name, servo] : m_current_servos) + { + servo->clearUserOffsets(); + } + return; + } + else if(!m_servos.count(servo_name)) + { + _EXCPT(MinorServoErrors::OffsetErrorExImpl, ex, "SRTMinorServoBossCore::clearUserOffsets()"); + ex.addData("Reason", ("Unknown servo '" + servo_name + "'.").c_str()); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + try + { + auto servo = m_current_servos.at(servo_name); + servo->clearUserOffsets(); + } + catch(std::out_of_range& oor) + { + _EXCPT(MinorServoErrors::OffsetErrorExImpl, ex, "SRTMinorServoBossCore::clearUserOffsets()"); + ex.addData("Reason", ("Servo '" + servo_name + "' not in use with the current configuration.").c_str()); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } +} + +void SRTMinorServoBossCore::setUserOffset(std::string servo_axis_name, double offset, bool log) +{ + AUTO_TRACE("SRTMinorServoBossCore::setUserOffset()"); + + checkLineStatus(); + + if(m_scan_active.load() == Management::MNG_TRUE) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::setUserOffset()"); + ex.setReason("The system is waiting for a scan to be completed."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + SRTMinorServoMotionStatus motion_status = m_motion_status.load(); + if(motion_status != MOTION_STATUS_CONFIGURED && motion_status != MOTION_STATUS_TRACKING) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::setUserOffset()"); + ex.setReason("The system is not configured yet."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + std::transform(servo_axis_name.begin(), servo_axis_name.end(), servo_axis_name.begin(), ::toupper); + + std::stringstream ss(servo_axis_name); + std::string servo_name, axis_name; + std::getline(ss, servo_name, '_'); + ss >> axis_name; + + if(!m_servos.count(servo_name)) + { + _EXCPT(MinorServoErrors::OffsetErrorExImpl, ex, "SRTMinorServoBossCore::setUserOffsets()"); + ex.addData("Reason", ("Unknown servo '" + servo_name + "'.").c_str()); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + try + { + auto servo = m_current_servos.at(servo_name); + if(log) + { + ACS_LOG(LM_FULL_INFO, "setServoOffset", (LM_NOTICE, ("SETTING '" + servo_name + "' '" + axis_name + "' OFFSET TO " + std::to_string(offset)).c_str())); + } + servo->setUserOffset(axis_name.c_str(), offset); + } + catch(std::out_of_range& oor) + { + _EXCPT(MinorServoErrors::OffsetErrorExImpl, ex, "SRTMinorServoBossCore::setUserOffset()"); + ex.addData("Reason", ("Servo '" + servo_name + "' not in use with the current configuration.").c_str()); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } +} + +ACS::doubleSeq* SRTMinorServoBossCore::getUserOffsets() +{ + AUTO_TRACE("SRTMinorServoBossCore::getUserOffsets()"); + + SRTMinorServoMotionStatus motion_status = m_motion_status.load(); + if(motion_status != MOTION_STATUS_CONFIGURED && motion_status != MOTION_STATUS_TRACKING) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::getUserOffsets()"); + ex.setReason("The system is not configured yet."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + ACS::doubleSeq_var offsets = new ACS::doubleSeq; + + for(const auto& [servo_name, servo] : m_current_servos) + { + ACS::doubleSeq servo_offsets = *servo->getUserOffsets(); + size_t start_index = offsets->length(); + offsets->length(start_index + servo_offsets.length()); + std::copy(servo_offsets.begin(), servo_offsets.end(), offsets->begin() + start_index); + } + + return offsets._retn(); +} + +void SRTMinorServoBossCore::clearSystemOffsets(std::string servo_name) +{ + AUTO_TRACE("SRTMinorServoBossCore::clearSystemOffsets()"); + + checkLineStatus(); + + if(m_scan_active.load() == Management::MNG_TRUE) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::clearSystemOffsets()"); + ex.setReason("The system is waiting for a scan to be completed."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + SRTMinorServoMotionStatus motion_status = m_motion_status.load(); + if(motion_status != MOTION_STATUS_CONFIGURED && motion_status != MOTION_STATUS_TRACKING) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::clearSystemOffsets()"); + ex.setReason("The system is not configured yet."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + std::transform(servo_name.begin(), servo_name.end(), servo_name.begin(), ::toupper); + + if(servo_name == "ALL") + { + for(const auto& [servo_name, servo] : m_current_servos) + { + servo->clearSystemOffsets(); + } + return; + } + else if(!m_servos.count(servo_name)) + { + _EXCPT(MinorServoErrors::OffsetErrorExImpl, ex, "SRTMinorServoBossCore::clearSystemOffsets()"); + ex.addData("Reason", ("Unknown servo '" + servo_name + "'.").c_str()); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + try + { + auto servo = m_current_servos.at(servo_name); + servo->clearSystemOffsets(); + } + catch(std::out_of_range& oor) + { + _EXCPT(MinorServoErrors::OffsetErrorExImpl, ex, "SRTMinorServoBossCore::clearSystemOffsets()"); + ex.addData("Reason", ("Servo '" + servo_name + "' not in use with the current configuration.").c_str()); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } +} + +void SRTMinorServoBossCore::setSystemOffset(std::string servo_axis_name, double offset) +{ + AUTO_TRACE("SRTMinorServoBossCore::setSystemOffset()"); + + checkLineStatus(); + + if(m_scan_active.load() == Management::MNG_TRUE) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::setSystemOffset()"); + ex.setReason("The system is waiting for a scan to be completed."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + SRTMinorServoMotionStatus motion_status = m_motion_status.load(); + if(motion_status != MOTION_STATUS_CONFIGURED && motion_status != MOTION_STATUS_TRACKING) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::setSystemOffset()"); + ex.setReason("The system is not configured yet."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + std::transform(servo_axis_name.begin(), servo_axis_name.end(), servo_axis_name.begin(), ::toupper); + + std::stringstream ss(servo_axis_name); + std::string servo_name, axis_name; + std::getline(ss, servo_name, '_'); + ss >> axis_name; + + if(!m_servos.count(servo_name)) + { + _EXCPT(MinorServoErrors::OffsetErrorExImpl, ex, "SRTMinorServoBossCore::setSystemOffset()"); + ex.addData("Reason", ("Unknown servo '" + servo_name + "'.").c_str()); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + try + { + auto servo = m_current_servos.at(servo_name); + servo->setSystemOffset(axis_name.c_str(), offset); + } + catch(std::out_of_range& oor) + { + _EXCPT(MinorServoErrors::OffsetErrorExImpl, ex, "SRTMinorServoBossCore::setSystemOffset()"); + ex.addData("Reason", ("Servo '" + servo_name + "' not in use with the current configuration.").c_str()); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } +} + +ACS::doubleSeq* SRTMinorServoBossCore::getSystemOffsets() +{ + AUTO_TRACE("SRTMinorServoBossCore::getSystemOffsets()"); + + SRTMinorServoMotionStatus motion_status = m_motion_status.load(); + if(motion_status != MOTION_STATUS_CONFIGURED && motion_status != MOTION_STATUS_TRACKING) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::getSystemOffsets()"); + ex.setReason("The system is not configured yet."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + ACS::doubleSeq_var offsets = new ACS::doubleSeq; + + for(const auto& [servo_name, servo] : m_current_servos) + { + ACS::doubleSeq servo_offsets = *servo->getSystemOffsets(); + size_t start_index = offsets->length(); + offsets->length(start_index + servo_offsets.length()); + std::copy(servo_offsets.begin(), servo_offsets.end(), offsets->begin() + start_index); + } + + return offsets._retn(); +} + +void SRTMinorServoBossCore::getAxesInfo(ACS::stringSeq_out axes_names_out, ACS::stringSeq_out axes_units_out) +{ + AUTO_TRACE("SRTMinorServoBossCore::getAxesInfo()"); + + SRTMinorServoMotionStatus motion_status = m_motion_status.load(); + if(motion_status != MOTION_STATUS_CONFIGURED && motion_status != MOTION_STATUS_TRACKING) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::getAxesInfo()"); + ex.setReason("The system is not configured yet."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + std::vector axes_names_vector, axes_units_vector; + + ACS::stringSeq_var axes_names = new ACS::stringSeq; + ACS::stringSeq_var axes_units = new ACS::stringSeq; + + for(const auto& [servo_name, servo] : m_current_servos) + { + ACS::stringSeq_var servo_axes_names; + ACS::stringSeq_var servo_axes_units; + servo->getAxesInfo(servo_axes_names, servo_axes_units); + + std::transform(servo_axes_names->begin(), servo_axes_names->end(), servo_axes_names->begin(), [servo_name](const char* axis_name) + { + return CORBA::string_dup((servo_name + "_" + axis_name).c_str()); + }); + + size_t names_index = axes_names->length(); + size_t units_index = axes_units->length(); + axes_names->length(names_index + servo_axes_names->length()); + axes_units->length(units_index + servo_axes_units->length()); + std::copy(servo_axes_names->begin(), servo_axes_names->end(), axes_names->begin() + names_index); + std::copy(servo_axes_units->begin(), servo_axes_units->end(), axes_units->begin() + units_index); + } + + axes_names_out = axes_names._retn(); + axes_units_out = axes_units._retn(); +} + +ACS::doubleSeq* SRTMinorServoBossCore::getAxesPositions(ACS::Time acs_time) +{ + SRTMinorServoMotionStatus motion_status = m_motion_status.load(); + if(motion_status != MOTION_STATUS_CONFIGURED && motion_status != MOTION_STATUS_TRACKING) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::getAxesPositions()"); + ex.setReason("The system is not configured yet."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + ACS::doubleSeq_var positions = new ACS::doubleSeq; + + for(const auto& [servo_name, servo] : m_current_servos) + { + ACS::doubleSeq servo_positions = *servo->getAxesPositions(acs_time); + size_t start_index = positions->length(); + positions->length(start_index + servo_positions.length()); + std::copy(servo_positions.begin(), servo_positions.end(), positions->begin() + start_index); + } + + return positions._retn(); +} + +SRTMinorServoScan SRTMinorServoBossCore::checkScanFeasibility(const ACS::Time& start_time, const MinorServoScan& scan_info, const Antenna::TRunTimeParameters& antenna_info) +{ + // TODO: check whether to start from the left side or the right side, LOW priority + AUTO_TRACE("SRTMinorServoBossCore::checkScanFeasibility()"); + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::checkScanFeasibility()"); + + SRTMinorServoScan new_scan; + new_scan.scan_range = scan_info.range; + new_scan.scan_duration = scan_info.total_time; + new_scan.starting_elevation = antenna_info.elevation * DR2D; + + std::string axis_code(scan_info.axis_code); + // Get the axis that will have to be moved during the scan + std::transform(axis_code.begin(), axis_code.end(), axis_code.begin(), ::toupper); + std::string servo_name = axis_code.substr(0, axis_code.find("_")); + std::string axis_name = axis_code.substr(axis_code.find("_") + 1); + + new_scan.servo_name = servo_name; + new_scan.axis_name = axis_name; + + ACS::stringSeq_var servo_axes_names, servo_axes_units; + + if(!m_servos.count(servo_name)) + { + ex.setReason(("Unknown servo '" + servo_name + "'.").c_str()); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + else if(!m_tracking_servos.count(servo_name)) + { + ex.setReason(("Servo '" + servo_name + "' is not a tracking servo.").c_str()); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + else if(!m_current_tracking_servos.count(servo_name)) + { + ex.setReason(("Servo '" + servo_name + "' not in use with the current configuration.").c_str()); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + SRTProgramTrackMinorServo_ptr servo = m_current_tracking_servos.at(servo_name); + servo->getAxesInfo(servo_axes_names, servo_axes_units); + + size_t axis_index; + for(axis_index = 0; axis_index < servo_axes_names->length(); axis_index++) + { + if(std::string(servo_axes_names[axis_index]) == axis_name) + { + break; + } + } + + if(axis_index == servo_axes_names->length()) + { + ex.setReason(("Axis '" + axis_code + "' not found.").c_str()); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + new_scan.axis_index = axis_index; + + ACS::doubleSeq starting_position = *servo->calcCoordinates(new_scan.starting_elevation); + ACS::doubleSeq final_position = *servo->calcCoordinates(new_scan.starting_elevation); + // Don't be fooled by the name, starting position now still holds the central scan position + new_scan.central_position = starting_position[axis_index]; + + starting_position[axis_index] -= scan_info.range / 2; + final_position[axis_index] += scan_info.range / 2; + + /** + * Note, we perform the calculations with some degree of approximation, + * we know the expected elevation at the start of the scan, + * but the true elevation value is going to be known only inside the startScan, + * depending on how much time the minor servos take to get to position + * TBH, I don't really know if this will really matter, since we will be starting + * the movement of the SRP far from the peak of the gaussian. + * We will check this behavior later. + * In any case, the scan will simply add an offset to the elevation tracking expected position, + * the offset value depends on the time elapsed since the start of the scan, which, + * in the case of the focusScan, it's probably always decided by the minor servo system + */ + ACS::doubleSeq_var min_ranges, max_ranges; + servo->getAxesRanges(min_ranges, max_ranges); + + // Read the servo offsets + ACS::doubleSeq user_offsets = *servo->getUserOffsets(); + ACS::doubleSeq system_offsets = *servo->getSystemOffsets(); + + // Check if starting or final positions are outside the axes range (considering offsets) + for(size_t i = 0; i < starting_position.length(); i++) + { + if(starting_position[i] + user_offsets[i] + system_offsets[i] < min_ranges[i] || starting_position[i] + user_offsets[i] + system_offsets[i] > max_ranges[i]) + { + ex.setReason("Starting position out of range."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + else if(final_position[i] + user_offsets[i] + system_offsets[i] < min_ranges[i] || final_position[i] + user_offsets[i] + system_offsets[i] > max_ranges[i]) + { + ex.setReason("Final position out of range."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + } + + // Time to reach the desired position if I were to start the movement now + ACS::Time min_starting_time = getTimeStamp() + servo->getTravelTime(ACS::doubleSeq(), starting_position) + PROGRAM_TRACK_FUTURE_TIME; + + if(start_time != 0 && min_starting_time > start_time) + { + ex.setReason("Not enough time to start the scan."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + else if(servo->getTravelTime(starting_position, final_position) > scan_info.total_time) // Check scan total time + { + ex.setReason("Not enough time to perform the scan."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + new_scan.start_time = min_starting_time; + return new_scan; +} + +bool SRTMinorServoBossCore::checkScan(const ACS::Time start_time, const MinorServoScan& scan_info, const Antenna::TRunTimeParameters& antenna_info, TRunTimeParameters_out ms_parameters) +{ + AUTO_TRACE("SRTMinorServoBossCore::checkScan()"); + + TRunTimeParameters_var ms_param_var = new TRunTimeParameters; + ms_param_var->onTheFly = !scan_info.is_empty_scan; + ms_param_var->scanAxis = CORBA::string_dup(scan_info.axis_code); + ms_param_var->startEpoch = 0; + ms_param_var->centerScan = 0; + ms_param_var->timeToStop = 0; + + try + { + checkLineStatus(); + } + catch(MinorServoErrors::MinorServoErrorsEx& ex) + { + // Something is not working, return ASAP + ACS_SHORT_LOG((LM_ERROR, ex.errorTrace.routine)); + ms_parameters = ms_param_var._retn(); + return false; + } + + // Check if we are already performing another scan + if(m_scan_active.load() == Management::MNG_TRUE) + { + ACS_LOG(LM_FULL_INFO, "SRTMinorServoBossCore::checkScan()", (LM_CRITICAL, "The system is waiting for a scan to be completed.")); + ms_parameters = ms_param_var._retn(); + return false; + } + + /** + * Empty scan, the old implementation always returned true + * Given the fact that we start the elevation tracking thread automatically + * and considering we will not have to move any axis because of the empty scan + * it is safe to return true in any situation. + * If the system is not configured this will not stop the antenna from starting to track a source + * If the system is about to be configured we will start tracking soon anyway + * The tracking flag will be set to true only when we are inside the tracking delta + * The change in status will be notified via the NotificationChannel + */ + if(scan_info.is_empty_scan) + { + ms_parameters = ms_param_var._retn(); + return true; + } + + // We need to be ready here, we cannot wait for a setup procedure to complete + // Check if the system is now configured and/or tracking + SRTMinorServoMotionStatus motion_status = m_motion_status.load(); + if(motion_status != MOTION_STATUS_TRACKING && motion_status != MOTION_STATUS_CONFIGURED) + { + ACS_LOG(LM_FULL_INFO, "SRTMinorServoBossCore::checkScan()", (LM_CRITICAL, "The system is not ready yet.")); + ms_parameters = ms_param_var._retn(); + return false; + } + + SRTMinorServoScan scan; + try + { + scan = checkScanFeasibility(start_time, scan_info, antenna_info); + } + catch(MinorServoErrors::MinorServoErrorsEx& ex) + { + ACS_SHORT_LOG((LM_ERROR, ex.errorTrace.routine)); + ms_parameters = ms_param_var._retn(); + return false; + } + + ms_param_var->startEpoch = scan.start_time; + ms_param_var->centerScan = scan.central_position; + ms_param_var->timeToStop = ms_param_var->startEpoch + scan_info.total_time; + ms_parameters = ms_param_var._retn(); + return true; +} + +void SRTMinorServoBossCore::startScan(ACS::Time& start_time, const MinorServoScan& scan_info, const Antenna::TRunTimeParameters& antenna_info) +{ + AUTO_TRACE("SRTMinorServoBossCore::startScan()"); + + // This will throw if anything is not working, no need to catch it since we will throw it again anyway + checkLineStatus(); + + // Check if we are already performing another scan + if(m_scan_active.load() == Management::MNG_TRUE) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::startScan()"); + ex.setReason("The system is waiting for a scan to be completed."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + // Empty scan, OK in any case + if(scan_info.is_empty_scan) + { + start_time = getTimeStamp(); + return; + } + + // Check again, this should never block, otherwise something weird is happening + SRTMinorServoMotionStatus motion_status = m_motion_status.load(); + if(motion_status != MOTION_STATUS_TRACKING && motion_status != MOTION_STATUS_CONFIGURED) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::startScan()"); + ex.setReason("The system is not ready yet."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + // This will throw if anything is not working, no need to catch it since we will throw it again anyway + SRTMinorServoScan scan = checkScanFeasibility(start_time, scan_info, antenna_info); + + stopThread(m_tracking_thread); + + // If we got here we are safe to start the scan + m_current_scan = scan; + start_time = scan.start_time; + m_scan_active.store(Management::MNG_TRUE); + startThread(m_scan_thread); +} + +void SRTMinorServoBossCore::closeScan(ACS::Time& close_time) +{ + AUTO_TRACE("SRTMinorServoBossCore::closeScan()"); + + checkLineStatus(); + + if(m_scan_active.load() != Management::MNG_TRUE) + { + // Not scanning, or scan just stopped and the servo is still going back to the initial position + close_time = std::max(getTimeStamp(), m_last_scan.close_time); + return; + } + + // If we got here the scan was not complete, we need to update some values + // Update the scan_duration value. The scan thread will read the value and update the scan center point accordingly. + m_current_scan.scan_duration = getTimeStamp() - m_current_scan.start_time; + + // The thread might have already stopped, but we don't care + stopThread(m_scan_thread); + + // At this point the thread must have stopped, we waited at most 200ms for it to finish its execution, so we can safely assume it's done + // The thread also have updated m_last_scan with the final values + + // Set the close_time + close_time = std::max(getTimeStamp(), m_last_scan.close_time); + ACS_LOG(LM_FULL_INFO, "SRTMinorServoBossCore::closeScan()", (LM_NOTICE, "Scan closed.")); +} + +double SRTMinorServoBossCore::getElevation(const ACS::Time& acs_time) +{ + AUTO_TRACE("SRTMinorServoBossCore::getElevation()"); + + double azimuth, elevation; + + try + { + m_antennaBoss->getRawCoordinates(acs_time, azimuth, elevation); + elevation *= DR2D; + return elevation == 0.0 ? 45.0 : elevation; + } + catch(ComponentErrors::CouldntGetComponentExImpl& ex) + { + ex.addData("Reason", "Cannot get the ANTENNA/Boss component"); + throw ex.getComponentErrorsEx(); + } +} + +void SRTMinorServoBossCore::checkLineStatus() +{ + AUTO_TRACE("SRTMinorServoBossCore::checkLineStatus()"); + + if(!m_socket.isConnected()) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::checkLineStatus()"); + ex.setReason("Socket not connected."); + ex.log(LM_DEBUG); + setFailure(); + throw ex.getMinorServoErrorsEx(); + } + + if(m_status.getControl() != CONTROL_DISCOS) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::checkLineStatus()"); + ex.setReason("MinorServo system is not controlled by DISCOS."); + ex.log(LM_DEBUG); + setFailure(); + throw ex.getMinorServoErrorsEx(); + } + + if(m_status.emergencyPressed() == Management::MNG_TRUE) + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossCore::checkLineStatus()"); + ex.setReason("MinorServo system in emergency status."); + ex.log(LM_DEBUG); + setFailure(); + throw ex.getMinorServoErrorsEx(); + } +} + +template >> +void SRTMinorServoBossCore::startThread(T*& thread, const ACS::TimeInterval& sleep_time) +{ + AUTO_TRACE("SRTMinorServoBossCore::startThread()"); + + try + { + if(thread != nullptr) + { + // We don't want to restart already active threads + if(!thread->isAlive()) + { + if(sleep_time != 0) + { + thread->setSleepTime(sleep_time); + } + thread->restart(); + } + } + else + { + thread = m_component.getContainerServices()->getThreadManager()->create(T::c_thread_name, *this); + if(sleep_time != 0) + { + thread->setSleepTime(sleep_time); + } + thread->resume(); + } + } + catch(acsthreadErrType::CanNotSpawnThreadExImpl& impl) + { + // The thread failed to start for some reason + _ADD_BACKTRACE(ComponentErrors::CanNotStartThreadExImpl, ex, impl, "SRTMinorServoBossCore::startThread()"); + ex.setThreadName(T::c_thread_name); + ex.log(LM_DEBUG); + setFailure(); + throw ex.getComponentErrorsEx(); + } +} + +template >> +void SRTMinorServoBossCore::stopThread(T*& thread) +{ + AUTO_TRACE("SRTMinorServoBossCore::stopThread()"); + + if(thread != nullptr && thread->isAlive()) + { + thread->stop(); + } +} + +template >> +void SRTMinorServoBossCore::destroyThread(T*& thread) +{ + AUTO_TRACE("SRTMinorServoBossCore::destroyThread()"); + + if(thread != nullptr) + { + thread->terminate(); + m_component.getContainerServices()->getThreadManager()->destroy(thread); + } + + ACS_LOG(LM_FULL_INFO, "SRTMinorServoBossCore::destroyThread()", (LM_NOTICE, (std::string(T::c_thread_name) + " destroyed.").c_str())); +} + +void SRTMinorServoBossCore::setFailure() +{ + AUTO_TRACE("SRTMinorServoBossCore::setFailure()"); + + m_subsystem_status.store(Management::MNG_FAILURE); + m_ready.store(Management::MNG_FALSE); + m_elevation_tracking.store(Management::MNG_FALSE); + m_starting.store(Management::MNG_FALSE); + m_scan_active.store(Management::MNG_FALSE); + m_scanning.store(Management::MNG_FALSE); + m_tracking.store(Management::MNG_FALSE); + m_motion_status.store(MOTION_STATUS_ERROR); +} + +Management::TBoolean SRTMinorServoBossCore::getCDBConfiguration(std::string which_configuration) +{ + AUTO_TRACE("SRTMinorServoBossCore::getCDBConfiguration()"); + std::string configuration = getCDBValue(m_component.getContainerServices(), which_configuration); + std::transform(configuration.begin(), configuration.end(), configuration.begin(), ::toupper); + + if(configuration != "ON" && configuration != "OFF") + { + _EXCPT(ComponentErrors::CDBAccessExImpl, ex, "SRTMinorServoBossCore::getCDBConfiguration()"); + ex.setFieldName(which_configuration.c_str()); + ex.addData("Reason", "Value should be 'ON' or 'OFF'"); + ex.log(LM_DEBUG); + throw ex.getComponentErrorsEx(); + } + + return configuration == "ON" ? Management::MNG_TRUE : Management::MNG_FALSE; +} diff --git a/SRT/Servers/SRTMinorServo/src/SRTMinorServoBossImpl.cpp b/SRT/Servers/SRTMinorServo/src/SRTMinorServoBossImpl.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8a8a55374147de8133418ee4e50b2c87ea0e74e5 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/src/SRTMinorServoBossImpl.cpp @@ -0,0 +1,422 @@ +#include "SRTMinorServoBossImpl.h" + +using namespace MinorServo; +namespace SP = SimpleParser; + +_IRA_LOGFILTER_DECLARE; + +SRTMinorServoBossImpl::SRTMinorServoBossImpl(const ACE_CString& component_name, maci::ContainerServices* container_services) : + CharacteristicComponentImpl(component_name, container_services), + m_component_name(std::string(component_name.c_str())), + m_core_ptr(std::make_shared(*this)), + m_core(*m_core_ptr), + m_parser(SP::CParser(this, 2)), + m_connected_ptr(this), + m_status_ptr(this), + m_ready_ptr(this), + m_actual_setup_ptr(this), + m_motion_info_ptr(this), + m_starting_ptr(this), + m_as_configuration_ptr(this), + m_elevation_tracking_ptr(this), + m_scan_active_ptr(this), + m_scanning_ptr(this), + m_tracking_ptr(this), + m_current_configuration_ptr(this), + m_simulation_enabled_ptr(this), + m_plc_time_ptr(this), + m_plc_version_ptr(this), + m_control_ptr(this), + m_power_ptr(this), + m_emergency_ptr(this), + m_gregorian_cover_ptr(this), + m_last_executed_command_ptr(this) +{ + AUTO_TRACE("SRTMinorServoBossImpl::SRTMinorServoBossImpl()"); + + ACS_LOG(LM_FULL_INFO, "SRTMinorServoBossImpl::SRTMinorServoBossImpl()", (LM_NOTICE, "COMPONENT CREATED")); +} + +SRTMinorServoBossImpl::~SRTMinorServoBossImpl() +{ + AUTO_TRACE("SRTMinorServoBossImpl::~SRTMinorServoBossImpl()"); + + ACS_LOG(LM_FULL_INFO, "SRTMinorServoBossImpl::SRTMinorServoBossImpl()", (LM_NOTICE, "COMPONENT DESTROYED")); +} + +void SRTMinorServoBossImpl::initialize() +{ + AUTO_TRACE("SRTMinorServoBossImpl::initialize()"); + _IRA_LOGFILTER_ACTIVATE(10000000, 20000000); + + try + { + m_connected_ptr = new ROEnumImpl((m_component_name + ":connected").c_str(), getComponent(), + new MSGenericDevIO>(m_core.m_socket_connected), true); + m_status_ptr = new ROEnumImpl((m_component_name + ":status").c_str(), getComponent(), + new MSGenericDevIO>(m_core.m_subsystem_status), true); + m_ready_ptr = new ROEnumImpl((m_component_name + ":ready").c_str(), getComponent(), + new MSGenericDevIO>(m_core.m_ready), true); + m_actual_setup_ptr = new baci::ROstring((m_component_name + ":actualSetup").c_str(), getComponent(), + new MSGenericDevIO(m_core.m_actual_setup), true); + m_motion_info_ptr = new baci::ROstring((m_component_name + ":motionInfo").c_str(), getComponent(), + new MSMotionInfoDevIO(m_core.m_motion_status, m_core.m_status, m_core.m_scanning, m_core.m_current_scan), true); + m_starting_ptr = new ROEnumImpl((m_component_name + ":starting").c_str(), getComponent(), + new MSGenericDevIO>(m_core.m_starting), true); + m_as_configuration_ptr = new ROEnumImpl((m_component_name + ":asConfiguration").c_str(), getComponent(), + new MSGenericDevIO>(m_core.m_as_configuration), true); + m_elevation_tracking_ptr = new ROEnumImpl((m_component_name + ":elevationTrack").c_str(), getComponent(), + new MSGenericDevIO>(m_core.m_elevation_tracking_enabled), true); + m_scan_active_ptr = new ROEnumImpl((m_component_name + ":scanActive").c_str(), getComponent(), + new MSGenericDevIO>(m_core.m_scan_active), true); + m_scanning_ptr = new ROEnumImpl((m_component_name + ":scanning").c_str(), getComponent(), + new MSGenericDevIO>(m_core.m_scanning), true); + m_tracking_ptr = new ROEnumImpl((m_component_name + ":tracking").c_str(), getComponent(), + new MSGenericDevIO>(m_core.m_tracking), true); + m_current_configuration_ptr = new ROEnumImpl((m_component_name + ":current_configuration").c_str(), getComponent(), new MSAnswerMapDevIO("current_configuration", m_core.m_status, &SRTMinorServoGeneralStatus::getFocalConfiguration), true); + m_simulation_enabled_ptr = new ROEnumImpl((m_component_name + ":simulation_enabled").c_str(), getComponent(), + new MSAnswerMapDevIO("simulation_enabled", m_core.m_status, &SRTMinorServoGeneralStatus::isSimulationEnabled), true); + m_plc_time_ptr = new baci::ROdouble((m_component_name + ":plc_time").c_str(), getComponent(), + new MSAnswerMapDevIO("plc_time", m_core.m_status, &SRTMinorServoGeneralStatus::getPLCTime), true); + m_plc_version_ptr = new baci::ROstring((m_component_name + ":plc_version").c_str(), getComponent(), + new MSAnswerMapDevIO("plc_version", m_core.m_status, &SRTMinorServoGeneralStatus::getPLCVersion), true); + m_control_ptr = new ROEnumImpl((m_component_name + ":control").c_str(), getComponent(), + new MSAnswerMapDevIO("control", m_core.m_status, &SRTMinorServoGeneralStatus::getControl), true); + m_power_ptr = new ROEnumImpl((m_component_name + ":power").c_str(), getComponent(), + new MSAnswerMapDevIO("power", m_core.m_status, &SRTMinorServoGeneralStatus::hasPower), true); + m_emergency_ptr = new ROEnumImpl((m_component_name + ":emergency").c_str(), getComponent(), + new MSAnswerMapDevIO("emergency", m_core.m_status, &SRTMinorServoGeneralStatus::emergencyPressed), true); + m_gregorian_cover_ptr = new ROEnumImpl((m_component_name + ":gregorian_cover").c_str(), getComponent(), new MSAnswerMapDevIO("gregorian_cover", m_core.m_status, &SRTMinorServoGeneralStatus::getGregorianCoverPosition), true); + m_last_executed_command_ptr = new baci::ROdouble((m_component_name + ":last_executed_command").c_str(), getComponent(), + new MSAnswerMapDevIO("last_executed_command", m_core.m_status, &SRTMinorServoGeneralStatus::getLastExecutedCommand), true); + } + catch(std::bad_alloc& ba) + { + _EXCPT(ComponentErrors::MemoryAllocationExImpl, ex, "SRTMinorServoBossImpl::initialize()"); + ex.log(LM_DEBUG); + throw ex.getComponentErrorsEx(); + } + + ACS_LOG(LM_FULL_INFO, "SRTMinorServoBossImpl::initialize()", (LM_NOTICE, "PROPERTIES INITIALIZED")); + + m_parser.add("servoSetup", new SP::function1>(this, &SRTMinorServoBossImpl::setup), 1); + m_parser.add("servoPark", new SP::function0(this, &SRTMinorServoBossImpl::park), 0); + m_parser.add("setServoElevationTracking", new SP::function1>(this, &SRTMinorServoBossImpl::setElevationTracking), 1); + m_parser.add("setServoASConfiguration", new SP::function1>(this, &SRTMinorServoBossImpl::setASConfiguration), 1); + m_parser.add("setServoOffset", new SP::function2, SP::I>(this, &SRTMinorServoBossImpl::setUserOffset), 2); + m_parser.add("clearServoOffsets", new SP::function0(this, &SRTMinorServoBossImpl::clearOffsets), 0); + //m_parser.add("setGregorianCoverPosition", new SP::function1>(this, &SRTMinorServoBossImpl::setGregorianCoverPosition), 1); + + ACS_LOG(LM_FULL_INFO, "SRTMinorServoBossImpl::initialize()", (LM_NOTICE, "PARSER INITIALIZED")); + + ACS_LOG(LM_FULL_INFO, "SRTMinorServoBossImpl::initialize()", (LM_NOTICE, "COMPONENT INITIALIZED")); +} + +void SRTMinorServoBossImpl::execute() +{ + AUTO_TRACE("SRTMinorServoBossImpl::execute()"); + + ACS_LOG(LM_FULL_INFO, "SRTMinorServoBossImpl::execute()", (LM_NOTICE, "STARTING TO ACCEPT FUNCTIONAL CALLS")); +} + +void SRTMinorServoBossImpl::cleanUp() +{ + AUTO_TRACE("SRTMinorServoBossImpl::cleanUp()"); + ACS_LOG(LM_FULL_INFO, "SRTMinorServoBossImpl::cleanUp()", (LM_NOTICE, "CLEANING UP RESOURCES")); + stopPropertiesMonitoring(); + _IRA_LOGFILTER_FLUSH; + _IRA_LOGFILTER_DESTROY; + CharacteristicComponentImpl::cleanUp(); +} + +void SRTMinorServoBossImpl::aboutToAbort() +{ + AUTO_TRACE("SRTMinorServoBossImpl::aboutToAbort()"); + ACS_LOG(LM_FULL_INFO, "SRTMinorServoBossImpl::aboutToAbort()", (LM_NOTICE, "ATTEMPTING TO CLEAN UP RESOURCES")); + stopPropertiesMonitoring(); + _IRA_LOGFILTER_FLUSH; + _IRA_LOGFILTER_DESTROY; + CharacteristicComponentImpl::aboutToAbort(); +} + +void SRTMinorServoBossImpl::setup(const char* configuration) +{ + AUTO_TRACE("SRTMinorServoBossImpl::setup()"); + m_core.setup(std::string(configuration)); +} + +void SRTMinorServoBossImpl::park() +{ + AUTO_TRACE("SRTMinorServoBossImpl::park()"); + m_core.park(); +} + +CORBA::Boolean SRTMinorServoBossImpl::isElevationTrackingEn() +{ + AUTO_TRACE("SRTMinorServoBossImpl::isElevationTrackingEn()"); + return m_core.m_elevation_tracking_enabled.load() == Management::MNG_TRUE ? true : false; +} + +CORBA::Boolean SRTMinorServoBossImpl::isElevationTracking() +{ + AUTO_TRACE("SRTMinorServoBossImpl::isElevationTracking()"); + return m_core.m_elevation_tracking.load() == Management::MNG_TRUE ? true : false; +} + +CORBA::Boolean SRTMinorServoBossImpl::isTracking() +{ + AUTO_TRACE("SRTMinorServoBossImpl::isTracking()"); + return m_core.m_tracking.load() == Management::MNG_TRUE ? true : false; +} + +CORBA::Boolean SRTMinorServoBossImpl::isStarting() +{ + AUTO_TRACE("SRTMinorServoBossImpl::isStarting()"); + return m_core.m_starting.load() == Management::MNG_TRUE ? true : false; +} + +CORBA::Boolean SRTMinorServoBossImpl::isASConfiguration() +{ + AUTO_TRACE("SRTMinorServoBossImpl::isASConfiguration()"); + return m_core.m_as_configuration.load() == Management::MNG_TRUE ? true : false; +} + +CORBA::Boolean SRTMinorServoBossImpl::isParking() +{ + AUTO_TRACE("SRTMinorServoBossImpl::isParking()"); + return (m_core.m_starting.load() == Management::MNG_TRUE && m_core.m_commanded_configuration.load() == CONFIGURATION_PARK); +} + +CORBA::Boolean SRTMinorServoBossImpl::isReady() +{ + AUTO_TRACE("SRTMinorServoBossImpl::isReady()"); + return m_core.m_ready.load() == Management::MNG_TRUE ? true : false; +} + +CORBA::Boolean SRTMinorServoBossImpl::isScanning() +{ + AUTO_TRACE("SRTMinorServoBossImpl::isScanning()"); + return m_core.m_scanning.load() == Management::MNG_TRUE ? true : false; +} + +CORBA::Boolean SRTMinorServoBossImpl::isScanActive() +{ + AUTO_TRACE("SRTMinorServoBossImpl::isScanActive()"); + return m_core.m_scan_active.load() == Management::MNG_TRUE ? true : false; +} + +char* SRTMinorServoBossImpl::getActualSetup() +{ + AUTO_TRACE("SRTMinorServoBossImpl::getActualSetup()"); + return CORBA::string_dup(m_core.m_actual_setup.c_str()); +} + +char* SRTMinorServoBossImpl::getCommandedSetup() +{ + AUTO_TRACE("SRTMinorServoBossImpl::getCommandedSetup()"); + return CORBA::string_dup(m_core.m_commanded_setup.c_str()); +} + +CORBA::Double SRTMinorServoBossImpl::getCentralScanPosition() +{ + AUTO_TRACE("SRTMinorServoBossImpl::getCentralScanPosition()"); + + if(m_core.m_scan_active.load() == Management::MNG_TRUE) + { + return CORBA::Double(m_core.m_current_scan.central_position); + } + else if(m_core.m_last_scan.servo_name != "") // We are not scanning now, but we performed a scan previously + { + return CORBA::Double(m_core.m_last_scan.central_position); + } + else + { + _EXCPT(MinorServoErrors::StatusErrorExImpl, ex, "SRTMinorServoBossImpl::getCentralPosition()"); + ex.setReason("No scan has been performed yet."); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } +} + +void SRTMinorServoBossImpl::clearOffsets() +{ + AUTO_TRACE("SRTMinorServoBossImpl::clearOffsets()"); + ACS_LOG(LM_FULL_INFO, "clearServoOffsets", (LM_NOTICE, "CLEARING ALL USER OFFSETS")); + m_core.clearUserOffsets("ALL"); +} + +void SRTMinorServoBossImpl::clearUserOffset(const char* servo_name) +{ + AUTO_TRACE("SRTMinorServoBossImpl::clearUserOffset()"); + m_core.clearUserOffsets(std::string(servo_name)); +} + +void SRTMinorServoBossImpl::setUserOffset(const char* servo_axis_name, CORBA::Double offset) +{ + AUTO_TRACE("SRTMinorServoBossImpl::setUserOffset()"); + m_core.setUserOffset(std::string(servo_axis_name), (double)offset); +} + +void SRTMinorServoBossImpl::setUserOffset(const char* servo_axis_name, const double& offset) +{ + AUTO_TRACE("SRTMinorServoBossImpl::setUserOffset()"); + m_core.setUserOffset(std::string(servo_axis_name), (double)offset, true); +} + +ACS::doubleSeq* SRTMinorServoBossImpl::getUserOffset() +{ + AUTO_TRACE("SRTMinorServoBossImpl::getUserOffset()"); + return m_core.getUserOffsets(); +} + +void SRTMinorServoBossImpl::clearSystemOffset(const char* servo_name) +{ + AUTO_TRACE("SRTMinorServoBossImpl::clearSystemOffset()"); + m_core.clearSystemOffsets(std::string(servo_name)); +} + +void SRTMinorServoBossImpl::setSystemOffset(const char* servo_axis_name, CORBA::Double offset) +{ + AUTO_TRACE("SRTMinorServoBossImpl::setSystemOffset()"); + m_core.setSystemOffset(std::string(servo_axis_name), (double)offset); +} + +ACS::doubleSeq* SRTMinorServoBossImpl::getSystemOffset() +{ + AUTO_TRACE("SRTMinorServoBossImpl::getSystemOffset()"); + return m_core.getSystemOffsets(); +} + +void SRTMinorServoBossImpl::getAxesInfo(ACS::stringSeq_out axes_names, ACS::stringSeq_out axes_units) +{ + AUTO_TRACE("SRTMinorServoBossImpl::getAxesInfo()"); + m_core.getAxesInfo(axes_names, axes_units); +} + +char* SRTMinorServoBossImpl::getScanAxis() +{ + AUTO_TRACE("SRTMinorServoBossImpl::getScanAxis()"); + + if(m_core.m_scan_active.load() == Management::MNG_TRUE) + { + return CORBA::string_dup((m_core.m_current_scan.servo_name + "_" + m_core.m_current_scan.axis_name).c_str()); + } + else if(!m_core.m_last_scan.servo_name.empty()) + { + return CORBA::string_dup((m_core.m_last_scan.servo_name + "_" + m_core.m_last_scan.axis_name).c_str()); + } + else + { + return CORBA::string_dup(""); + } +} + +ACS::doubleSeq* SRTMinorServoBossImpl::getAxesPosition(ACS::Time acs_time) +{ + AUTO_TRACE("SRTMinorServoBossImpl::getAxesPositions()"); + return m_core.getAxesPositions(acs_time == 0 ? getTimeStamp() : acs_time); +} + +void SRTMinorServoBossImpl::setElevationTracking(const char* configuration) +{ + AUTO_TRACE("SRTMinorServoBossImpl::setElevationTracking()"); + m_core.setElevationTracking(std::string(configuration)); +} + +void SRTMinorServoBossImpl::setASConfiguration(const char* configuration) +{ + AUTO_TRACE("SRTMinorServoBossImpl::setASConfiguration()"); + m_core.setASConfiguration(std::string(configuration)); +} + +void SRTMinorServoBossImpl::setGregorianCoverPosition(const char* position) +{ + AUTO_TRACE("SRTMinorServoBossImpl::setGregorianCoverPosition()"); + m_core.setGregorianCoverPosition(std::string(position)); +} + +CORBA::Boolean SRTMinorServoBossImpl::checkScan(const ACS::Time start_time, const MinorServoScan& scan_info, const Antenna::TRunTimeParameters& antenna_info, TRunTimeParameters_out ms_parameters) +{ + AUTO_TRACE("SRTMinorServoBossImpl::checkScan()"); + return m_core.checkScan(start_time, scan_info, antenna_info, ms_parameters); +} + +void SRTMinorServoBossImpl::startScan(ACS::Time& start_time, const MinorServoScan& scan_info, const Antenna::TRunTimeParameters& antenna_info) +{ + AUTO_TRACE("SRTMinorServoBossImpl::startScan()"); + m_core.startScan(start_time, scan_info, antenna_info); +} + +void SRTMinorServoBossImpl::closeScan(ACS::Time& close_time) +{ + AUTO_TRACE("SRTMinorServoBossImpl::closeScan()"); + m_core.closeScan(close_time); +} + +void SRTMinorServoBossImpl::preset(double elevation) +{ + AUTO_TRACE("SRTMinorServoBossImpl::preset()"); + m_core.preset(elevation); +} + +CORBA::Boolean SRTMinorServoBossImpl::command(const char* cmd, CORBA::String_out answer) +{ + AUTO_TRACE("SRTMinorServoBossImpl::command()"); + + IRA::CString out; + bool res = false; + + try + { + m_parser.run(cmd, out); + res = true; + } + catch(MinorServoErrors::MinorServoErrorsEx& ex) + { + LOG_EX(MinorServoErrors::MinorServoErrorsEx); + } + catch(ComponentErrors::ComponentErrorsEx& ex) + { + LOG_EX(ComponentErrors::ComponentErrorsEx); + } + catch(ManagementErrors::ConfigurationErrorEx& ex) + { + LOG_EX(ManagementErrors::ConfigurationErrorEx); + } + catch(ManagementErrors::ParkingErrorEx& ex) + { + LOG_EX(ManagementErrors::ParkingErrorEx); + } + catch(...) // Unknown exception. If the above catch blocks are written correctly we should never get here. + { + ACS_SHORT_LOG((LM_ERROR, "SRTMinorServoBossImpl::command()")); + } + + answer = CORBA::string_dup((const char *)out); + return res; +} + + +GET_PROPERTY_REFERENCE(Management::ROTSystemStatus, SRTMinorServoBossImpl, m_status_ptr, status) +GET_PROPERTY_REFERENCE(Management::ROTBoolean, SRTMinorServoBossImpl, m_ready_ptr, ready) +GET_PROPERTY_REFERENCE(ACS::ROstring, SRTMinorServoBossImpl, m_actual_setup_ptr, actualSetup) +GET_PROPERTY_REFERENCE(ACS::ROstring, SRTMinorServoBossImpl, m_motion_info_ptr, motionInfo) +GET_PROPERTY_REFERENCE(Management::ROTBoolean, SRTMinorServoBossImpl, m_starting_ptr, starting) +GET_PROPERTY_REFERENCE(Management::ROTBoolean, SRTMinorServoBossImpl, m_as_configuration_ptr, asConfiguration) +GET_PROPERTY_REFERENCE(Management::ROTBoolean, SRTMinorServoBossImpl, m_elevation_tracking_ptr, elevationTrack) +GET_PROPERTY_REFERENCE(Management::ROTBoolean, SRTMinorServoBossImpl, m_scan_active_ptr, scanActive) +GET_PROPERTY_REFERENCE(Management::ROTBoolean, SRTMinorServoBossImpl, m_scanning_ptr, scanning) +GET_PROPERTY_REFERENCE(Management::ROTBoolean, SRTMinorServoBossImpl, m_tracking_ptr, tracking) +GET_PROPERTY_REFERENCE(Management::ROTBoolean, SRTMinorServoBossImpl, m_connected_ptr, connected) +GET_PROPERTY_REFERENCE(ROSRTMinorServoFocalConfiguration, SRTMinorServoBossImpl, m_current_configuration_ptr, current_configuration); +GET_PROPERTY_REFERENCE(Management::ROTBoolean, SRTMinorServoBossImpl, m_simulation_enabled_ptr, simulation_enabled); +GET_PROPERTY_REFERENCE(ACS::ROdouble, SRTMinorServoBossImpl, m_plc_time_ptr, plc_time); +GET_PROPERTY_REFERENCE(ACS::ROstring, SRTMinorServoBossImpl, m_plc_version_ptr, plc_version); +GET_PROPERTY_REFERENCE(ROSRTMinorServoControlStatus, SRTMinorServoBossImpl, m_control_ptr, control); +GET_PROPERTY_REFERENCE(Management::ROTBoolean, SRTMinorServoBossImpl, m_power_ptr, power); +GET_PROPERTY_REFERENCE(Management::ROTBoolean, SRTMinorServoBossImpl, m_emergency_ptr, emergency); +GET_PROPERTY_REFERENCE(ROSRTMinorServoGregorianCoverStatus, SRTMinorServoBossImpl, m_gregorian_cover_ptr, gregorian_cover); +GET_PROPERTY_REFERENCE(ACS::ROdouble, SRTMinorServoBossImpl, m_last_executed_command_ptr, last_executed_command); + +#include +MACI_DLL_SUPPORT_FUNCTIONS(SRTMinorServoBossImpl) diff --git a/SRT/Servers/SRTMinorServo/src/SRTMinorServoParkThread.cpp b/SRT/Servers/SRTMinorServo/src/SRTMinorServoParkThread.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8afaab79f449b131b30d2e994fd85f13ac377114 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/src/SRTMinorServoParkThread.cpp @@ -0,0 +1,93 @@ +#include "SRTMinorServoParkThread.h" + +using namespace MinorServo; + +SRTMinorServoParkThread::SRTMinorServoParkThread(const ACE_CString& name, SRTMinorServoBossCore& core, const ACS::TimeInterval& response_time, const ACS::TimeInterval& sleep_time) : + ACS::Thread(name, response_time, sleep_time), + m_core(core) +{ + AUTO_TRACE("SRTMinorServoParkThread::SRTMinorServoParkThread()"); +} + +SRTMinorServoParkThread::~SRTMinorServoParkThread() +{ + AUTO_TRACE("SRTMinorServoParkThread::~SRTMinorServoParkThread()"); +} + +void SRTMinorServoParkThread::onStart() +{ + AUTO_TRACE("SRTMinorServoParkThread::onStart()"); + this->setSleepTime(500000); // 50 milliseconds + m_start_time = IRA::CIRATools::getUNIXEpoch(); + + m_status = 0; + + ACS_LOG(LM_FULL_INFO, "SRTMinorServoParkThread::onStart()", (LM_NOTICE, "PARK THREAD STARTED")); +} + +void SRTMinorServoParkThread::onStop() +{ + AUTO_TRACE("SRTMinorServoParkThread::onStop()"); + + ACS_LOG(LM_FULL_INFO, "SRTMinorServoParkThread::onStop()", (LM_NOTICE, "PARK THREAD STOPPED")); +} + +void SRTMinorServoParkThread::runLoop() +{ + AUTO_TRACE("SRTMinorServoParkThread::runLoop()"); + + try + { + m_core.checkLineStatus(); + } + catch(MinorServoErrors::MinorServoErrorsEx& ex) + { + ACS_SHORT_LOG((LM_ERROR, ex.errorTrace.routine)); + m_core.setFailure(); + this->setStopped(); + return; + } + + if(IRA::CIRATools::getUNIXEpoch() - m_start_time >= PARK_TIMEOUT) + { + ACS_LOG(LM_FULL_INFO, "SRTMinorServoParkThread::runLoop()", (LM_CRITICAL, "Timeout while performing a park operation.")); + m_core.setFailure(); + this->setStopped(); + return; + } + + switch(m_status) + { + case 0: + { + // First we check if the gregorian cover has closed + //bool completed = m_core.m_status.getGregorianCoverPosition() == COVER_STATUS_CLOSED ? true : false; + + // Then we cycle through all the servos and make sure their operative mode is STOP + if(/*completed && */std::all_of(m_core.m_servos.begin(), m_core.m_servos.end(), [](const std::pair& servo) -> bool + { + ACSErr::Completion_var comp; + return servo.second->operative_mode()->get_sync(comp.out()) == OPERATIVE_MODE_STOP ? true : false; + })) + { + m_status = 1; + } + + break; + } + case 1: + { + for(const auto& [name, servo] : m_core.m_servos) + { + servo->setup(""); + } + + m_core.m_actual_setup = m_core.m_commanded_setup; + m_core.m_starting.store(Management::MNG_FALSE); + m_core.m_motion_status.store(MOTION_STATUS_PARKED); + m_core.m_subsystem_status.store(Management::MNG_OK); + this->setStopped(); + break; + } + } +} diff --git a/SRT/Servers/SRTMinorServo/src/SRTMinorServoScanThread.cpp b/SRT/Servers/SRTMinorServo/src/SRTMinorServoScanThread.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a70f667954ee082a1cf978a8ede668526d493b4e --- /dev/null +++ b/SRT/Servers/SRTMinorServo/src/SRTMinorServoScanThread.cpp @@ -0,0 +1,237 @@ +#include "SRTMinorServoScanThread.h" + +using namespace MinorServo; + +SRTMinorServoScanThread::SRTMinorServoScanThread(const ACE_CString& name, SRTMinorServoBossCore& core, const ACS::TimeInterval& response_time, const ACS::TimeInterval& sleep_time): + ACS::Thread(name, response_time, sleep_time), + m_core(core), + m_error(false) +{ + AUTO_TRACE("SRTMinorServoScanThread::SRTMinorServoScanThread()"); +} + +SRTMinorServoScanThread::~SRTMinorServoScanThread() +{ + AUTO_TRACE("SRTMinorServoScanThread::~SRTMinorServoScanThread()"); +} + +void SRTMinorServoScanThread::onStart() +{ + AUTO_TRACE("SRTMinorServoScanThread::onStart()"); + + // We want to start moving the scan servo ASAP and bring its scan axis to the start position. + // The other servos will keep tracking normally. + // The general implementation accounts for derotators which will be integrated in the future. + // The requested scan will start at start_time by adding the correct scan offset to each point. + + // This will find the first useful time to send a point synchronized with the scan start time: m_core.m_current_scan.start_time - (PROGRAM_TRACK_TIMEGAP * N) > now + PROGRAM_TRACK_FUTURE_TIME + size_t pre_scan_points = std::floor((m_core.m_current_scan.start_time - (getTimeStamp() + PROGRAM_TRACK_FUTURE_TIME)) / PROGRAM_TRACK_TIMEGAP); + m_point_time = m_core.m_current_scan.start_time - pre_scan_points * PROGRAM_TRACK_TIMEGAP; + m_point_id = 0; + m_trajectory_id = (unsigned int)(IRA::CIRATools::ACSTime2UNIXEpoch(m_point_time)); + + // How many points in the scan trajectory? + // We don't check the modulo of the division, we assume all the scans duration is always expressed in seconds, therefore the modulo should always be 0 + size_t scan_points = m_core.m_current_scan.scan_duration / PROGRAM_TRACK_TIMEGAP + 1; + // How big is each offset step? We have N points but N-1 segments + double scan_delta = m_core.m_current_scan.scan_range / (scan_points - 1); + + m_scan_offsets = SRTMinorServoPositionsQueue(pre_scan_points + scan_points, 1); + for(size_t i = 0; i < pre_scan_points; i++) + { + m_scan_offsets.put(m_point_time + i * PROGRAM_TRACK_TIMEGAP, { -m_core.m_current_scan.scan_range / 2 }); + } + for(size_t i = 0; i < scan_points; i++) + { + m_scan_offsets.put(m_core.m_current_scan.start_time + i * PROGRAM_TRACK_TIMEGAP, {-m_core.m_current_scan.scan_range / 2 + i * scan_delta}); + } + + // We save the starting coordinates to return to them when the scan is finished + ACSErr::Completion_var comp; + auto servo = m_core.m_tracking_servos.at(m_core.m_current_scan.servo_name); + m_starting_coordinates = *servo->getAxesPositions(0); + + ACS_LOG(LM_FULL_INFO, "SRTMinorServoScanThread::onStart()", (LM_NOTICE, + ("SCAN THREAD STARTED, SCANNING " + std::to_string((int)m_core.m_current_scan.scan_range) + "MM IN " + std::to_string((int)(m_core.m_current_scan.scan_duration / 10000000)) + " SECONDS ALONG " + m_core.m_current_scan.servo_name + " " + m_core.m_current_scan.axis_name + " AXIS").c_str() + )); +} + +void SRTMinorServoScanThread::onStop() +{ + AUTO_TRACE("SRTMinorServoScanThread::onStop()"); + + ACS_LOG(LM_FULL_INFO, "SRTMinorServoScanThread::onStop()", (LM_NOTICE, "SCAN THREAD STOPPED")); + + m_core.m_scanning.store(Management::MNG_FALSE); + + if(m_error) // We didn't reach the end of the scan because of some error + { + // Should we set everything to failure? + m_core.setFailure(); + return; + } + + m_core.m_scanning.store(Management::MNG_FALSE); + m_core.m_elevation_tracking.store(Management::MNG_FALSE); + + // Reset m_point_time + m_point_time -= PROGRAM_TRACK_FUTURE_TIME; + + auto servo = m_core.m_tracking_servos.at(m_core.m_current_scan.servo_name); + + // Update the central scan position using the correct elevation (from the past) + m_core.m_current_scan.central_position = (*servo->calcCoordinates(m_core.getElevation((m_core.m_current_scan.start_time + m_core.m_current_scan.scan_duration) / 2)))[m_core.m_current_scan.axis_index]; + + ACS::doubleSeq return_coordinates; + if(m_core.m_motion_status.load() == MOTION_STATUS_TRACKING) + { + // Restart the tracking + m_core.startThread(m_core.m_tracking_thread); + return_coordinates = *servo->calcCoordinates(m_core.getElevation(getTimeStamp())); + } + else + { + // We were not tracking, send the servo to the starting position + servo->preset(m_starting_coordinates); + return_coordinates = m_starting_coordinates; + } + + // Calculate the close_time again now that the servos are already moving back to their original position + m_core.m_current_scan.close_time = getTimeStamp() + servo->getTravelTime(ACS::doubleSeq(), return_coordinates); + + // Copy current_scan into last_scan + m_core.m_last_scan = m_core.m_current_scan; + + // Finally unlock the scan capabilities + m_core.m_scan_active.store(Management::MNG_FALSE); +} + +void SRTMinorServoScanThread::runLoop() +{ + AUTO_TRACE("SRTMinorServoScanThread::runLoop()"); + + if(m_point_time == m_core.m_current_scan.start_time + PROGRAM_TRACK_FUTURE_TIME) // The system should be starting to scan right now + { + m_core.m_scanning.store(Management::MNG_TRUE); + } + else if(m_point_time == m_core.m_current_scan.start_time + m_core.m_current_scan.scan_duration + PROGRAM_TRACK_FUTURE_TIME) // The system should have finished to scan right now + { + this->setStopped(); + return; + } + else if(m_point_time > m_core.m_current_scan.start_time + m_core.m_current_scan.scan_duration) // All points were sent, we can stop sending but we'll wait for the scan to finish + { + m_point_time += PROGRAM_TRACK_TIMEGAP; + + // Sleep until the next point - PROGRAM_TRACK_FUTURE_TIME + this->setSleepTime((m_point_time - PROGRAM_TRACK_FUTURE_TIME) - getTimeStamp()); + return; + } + + try + { + m_core.checkLineStatus(); + } + catch(MinorServoErrors::MinorServoErrorsEx& ex) + { + ACS_SHORT_LOG((LM_ERROR, ex.errorTrace.routine)); + m_error = true; + this->setStopped(); + return; + } + + if(m_point_id == 1) + { + if(std::all_of(m_core.m_current_tracking_servos.begin(), m_core.m_current_tracking_servos.end(), [this](const std::pair& servo) -> bool + { + if(m_core.m_motion_status.load() == MOTION_STATUS_CONFIGURED && servo.first != m_core.m_current_scan.servo_name) + { + // Not tracking the elevation and this servo is not involved in the current scan + return true; + } + + ACSErr::Completion_var comp; + if(servo.second->operative_mode()->get_sync(comp.out()) != OPERATIVE_MODE_PROGRAMTRACK) + { + ACS_LOG(LM_FULL_INFO, "SRTMinorServoScanThread::runLoop()", (LM_CRITICAL, (servo.first + ": failed to set PROGRAM_TRACK operative mode!").c_str())); + return false; + } + return true; + })) + { + // All used servos are set to PROGRAMTRACK operative mode, we are tracking the elevation + if(m_core.m_motion_status.load() == MOTION_STATUS_TRACKING) + { + m_core.m_elevation_tracking.store(Management::MNG_TRUE); + } + } + else + { + m_error = true; + this->setStopped(); + return; + } + } + + double elevation = m_core.m_current_scan.starting_elevation; + + if(m_core.m_motion_status.load() == MOTION_STATUS_TRACKING) // We are in elevation tracking mode + { + try + { + elevation = m_core.getElevation(m_point_time); + } + catch(ComponentErrors::ComponentErrorsEx& ex) + { + _IRA_LOGFILTER_LOG(LM_WARNING, "SRTMinorServoScanThread::runLoop()", (std::string(getErrorFromEx(ex)) + " Using a fixed elevation of " + std::to_string(elevation) + "° as scan central coordinate!").c_str()); + m_core.m_elevation_tracking.store(Management::MNG_FALSE); + } + } + + for(const auto& [servo_name, servo] : m_core.m_current_tracking_servos) + { + if(m_core.m_motion_status.load() == MOTION_STATUS_CONFIGURED && servo_name != m_core.m_current_scan.servo_name) + { + // Not tracking the elevation and this servo is not involved in the current scan + continue; + } + + ACS::doubleSeq coordinates = m_starting_coordinates; + + if(m_core.m_motion_status.load() == MOTION_STATUS_TRACKING) + { + coordinates = *servo->calcCoordinates(elevation); + } + + if(servo_name == m_core.m_current_scan.servo_name) + { + try + { + // Ask for the offset in the exact m_point_time (we don't want to interpolate or retrieve any value before or after the scan time span) + coordinates[m_core.m_current_scan.axis_index] += m_scan_offsets.get(m_point_time, true).second[0]; + } + catch(...) + { + // No offset found for this point_time, ignore + } + } + + try + { + servo->programTrack(m_trajectory_id, m_point_id, m_point_time, coordinates); + } + catch(MinorServoErrors::MinorServoErrorsEx& ex) + { + ACS_SHORT_LOG((LM_ERROR, ex.errorTrace.routine)); + m_error = true; + this->setStopped(); + return; + } + } + + m_point_id++; + m_point_time += PROGRAM_TRACK_TIMEGAP; + + // Sleep until the next point - PROGRAM_TRACK_FUTURE_TIME + this->setSleepTime((m_point_time - PROGRAM_TRACK_FUTURE_TIME) - getTimeStamp()); +} diff --git a/SRT/Servers/SRTMinorServo/src/SRTMinorServoSetupThread.cpp b/SRT/Servers/SRTMinorServo/src/SRTMinorServoSetupThread.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1daa85968c960ed1b27cf55f589ce06904b01b47 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/src/SRTMinorServoSetupThread.cpp @@ -0,0 +1,255 @@ +#include "SRTMinorServoSetupThread.h" + +using namespace MinorServo; + +SRTMinorServoSetupThread::SRTMinorServoSetupThread(const ACE_CString& name, SRTMinorServoBossCore& core, const ACS::TimeInterval& response_time, const ACS::TimeInterval& sleep_time) : + ACS::Thread(name, response_time, sleep_time), + m_core(core) +{ + AUTO_TRACE("SRTMinorServoSetupThread::SRTMinorServoSetupThread()"); +} + +SRTMinorServoSetupThread::~SRTMinorServoSetupThread() +{ + AUTO_TRACE("SRTMinorServoSetupThread::~SRTMinorServoSetupThread()"); +} + +void SRTMinorServoSetupThread::onStart() +{ + AUTO_TRACE("SRTMinorServoSetupThread::onStart()"); + + this->setSleepTime(500000); // 50 milliseconds + m_start_time = IRA::CIRATools::getUNIXEpoch(); + + m_status = 0; + + //SRTMinorServoFocalConfiguration commanded_configuration = m_core.m_commanded_configuration.load(); + //m_LDO_configuration = LDOConfigurationNameTable.left.at(commanded_configuration); + //m_gregorian_cover_position = commanded_configuration == CONFIGURATION_PRIMARY ? COVER_STATUS_CLOSED : COVER_STATUS_OPEN; + + ACS_LOG(LM_FULL_INFO, "SRTMinorServoSetupThread::onStart()", (LM_NOTICE, ("SETUP THREAD STARTED WITH '" + m_core.m_commanded_setup + "' CONFIGURATION").c_str())); +} + +void SRTMinorServoSetupThread::onStop() +{ + AUTO_TRACE("SRTMinorServoSetupThread::onStop()"); + + ACS_LOG(LM_FULL_INFO, "SRTMinorServoSetupThread::onStop()", (LM_NOTICE, "SETUP THREAD STOPPED")); + + if(m_core.m_motion_status.load() == MOTION_STATUS_TRACKING) + { + try + { + m_core.startThread(m_core.m_tracking_thread); + } + catch(ComponentErrors::ComponentErrorsEx& ex) + { + ACS_SHORT_LOG((LM_ERROR, ex.errorTrace.routine)); + m_core.setFailure(); + } + } +} + +void SRTMinorServoSetupThread::runLoop() +{ + AUTO_TRACE("SRTMinorServoSetupThread::runLoop()"); + + try + { + m_core.checkLineStatus(); + } + catch(MinorServoErrors::StatusErrorEx& ex) + { + ACS_SHORT_LOG((LM_ERROR, ex.errorTrace.routine)); + m_core.setFailure(); + this->setStopped(); + return; + } + + if(IRA::CIRATools::getUNIXEpoch() - m_start_time >= SETUP_TIMEOUT) + { + ACS_LOG(LM_FULL_INFO, "SRTMinorServoSetupThread::runLoop()", (LM_CRITICAL, "Timeout while performing a setup operation.")); + m_core.setFailure(); + this->setStopped(); + return; + } + + switch(m_status) + { + case 0: // Check if all the servos stopped + { + if(std::all_of(m_core.m_servos.begin(), m_core.m_servos.end(), [](const std::pair& servo) -> bool + { + ACSErr::Completion_var comp; + return servo.second->operative_mode()->get_sync(comp.out()) == OPERATIVE_MODE_STOP ? true : false; + })) + { + m_status = 1; + } + + break; + } + case 1: // Set all the servo offsets to 0 + { + for(const auto& [name, servo] : m_core.m_servos) + { + // Not sure about this + servo->clearSystemOffsets(); + servo->clearUserOffsets(); + } + + //m_status = 2; + //m_status = 100; + m_status = 5; + break; + } + /*case 100: // Send the STOW command to the gregorian cover + { + if(!m_core.m_socket.sendCommand(SRTMinorServoCommandLibrary::stow("Gregoriano", m_gregorian_cover_position)).checkOutput()) + { + ACS_LOG(LM_FULL_INFO, "SRTMinorServoSetupThread::runLoop()", (LM_CRITICAL, "Received NAK when setting the gregorian cover position.")); + m_core.setFailure(); + this->setStopped(); + return; + } + + m_status = 5; + break; + }*/ + /*case 2: // Send the SETUP command + { + try + { + if(!m_core.m_socket.sendCommand(SRTMinorServoCommandLibrary::setup(m_LDO_configuration)).checkOutput()) + { + ACS_LOG(LM_FULL_INFO, "SRTMinorServoSetupThread::runLoop()", (LM_CRITICAL, "Received NAK in response to a SETUP command.")); + m_core.setFailure(); + this->setStopped(); + return; + } + else + { + m_status = 3; + } + } + catch(...) + { + ACS_LOG(LM_FULL_INFO, "SRTMinorServoSetupThread::runLoop()", (LM_CRITICAL, "Communication error while sending a SETUP command.")); + m_core.setFailure(); + this->setStopped(); + return; + } + + break; + } + case 3: // Wait for the system to show the commanded configuration + { + if(m_core.m_status.getFocalConfiguration() == m_core.m_commanded_configuration.load()) + { + m_status = 4; + } + + break; + } + case 4: // Wait for the whole system to reach the desired configuration + { + // First we check the status of the gregorian cover + bool completed = m_core.m_status.getGregorianCoverPosition() == m_gregorian_cover_position ? true : false; + + // Then we cycle through all the servos and make sure their operative mode is SETUP + if(completed && std::all_of(m_core.m_servos.begin(), m_core.m_servos.end(), [](const std::pair& servo) -> bool + { + ACSErr::Completion_var comp; + return servo.second->operative_mode()->get_sync(comp.out()) == OPERATIVE_MODE_SETUP ? true : false; + })) + { + m_status = 5; + } + + break; + }*/ + case 5: // Load the servos coefficients and send a PRESET command + { + for(const auto& [servo_name, servo] : m_core.m_servos) + { + try + { + if(servo->setup(m_core.m_commanded_setup.c_str())) + { + m_core.m_current_servos[servo_name] = servo; + try + { + m_core.m_current_tracking_servos[servo_name] = m_core.m_tracking_servos.at(servo_name); + } + catch(...) + { + // Not a tracking servo, ignore + } + } + } + catch(...) + { + ACS_LOG(LM_FULL_INFO, "SRTMinorServoSetupThread::runLoop()", (LM_CRITICAL, ("Error while loading a SETUP to servo'" + servo_name + "'.").c_str())); + m_core.setFailure(); + this->setStopped(); + return; + } + } + + // This step is necessary because we have _ASACTIVE configurations that have a slightly different position from the commanded one + // Unfortunately, the Leonardo implementation accepts a fixed number of configurations, therefore we share the _ASACTIVE and AS not active configurations for each focal position + for(const auto& [servo_name, servo] : m_core.m_current_servos) + { + try + { + servo->preset(*servo->calcCoordinates(45)); + } + catch(MinorServoErrors::MinorServoErrorsEx& ex) + { + ACS_SHORT_LOG((LM_ERROR, ex.errorTrace.routine)); + m_core.setFailure(); + this->setStopped(); + return; + } + } + + m_status = 6; + break; + } + case 6: // Wait for the whole system to reach the PRESET configuration + { + // First we check the status of the gregorian cover + //bool completed = m_core.m_status.getGregorianCoverPosition() == m_gregorian_cover_position ? true : false; + + if(/*completed && */std::all_of(m_core.m_current_servos.begin(), m_core.m_current_servos.end(), [this](const std::pair& servo) -> bool + { + ACSErr::Completion_var comp; + return servo.second->operative_mode()->get_sync(comp.out()) == OPERATIVE_MODE_PRESET ? true : false; + })) + { + m_status = 7; + } + + break; + } + case 7: // Finally set all the variables values and eventually start the elevation tracking thread + { + m_core.m_actual_setup = m_core.m_commanded_setup; + m_core.m_starting.store(Management::MNG_FALSE); + m_core.m_ready.store(Management::MNG_TRUE); + m_core.m_subsystem_status.store(Management::MNG_OK); + + if(m_core.m_elevation_tracking_enabled.load() == Management::MNG_TRUE) + { + m_core.m_motion_status.store(MOTION_STATUS_TRACKING); + } + else + { + m_core.m_motion_status.store(MOTION_STATUS_CONFIGURED); + } + + this->setStopped(); + break; + } + } +} diff --git a/SRT/Servers/SRTMinorServo/src/SRTMinorServoStatusThread.cpp b/SRT/Servers/SRTMinorServo/src/SRTMinorServoStatusThread.cpp new file mode 100644 index 0000000000000000000000000000000000000000..3cbedce9a1371343c36b31efbaff738ea7dd63ce --- /dev/null +++ b/SRT/Servers/SRTMinorServo/src/SRTMinorServoStatusThread.cpp @@ -0,0 +1,112 @@ +#include "SRTMinorServoStatusThread.h" + +using namespace MinorServo; + +SRTMinorServoStatusThread::SRTMinorServoStatusThread(const ACE_CString& name, SRTMinorServoBossCore& core, const ACS::TimeInterval& response_time, const ACS::TimeInterval& sleep_time) : + ACS::Thread(name, response_time, sleep_time), + m_core(core), + m_sleep_time(this->getSleepTime()), + m_notification_channel(nullptr) +{ + AUTO_TRACE("SRTMinorServoStatusThread::SRTMinorServoStatusThread()"); +} + +SRTMinorServoStatusThread::~SRTMinorServoStatusThread() +{ + AUTO_TRACE("SRTMinorServoStatusThread::~SRTMinorServoStatusThread()"); +} + +void SRTMinorServoStatusThread::onStart() +{ + AUTO_TRACE("SRTMinorServoStatusThread::onStart()"); + + m_status = 0; + + ACS_LOG(LM_FULL_INFO, "SRTMinorServoStatusThread::onStart()", (LM_NOTICE, "STATUS THREAD STARTED")); +} + +void SRTMinorServoStatusThread::onStop() +{ + AUTO_TRACE("SRTMinorServoStatusThread::onStop()"); + + if(m_notification_channel != nullptr) + { + m_notification_channel->disconnect(); + m_notification_channel = nullptr; + } + + ACS_LOG(LM_FULL_INFO, "SRTMinorServoStatusThread::onStop()", (LM_NOTICE, "STATUS THREAD STOPPED")); +} + +void SRTMinorServoStatusThread::runLoop() +{ + AUTO_TRACE("SRTMinorServoStatusThread::runLoop()"); + + ACS::Time t0 = getTimeStamp(); + unsigned long sleep_time = 10000000; + + switch(m_status) + { + case 0: + { + // Wait for the properties to be monitored by the Boss component. + // This is necessary since this thread might start before the start of the properties monitoring. + if(m_core.m_component.isPropertiesMonitoringActive()) + { + m_status = 1; + } + + break; + } + case 1: + { + if(m_core.status()) + { + // Update the sleep time in order to not drift away by adding latency + sleep_time = std::max(m_sleep_time - (getTimeStamp() - t0), (long unsigned int)0); + } + + break; + } + } + + publish(); + + this->setSleepTime(sleep_time); +} + +void SRTMinorServoStatusThread::publish() +{ + // TODO: maybe update only when anything changed + MinorServoDataBlock data; + TIMEVALUE now; + IRA::CIRATools::getTime(now); + data.timeMark = now.value().value; + data.tracking = m_core.m_tracking.load() == Management::MNG_TRUE; + data.starting = m_core.m_starting.load() == Management::MNG_TRUE; + data.parking = m_core.m_motion_status.load() == MOTION_STATUS_PARKING; + data.parked = m_core.m_motion_status.load() == MOTION_STATUS_PARKED; + data.status = m_core.m_subsystem_status.load(); + + if(m_notification_channel == nullptr) + { + try + { + m_notification_channel = new nc::SimpleSupplier(MINORSERVO_DATA_CHANNEL, &m_core.m_component); + } + catch(...) + { + _IRA_LOGFILTER_LOG(LM_WARNING, "SRTMinorServoStatusThread::publish()", "cannot access the MinorServoData notification channel!"); + return; + } + } + + try + { + m_notification_channel->publishData(data); + } + catch(ComponentErrors::CORBAProblemEx& ex) + { + _IRA_LOGFILTER_LOG(LM_WARNING, "SRTMinorServoStatusThread::publish()", "cannot send MinorServoData over the notification channel!"); + } +} diff --git a/SRT/Servers/SRTMinorServo/src/SRTMinorServoTrackingThread.cpp b/SRT/Servers/SRTMinorServo/src/SRTMinorServoTrackingThread.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7f4350101bed7254a575850e5c19ead1cac34afc --- /dev/null +++ b/SRT/Servers/SRTMinorServo/src/SRTMinorServoTrackingThread.cpp @@ -0,0 +1,142 @@ +#include "SRTMinorServoTrackingThread.h" + +using namespace MinorServo; + +SRTMinorServoTrackingThread::SRTMinorServoTrackingThread(const ACE_CString& name, SRTMinorServoBossCore& core, const ACS::TimeInterval& response_time, const ACS::TimeInterval& sleep_time): + ACS::Thread(name, response_time, sleep_time), + m_core(core), + m_error(false) +{ + AUTO_TRACE("SRTMinorServoTrackingThread::SRTMinorServoTrackingThread()"); +} + +SRTMinorServoTrackingThread::~SRTMinorServoTrackingThread() +{ + AUTO_TRACE("SRTMinorServoTrackingThread::~SRTMinorServoTrackingThread()"); +} + +void SRTMinorServoTrackingThread::onStart() +{ + AUTO_TRACE("SRTMinorServoTrackingThread::onStart()"); + + this->setSleepTime(500000); // 50 milliseconds + m_point_id = 0; + m_point_time = 0; + + ACS_LOG(LM_FULL_INFO, "SRTMinorServoTrackingThread::onStart()", (LM_NOTICE, "TRACKING THREAD STARTED")); +} + +void SRTMinorServoTrackingThread::onStop() +{ + AUTO_TRACE("SRTMinorServoTrackingThread::onStop()"); + + ACS_LOG(LM_FULL_INFO, "SRTMinorServoTrackingThread::onStop()", (LM_NOTICE, "TRACKING THREAD STOPPED")); + + m_core.m_elevation_tracking.store(Management::MNG_FALSE); + + if(m_error) + { + m_core.setFailure(); + } +} + +void SRTMinorServoTrackingThread::runLoop() +{ + AUTO_TRACE("SRTMinorServoTrackingThread::runLoop()"); + + try + { + m_core.checkLineStatus(); + } + catch(MinorServoErrors::MinorServoErrorsEx& ex) + { + ACS_SHORT_LOG((LM_ERROR, ex.errorTrace.routine)); + m_error = true; + this->setStopped(); + return; + } + + if(m_core.m_motion_status.load() != MOTION_STATUS_TRACKING) + { + // System is not ready or is not configured for tracking yet, we wait, even though we should never get here + _IRA_LOGFILTER_LOG(LM_WARNING, "SRTMinorServoScanThread::runLoop()", "Waiting for the system to be configured for tracking!"); + return; + } + + // All checks passed, we have to track + if(m_point_id == 0) + { + m_point_time = getTimeStamp() + PROGRAM_TRACK_FUTURE_TIME; + m_trajectory_id = (unsigned int)(IRA::CIRATools::ACSTime2UNIXEpoch(m_point_time)); + } + else if(m_point_id == 1) + { + if(std::all_of(m_core.m_current_tracking_servos.begin(), m_core.m_current_tracking_servos.end(), [this](const std::pair& servo) -> bool + { + ACSErr::Completion_var comp; + if(servo.second->operative_mode()->get_sync(comp.out()) != OPERATIVE_MODE_PROGRAMTRACK) + { + ACS_LOG(LM_FULL_INFO, "SRTMinorServoTrackingThread::runLoop()", (LM_CRITICAL, (servo.first + ": failed to set PROGRAM_TRACK operative mode!").c_str())); + return false; + } + return true; + })) + { + // All used servos are set to PROGRAMTRACK operative mode, we are tracking the elevation + m_core.m_elevation_tracking.store(Management::MNG_TRUE); + } + else + { + m_error = true; + this->setStopped(); + return; + } + } + + for(const auto& [servo_name, servo] : m_core.m_current_tracking_servos) + { + double elevation = 45.0; + + try + { + elevation = m_core.getElevation(m_point_time); + } + catch(ComponentErrors::ComponentErrorsEx& ex) + { + _IRA_LOGFILTER_LOG(LM_WARNING, "SRTMinorServoScanThread::runLoop()", (std::string(getReasonFromEx(ex)) + ": using a fixed elevation of 45° for tracking!").c_str()); + m_core.m_elevation_tracking.store(Management::MNG_FALSE); + } + + try + { + servo->programTrack(m_trajectory_id, m_point_id, m_point_time, *servo->calcCoordinates(elevation)); + } + catch(MinorServoErrors::MinorServoErrorsEx& ex) + { + ACS_SHORT_LOG((LM_ERROR, ex.errorTrace.routine)); + m_error = true; + this->setStopped(); + return; + } + catch(std::exception& ex) + { + ACS_SHORT_LOG((LM_ERROR, ex.what())); + m_error = true; + this->setStopped(); + return; + } + catch(CORBA::Exception& ex) + { + ACS_SHORT_LOG((LM_ERROR, ex._info().c_str())); + m_error = true; + this->setStopped(); + return; + } + } + + m_point_id++; + m_point_time += PROGRAM_TRACK_TIMEGAP; + + // Sleep until the next point - PROGRAM_TRACK_FUTURE_TIME + this->setSleepTime((unsigned long)(m_point_time - PROGRAM_TRACK_FUTURE_TIME - getTimeStamp())); +} diff --git a/SRT/Servers/SRTMinorServo/src/SRTProgramTrackMinorServoImpl.cpp b/SRT/Servers/SRTMinorServo/src/SRTProgramTrackMinorServoImpl.cpp new file mode 100644 index 0000000000000000000000000000000000000000..16fa1ebd64b880e4f4023d8b1e01314201be6af0 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/src/SRTProgramTrackMinorServoImpl.cpp @@ -0,0 +1,173 @@ +#include "SRTMinorServoImpl.h" + +using namespace MinorServo; + +SRTProgramTrackMinorServoImpl::SRTProgramTrackMinorServoImpl(const ACE_CString &componentName, maci::ContainerServices *containerServices) : + SRTBaseMinorServoImpl(componentName, containerServices), + m_tracking_queue(1500, m_virtual_axes), + m_tracking_delta(SRTBaseMinorServoImpl::getMotionConstant(*this, "tracking_delta")), + m_tracking_error(m_virtual_axes, 0.0), + m_tracking(Management::MNG_FALSE), + m_trajectory_id(0), + m_total_trajectory_points(0), + m_remaining_trajectory_points(0), + m_tracking_ptr(this), + m_trajectory_id_ptr(this), + m_total_trajectory_points_ptr(this), + m_remaining_trajectory_points_ptr(this), + m_tracking_error_ptr(this) +{ + AUTO_TRACE(m_servo_name + "::SRTProgramTrackMinorServoImpl()"); +} + +SRTProgramTrackMinorServoImpl::~SRTProgramTrackMinorServoImpl() +{ + AUTO_TRACE(m_servo_name + "::~SRTProgramTrackMinorServoImpl()"); +} + +void SRTProgramTrackMinorServoImpl::initialize() +{ + SRTBaseMinorServoImpl::initialize(); + + try + { + m_tracking_ptr = new ROEnumImpl((m_component_name + ":tracking").c_str(), getComponent(), + new MSGenericDevIO>(m_tracking), true); + m_trajectory_id_ptr = new baci::ROlong((m_component_name + ":trajectory_id").c_str(), getComponent(), + new MSGenericDevIO>(m_trajectory_id), true); + m_total_trajectory_points_ptr = new baci::ROlong((m_component_name + ":total_trajectory_points").c_str(), getComponent(), + new MSGenericDevIO>(m_total_trajectory_points), true); + m_remaining_trajectory_points_ptr = new baci::ROlong((m_component_name + ":remaining_trajectory_points").c_str(), getComponent(), + new MSGenericDevIO>(m_remaining_trajectory_points), true); + m_tracking_error_ptr = new baci::ROdoubleSeq((m_component_name + ":tracking_error").c_str(), getComponent(), + new MSGenericDevIO>(m_tracking_error), true); + } + catch(std::bad_alloc& ba) + { + _EXCPT(ComponentErrors::MemoryAllocationExImpl, ex, (m_servo_name + "::initialize()").c_str()); + ex.log(LM_DEBUG); + throw ex.getComponentErrorsEx(); + } +} + +/////////////////// PUBLIC methods +bool SRTProgramTrackMinorServoImpl::status() +{ + bool status = SRTBaseMinorServoImpl::status(); + + try + { + ACS::Time last_timestamp = m_status.getTimestamp(); + + // The timestamp of the read positions always corresponds to the one we're asking since they both belong to the same STATUS command answer + // The tracking timestamp is interpolated instead + ACS::doubleSeq virtual_positions = m_status.getVirtualPositions(); + std::pair> tracking_point = m_tracking_queue.get(last_timestamp); + + m_remaining_trajectory_points.store(m_tracking_queue.getRemainingPoints(last_timestamp)); + + if(tracking_point.first < last_timestamp) + { + // We are past the last point of the trajectory, we concluded it + m_tracking.store(Management::MNG_FALSE); + m_tracking_queue.clear(); + m_tracking_error = std::vector(m_virtual_axes, 0.0); + } + else + { + bool is_tracking = true; + + std::transform(virtual_positions.begin(), virtual_positions.end(), tracking_point.second.begin(), m_tracking_error.begin(), [](double current_pos, double commanded_pos) + { + return std::fabs(current_pos - commanded_pos); + }); + + for(size_t i = 0; i < m_virtual_axes; i++) + { + if(std::fabs(m_tracking_error[i]) > m_tracking_delta[i]) + { + is_tracking = false; + break; + } + } + + m_tracking.store(is_tracking ? Management::MNG_TRUE : Management::MNG_FALSE); + } + } + catch(...) + { + // We might get here if m_tracking_queue is empty + // So whenever we just got a new setup or if we are past the last point inside the trajectory + m_tracking.store(Management::MNG_FALSE); // May be redundant but who cares? + m_tracking_error = std::vector(m_virtual_axes, 0.0); + } + + return status; +} + +bool SRTProgramTrackMinorServoImpl::setup(const char* configuration_name) +{ + bool return_value = SRTBaseMinorServoImpl::setup(configuration_name); + + m_tracking_queue.clear(); + m_total_trajectory_points.store(0); + m_remaining_trajectory_points.store(0); + + return return_value; +} + +void SRTProgramTrackMinorServoImpl::programTrack(CORBA::Long trajectory_id, CORBA::Long point_id, ACS::Time point_time, const ACS::doubleSeq& virtual_coordinates) +{ + AUTO_TRACE("SRTProgramTrackMinorServoImpl::programTrack()"); + + checkLineStatus(); + + if(virtual_coordinates.length() != m_virtual_axes) + { + _EXCPT(MinorServoErrors::TrackingErrorExImpl, ex, (m_servo_name + "::programTrack()").c_str()); + ex.addData("Reason", "Wrong number of values for this servo system!"); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + std::vector coordinates(virtual_coordinates.get_buffer(), virtual_coordinates.get_buffer() + virtual_coordinates.length()); + ACS::doubleSeq offsets = m_status.getVirtualOffsets(); + + for(size_t i = 0; i < m_virtual_axes; i++) + { + double coordinate = coordinates[i] + offsets[i]; + if(coordinate < m_min[i] || coordinate > m_max[i]) + { + _EXCPT(MinorServoErrors::TrackingErrorExImpl, ex, (m_servo_name + "::programTrack()").c_str()); + ex.addData("Reason", "Resulting position out of range, check the offsets!"); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + } + + if(!m_socket.sendCommand(SRTMinorServoCommandLibrary::programTrack(m_servo_name, trajectory_id, point_id, coordinates, point_id > 0 ? 0 : IRA::CIRATools::ACSTime2UNIXEpoch(point_time))).checkOutput()) + { + _EXCPT(MinorServoErrors::CommunicationErrorExImpl, ex, (m_servo_name + "::programTrack()").c_str()); + ex.setReason("Received NAK in response to a PROGRAMTRACK command!"); + ex.log(LM_DEBUG); + throw ex.getMinorServoErrorsEx(); + } + + m_trajectory_id.store(trajectory_id); + m_total_trajectory_points.store(point_id + 1); + if(point_id == 0) + { + // Clear the tracking queue to avoid interpolation between 2 different trajectories + m_tracking_queue.clear(); + } + m_tracking_queue.put(point_time, coordinates); +} + + +GET_PROPERTY_REFERENCE(Management::ROTBoolean, SRTProgramTrackMinorServoImpl, m_tracking_ptr, tracking); +GET_PROPERTY_REFERENCE(ACS::ROlong, SRTProgramTrackMinorServoImpl, m_trajectory_id_ptr, trajectory_id); +GET_PROPERTY_REFERENCE(ACS::ROlong, SRTProgramTrackMinorServoImpl, m_total_trajectory_points_ptr, total_trajectory_points); +GET_PROPERTY_REFERENCE(ACS::ROlong, SRTProgramTrackMinorServoImpl, m_remaining_trajectory_points_ptr, remaining_trajectory_points); +GET_PROPERTY_REFERENCE(ACS::ROdoubleSeq, SRTProgramTrackMinorServoImpl, m_tracking_error_ptr, tracking_error); + +MACI_DLL_SUPPORT_FUNCTIONS(SRTProgramTrackMinorServoImpl) diff --git a/SRT/Servers/SRTMinorServo/src/_cover.py b/SRT/Servers/SRTMinorServo/src/_cover.py new file mode 100644 index 0000000000000000000000000000000000000000..add318233a4f0f3de86278f35c7b5b123b45d993 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/src/_cover.py @@ -0,0 +1,42 @@ +#!/usr/bin/env python + +# This is a python script that can be used to set the gregorian cover position +# who when what +# Giuseppe Carboni(giuseppe.carboni@inaf.it) 21/01/2024 Creation + +from Acspy.Clients.SimpleClient import PySimpleClient +import ACSLog +import maciErrType +import maciErrTypeImpl +import ClientErrorsImpl +import MinorServoErrors +import ManagementErrorsImpl +import sys +from SimpleParserPy import add_user_message + +def main(): + compName = 'MINORSERVO/Boss' + + simpleClient = PySimpleClient() + + try: + component = simpleClient.getComponent(compName) + except Exception as ex: + newEx = ClientErrorsImpl.CouldntAccessComponentExImpl(exception=ex, create=1) + newEx.setComponentName(compName) + add_user_message(newEx, 'MinorServoBoss not ready or not properly configured') + simpleClient.disconnect() + sys.exit(1) + + try: + inputs = component.setGregorianCoverPosition(sys.argv[1]) + except MinorServoErrors.MinorServoErrorsEx as ex: + newEx = ClientErrorsImpl.CouldntPerformActionExImpl(exception=ex, create=1) + newEx.setReason('MinorServoBoss gregorian cover position') + add_user_message(newEx, 'Unable to set the gregorian cover position') + simpleClient.disconnect() + sys.exit(1) + + +if __name__ == "__main__": + main() diff --git a/SRT/Servers/SRTMinorServo/test/.gitignore b/SRT/Servers/SRTMinorServo/test/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..d3a2a1447394e51b5d34814f70fc1043a3df5bd7 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/test/.gitignore @@ -0,0 +1,3 @@ +TESTS/SRP/* +TESTS/COMBINED/* +TESTS/DEROTATOR* diff --git a/SRT/Servers/SRTMinorServo/test/CombinedProgramTrackTest.cpp b/SRT/Servers/SRTMinorServo/test/CombinedProgramTrackTest.cpp new file mode 100644 index 0000000000000000000000000000000000000000..d53245b9fb151830335d21bb66bf6f83e4b3eaa4 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/test/CombinedProgramTrackTest.cpp @@ -0,0 +1,532 @@ +#include "gtest/gtest.h" +#include +#include +#include +#include +#include +#include "SRTMinorServoSocket.h" +#include "SRTMinorServoCommandLibrary.h" + +// This address and port are the ones set in the simulator +// In order for the test to properly be executed, the simulator should be launched with the following command: +// discos-simulator -s minor_servo start & +#define SIMULATION +#ifdef SIMULATION + #define ADDRESS std::string("127.0.0.1") + #define PORT 12800 +#else + #define ADDRESS std::string("192.168.200.13") + #define PORT 4758 +#endif + +#define SOCKET_TIMEOUT 0.5 +#define NOISE_THRESHOLD 1 +#define TIMEGAP 0.2 +#define ADVANCE_TIMEGAP 5 +#define STATUS_PERIOD 0.01 +#define EPSILON 0.00001 + +#define SRP_COORDINATES std::vector{ "SRP_TX", "SRP_TY", "SRP_TZ", "SRP_RX", "SRP_RY", "SRP_RZ" } +#define SRP_MAX_RANGES std::vector{ 50, 110, 50, 0.25, 0.25, 0.25 } +#define SRP_MAX_SPEED std::vector{ 4.0, 4.0, 4.0, 0.04, 0.04, 0.04 } + +#define DEROTATOR std::string("GFR1") +#define DEROTATOR_COORDINATES std::vector{ DEROTATOR + "_ROTATION" } +#define DEROTATOR_RANGES std::vector{ 10.0, 100.0 } +#define DEROTATOR_MAX_SPEED std::vector{ 3.3 } + + +std::atomic terminate = false; + + +class CombinedProgramTrackTest : public ::testing::Test +{ +protected: + std::vector SRPStartingCoordinates = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + std::vector DerotatorStartingCoordinates = { 10.0 }; + std::string directory; + std::thread statusThread; + + static void printStatus(std::string directory) + { + SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance(); + SRTMinorServoAnswerMap SRPStatus, DerotatorStatus; + + ofstream SRPStatusFile, DerotatorStatusFile; + SRPStatusFile.open(directory + "/SRP/status.txt", ios::out); + DerotatorStatusFile.open(directory + "/DEROTATOR/status.txt", ios::out); + + long unsigned int counter = 0; + + double tn = CIRATools::getUNIXEpoch(); + + while(!terminate) + { + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP")); + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("Derotatore" + DEROTATOR)); + std::string SRPStringStatus = serializeSRPStatus(SRPStatus); + std::string DerotatorStringStatus = serializeDerotatorStatus(DerotatorStatus); + + SRPStatusFile << SRPStringStatus << std::endl; + DerotatorStatusFile << DerotatorStringStatus << std::endl; + if(counter % 10 == 0) + { + std::cout << SRPStringStatus << std::endl; + std::cout << DerotatorStringStatus << std::endl; + } + counter++; + + tn += STATUS_PERIOD; + std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, tn - CIRATools::getUNIXEpoch())))); + } + + SRPStatusFile.close(); + DerotatorStatusFile.close(); + } + + static void printSRPStatus(std::string filename) + { + SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance(); + SRTMinorServoAnswerMap SRPStatus; + + ofstream statusFile; + statusFile.open(filename, ios::out); + + long unsigned int counter = 0; + + double tn = CIRATools::getUNIXEpoch(); + + while(!terminate) + { + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP")); + std::string status = serializeSRPStatus(SRPStatus); + + statusFile << status << std::endl; + if(counter % 10 == 0) + std::cout << status << std::endl; + counter++; + + tn += STATUS_PERIOD; + std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, tn - CIRATools::getUNIXEpoch())))); + } + + statusFile.close(); + } + + static void printDerotatorStatus(std::string filename) + { + SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance(); + SRTMinorServoAnswerMap DerotatorStatus; + + ofstream statusFile; + statusFile.open(filename, ios::out); + + long unsigned int counter = 0; + + double tn = CIRATools::getUNIXEpoch(); + + while(!terminate) + { + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("Derotatore" + DEROTATOR)); + std::string status = serializeDerotatorStatus(DerotatorStatus); + + statusFile << status << std::endl; + if(counter % 10 == 0) + std::cout << status << std::endl; + counter++; + + tn += STATUS_PERIOD; + std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, tn - CIRATools::getUNIXEpoch())))); + } + + statusFile.close(); + } + + static void sigintHandler(int sig_num) + { + std::cout << std::endl << "Terminating..." << std::endl; + terminate = true; + } + + static std::string serializeSRPStatus(SRTMinorServoAnswerMap map) + { + std::string status = serializeCoordinates(std::get(map["TIMESTAMP"]), getCoordinates(map, SRP_COORDINATES)); + status += serializeElongations(getElongations(map)); + return status; + } + + static std::string serializeDerotatorStatus(SRTMinorServoAnswerMap map) + { + return serializeCoordinates(std::get(map["TIMESTAMP"]), getCoordinates(map, DEROTATOR_COORDINATES)); + } + + static std::string serializeCoordinates(double timestamp, std::vector coordinates) + { + std::stringstream stream; + stream << std::fixed << std::setprecision(6) << timestamp; + for(double coordinate : coordinates) + stream << "\t" << coordinate; + return stream.str(); + } + + static std::string serializeElongations(std::vector elongations) + { + std::stringstream stream; + stream << std::fixed << std::setprecision(6); + for(double elongation : elongations) + stream << "\t" << elongation; + return stream.str(); + } + + static std::vector getCoordinates(SRTMinorServoAnswerMap SRPStatus, std::vector coordinates) + { + std::vector currentCoordinates; + + for(std::string coordinate : coordinates) + { + auto value = SRPStatus[coordinate]; + + try + { + currentCoordinates.push_back(std::get(value)); + } + catch(std::bad_variant_access const& ex) + { + std::cout << ex.what() << ", variant index: " << value.index() << std::endl; + currentCoordinates.push_back(200.0); + } + } + + return currentCoordinates; + } + + static std::vector getElongations(SRTMinorServoAnswerMap SRPStatus) + { + std::vector currentElongations; + std::vector elongations = { "SRP_ELONG_Z1", "SRP_ELONG_Z2", "SRP_ELONG_Z3", "SRP_ELONG_Y1", "SRP_ELONG_Y2", "SRP_ELONG_X1" }; + + for(std::string elongation : elongations) + { + auto value = SRPStatus[elongation]; + + try + { + currentElongations.push_back(std::get(value)); + } + catch(std::bad_variant_access const& ex) + { + std::cout << ex.what() << ", variant index: " << value.index() << std::endl; + currentElongations.push_back(10000.0); + } + } + + return currentElongations; + } + + static bool compareCoordinates(std::vector first, std::vector second) + { + if(first.size() != second.size()) + return false; + + for(size_t i = 0; i < first.size(); i++) + { + double diff = fabs(first[i] - second[i]); + if(diff > EPSILON) + return false; + } + + return true; + } + + void SetUp() override + { + srand((int)CIRATools::getUNIXEpoch()); + std::cout << std::fixed << std::setprecision(6); + + try + { + SRTMinorServoSocket::getInstance(ADDRESS, PORT, SOCKET_TIMEOUT); + std::cout << "Socket connected." << std::endl; + } + catch(ComponentErrors::SocketErrorExImpl& ex) + { + if(ex.getData("Reason") == std::string("Cannot connect the socket.").c_str()) + FAIL() << "Socket failed to connect. Check if the simulator or the hardware can be reached." << std::endl; + else + FAIL() << "Unexpected failure." << std::endl; + } + + SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance(); + + std::cout << "Sending MS STATUS command..."; + + SRTMinorServoAnswerMap MSStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status()); + EXPECT_EQ(std::get(MSStatus["OUTPUT"]), "GOOD"); + EXPECT_EQ(std::get(MSStatus["CONTROL"]), 1); + EXPECT_EQ(std::get(MSStatus["POWER"]), 1); + EXPECT_EQ(std::get(MSStatus["EMERGENCY"]), 2); + EXPECT_EQ(std::get(MSStatus["ENABLED"]), 1); + std::cout << "OK." << std::endl; + + SRTMinorServoAnswerMap::iterator iterator; + for(iterator = MSStatus.begin(); iterator != MSStatus.end(); ++iterator) + { + std::visit([iterator](const auto& var) mutable { std::cout << iterator->first << ": " << var << std::endl; }, iterator->second); + } + + std::cout << "Sending initial SRP STATUS command..."; + + SRTMinorServoAnswerMap SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP")); + EXPECT_EQ(std::get(SRPStatus["OUTPUT"]), "GOOD"); + EXPECT_EQ(std::get(SRPStatus["SRP_STATUS"]), 1); + EXPECT_EQ(std::get(SRPStatus["SRP_BLOCK"]), 2); + std::cout << "OK." << std::endl; + + for(iterator = SRPStatus.begin(); iterator != SRPStatus.end(); ++iterator) + { + std::visit([iterator](const auto& var) mutable { std::cout << iterator->first << ": " << var << std::endl; }, iterator->second); + } + + SRTMinorServoAnswerMap DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("Derotatore" + DEROTATOR)); + EXPECT_EQ(std::get(SRPStatus["OUTPUT"]), "GOOD"); + EXPECT_EQ(std::get(DerotatorStatus[DEROTATOR + "_STATUS"]), 1); + EXPECT_EQ(std::get(DerotatorStatus[DEROTATOR + "_BLOCK"]), 2); + + for(iterator = DerotatorStatus.begin(); iterator != DerotatorStatus.end(); ++iterator) + { + std::visit([iterator](const auto& var) mutable { std::cout << iterator->first << ": " << var << std::endl; }, iterator->second); + } + + std::cout << "OK." << std::endl; + std::cout << "Sending all axes to 0..." << std::endl; + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::preset("SRP", SRPStartingCoordinates)); + EXPECT_EQ(std::get(SRPStatus["OUTPUT"]), "GOOD"); + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::preset("Derotatore" + DEROTATOR, DerotatorStartingCoordinates)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + + signal(SIGINT, CombinedProgramTrackTest::sigintHandler); + + bool SRPReady = false, DerotatorReady = false; + + do + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP")); + EXPECT_EQ(std::get(SRPStatus["OUTPUT"]), "GOOD"); + std::cout << serializeSRPStatus(SRPStatus) << std::endl; + SRPReady = std::get(SRPStatus["SRP_OPERATIVE_MODE"]) == 40 ? true : false; + + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("Derotatore" + DEROTATOR)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + std::cout << serializeDerotatorStatus(DerotatorStatus) << std::endl; + DerotatorReady = std::get(DerotatorStatus[DEROTATOR + "_OPERATIVE_MODE"]) == 40 ? true : false; + + if(terminate) + FAIL() << "Aborting test..." << std::endl; + } + while(!SRPReady || !DerotatorReady); + EXPECT_EQ(std::get(SRPStatus["SRP_OPERATIVE_MODE"]), 40); + EXPECT_EQ(std::get(DerotatorStatus[DEROTATOR + "_OPERATIVE_MODE"]), 40); + + std::cout << "OK." << std::endl; + + std::time_t tn = std::time(0); + std::tm* now = std::localtime(&tn); + std::stringstream directory_ss; + directory_ss << "TESTS/COMBINED"; + boost::filesystem::create_directory(directory_ss.str()); + directory_ss << "/"; + directory_ss << ::testing::UnitTest::GetInstance()->current_test_info()->name(); + directory_ss << "-"; + directory_ss << 1900 + now->tm_year; + directory_ss << std::setfill('0') << std::setw(2) << now->tm_mon + 1; + directory_ss << std::setfill('0') << std::setw(2) << now->tm_mday; + directory_ss << "-"; + directory_ss << std::setfill('0') << std::setw(2) << now->tm_hour; + directory_ss << std::setfill('0') << std::setw(2) << now->tm_min; + directory_ss << std::setfill('0') << std::setw(2) << now->tm_sec; + directory = directory_ss.str(); + boost::filesystem::create_directory(directory); + boost::filesystem::create_directory(directory + "/SRP"); + boost::filesystem::create_directory(directory + "/DEROTATOR"); + + statusThread = std::thread(&CombinedProgramTrackTest::printStatus, directory); + } + + void TearDown() override + { + SRTMinorServoSocket::destroyInstance(); + terminate = false; + } +}; + +TEST_F(CombinedProgramTrackTest, SineWaveMovementTest) +{ + SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance(); + SRTMinorServoAnswerMap SRPStatus, DerotatorStatus; + + double start_time = CIRATools::getUNIXEpoch() + ADVANCE_TIMEGAP; + std::cout << "PRESET position reached, starting PROGRAMTRACK with start time: " << start_time << std::endl; + long unsigned int trajectory_id = int(start_time); + unsigned int point_id = 0; + std::vector SRPCoordinates = SRPStartingCoordinates; + std::vector DerotatorCoordinates = DerotatorStartingCoordinates; + + std::vector phase_shift; + std::vector period; + for(size_t axis = 0; axis < 6; axis++) + { + period.push_back(SRP_MAX_RANGES[axis] / SRP_MAX_SPEED[axis] * 4); + phase_shift.push_back((double)std::rand() / RAND_MAX * period[axis]); + SRPCoordinates[axis] = SRP_MAX_RANGES[axis] * sin(phase_shift[axis] * 2 * M_PI / period[axis]); + } + + double derotator_amplitude = (DEROTATOR_RANGES[1] - DEROTATOR_RANGES[0]) / 2; + double derotator_center = (DEROTATOR_RANGES[0] + DEROTATOR_RANGES[1]) / 2; + // Derotator period + period.push_back(80); + phase_shift.push_back((double)std::rand() / RAND_MAX * period[6]); + DerotatorCoordinates[0] = derotator_center + derotator_amplitude * sin(phase_shift[6] * 2 * M_PI / period[6]); + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("SRP", trajectory_id, point_id, SRPCoordinates, start_time)); + EXPECT_EQ(std::get(SRPStatus["OUTPUT"]), "GOOD"); + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("Derotatore" + DEROTATOR, trajectory_id, point_id, DerotatorCoordinates, start_time)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP")); + EXPECT_EQ(std::get(SRPStatus["OUTPUT"]), "GOOD"); + EXPECT_EQ(std::get(SRPStatus["SRP_OPERATIVE_MODE"]), 50); + + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("Derotatore" + DEROTATOR)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + EXPECT_EQ(std::get(DerotatorStatus[DEROTATOR + "_OPERATIVE_MODE"]), 50); + + ofstream SRPProgramTrackFile; + SRPProgramTrackFile.open(directory + "/SRP/trajectory.txt", ios::out); + SRPProgramTrackFile << CombinedProgramTrackTest::serializeCoordinates(start_time, SRPCoordinates) << std::endl; + + ofstream DerotatorProgramTrackFile; + DerotatorProgramTrackFile.open(directory + "/DEROTATOR/trajectory.txt", ios::out); + DerotatorProgramTrackFile << CombinedProgramTrackTest::serializeCoordinates(start_time, DerotatorCoordinates) << std::endl; + + double next_expected_time = start_time; + + while(!terminate) + { + next_expected_time += TIMEGAP; + double time_delta = next_expected_time - start_time; + + std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, next_expected_time - ADVANCE_TIMEGAP - CIRATools::getUNIXEpoch())))); + point_id++; + + for(size_t axis = 0; axis < 6; axis++) + { + SRPCoordinates[axis] = SRP_MAX_RANGES[axis] * sin((time_delta + phase_shift[axis]) * 2 * M_PI / period[axis]); + } + DerotatorCoordinates[0] = derotator_center + derotator_amplitude * sin((time_delta + phase_shift[6]) * 2 * M_PI / period[6]); + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("SRP", trajectory_id, point_id, SRPCoordinates)); + EXPECT_EQ(std::get(SRPStatus["OUTPUT"]), "GOOD"); + SRPProgramTrackFile << CombinedProgramTrackTest::serializeCoordinates(next_expected_time, SRPCoordinates) << std::endl; + + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("Derotatore" + DEROTATOR, trajectory_id, point_id, DerotatorCoordinates)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + DerotatorProgramTrackFile << CombinedProgramTrackTest::serializeCoordinates(next_expected_time, DerotatorCoordinates) << std::endl; + } + + SRPProgramTrackFile.close(); + DerotatorProgramTrackFile.close(); + statusThread.join(); +} + +TEST_F(CombinedProgramTrackTest, SineWaveSeparateMovementTest) +{ + SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance(); + SRTMinorServoAnswerMap SRPStatus, DerotatorStatus; + + ofstream SRPProgramTrackFile; + SRPProgramTrackFile.open(directory + "/SRP/trajectory.txt", ios::out); + + ofstream DerotatorProgramTrackFile; + DerotatorProgramTrackFile.open(directory + "/DEROTATOR/trajectory.txt", ios::out); + + while(!terminate) + { + double start_time = CIRATools::getUNIXEpoch() + ADVANCE_TIMEGAP; + std::cout << "Starting new trajectory with start time: " << start_time << std::endl; + long unsigned int trajectory_id = int(start_time); + unsigned int point_id = 0; + std::vector SRPCoordinates = SRPStartingCoordinates; + std::vector DerotatorCoordinates = DerotatorStartingCoordinates; + + std::vector phase_shift; + std::vector period; + for(size_t axis = 0; axis < 6; axis++) + { + period.push_back(SRP_MAX_RANGES[axis] / SRP_MAX_SPEED[axis] * 4); + phase_shift.push_back((double)std::rand() / RAND_MAX * period[axis]); + SRPCoordinates[axis] = SRP_MAX_RANGES[axis] * sin(phase_shift[axis] * 2 * M_PI / period[axis]); + } + + double derotator_amplitude = (DEROTATOR_RANGES[1] - DEROTATOR_RANGES[0]) / 2; + double derotator_center = (DEROTATOR_RANGES[0] + DEROTATOR_RANGES[1]) / 2; + // Derotator period + period.push_back(80); + phase_shift.push_back((double)std::rand() / RAND_MAX * period[6]); + DerotatorCoordinates[0] = derotator_center + derotator_amplitude * sin(phase_shift[6] * 2 * M_PI / period[6]); + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("SRP", trajectory_id, point_id, SRPCoordinates, start_time)); + EXPECT_EQ(std::get(SRPStatus["OUTPUT"]), "GOOD"); + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("Derotatore" + DEROTATOR, trajectory_id, point_id, DerotatorCoordinates, start_time)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + + SRPProgramTrackFile << CombinedProgramTrackTest::serializeCoordinates(start_time, SRPCoordinates) << std::endl; + DerotatorProgramTrackFile << CombinedProgramTrackTest::serializeCoordinates(start_time, DerotatorCoordinates) << std::endl; + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP")); + EXPECT_EQ(std::get(SRPStatus["OUTPUT"]), "GOOD"); + EXPECT_EQ(std::get(SRPStatus["SRP_OPERATIVE_MODE"]), 50); + + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("Derotatore" + DEROTATOR)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + EXPECT_EQ(std::get(DerotatorStatus[DEROTATOR + "_OPERATIVE_MODE"]), 50); + + double next_expected_time = start_time; + + while(!terminate) + { + next_expected_time += TIMEGAP; + double time_delta = next_expected_time - start_time; + + std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, next_expected_time - ADVANCE_TIMEGAP - CIRATools::getUNIXEpoch())))); + point_id++; + + for(size_t axis = 0; axis < 6; axis++) + { + SRPCoordinates[axis] = SRP_MAX_RANGES[axis] * sin((time_delta + phase_shift[axis]) * 2 * M_PI / period[axis]); + } + DerotatorCoordinates[0] = derotator_center + derotator_amplitude * sin((time_delta + phase_shift[6]) * 2 * M_PI / period[6]); + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("SRP", trajectory_id, point_id, SRPCoordinates)); + EXPECT_EQ(std::get(SRPStatus["OUTPUT"]), "GOOD"); + SRPProgramTrackFile << CombinedProgramTrackTest::serializeCoordinates(next_expected_time, SRPCoordinates) << std::endl; + + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("Derotatore" + DEROTATOR, trajectory_id, point_id, DerotatorCoordinates)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + DerotatorProgramTrackFile << CombinedProgramTrackTest::serializeCoordinates(next_expected_time, DerotatorCoordinates) << std::endl; + + if(point_id == 1000) + break; + } + } + + SRPProgramTrackFile.close(); + DerotatorProgramTrackFile.close(); + statusThread.join(); +} diff --git a/SRT/Servers/SRTMinorServo/test/DerotatorProgramTrackTest.cpp b/SRT/Servers/SRTMinorServo/test/DerotatorProgramTrackTest.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6f5f719dac0066bad94052a3f3bd3144d3e20c1b --- /dev/null +++ b/SRT/Servers/SRTMinorServo/test/DerotatorProgramTrackTest.cpp @@ -0,0 +1,495 @@ +#include "gtest/gtest.h" +#include +#include +#include +#include +#include +#include "SRTMinorServoSocket.h" +#include "SRTMinorServoCommandLibrary.h" + +// This address and port are the ones set in the simulator +// In order for the test to properly be executed, the simulator should be launched with the following command: +// discos-simulator -s minor_servo start & +#define SIMULATION +#ifdef SIMULATION + #define ADDRESS std::string("127.0.0.1") + #define PORT 12800 +#else + #define ADDRESS std::string("192.168.200.13") + #define PORT 4758 +#endif +#define SOCKET_TIMEOUT 0.5 +#define TIMEGAP 0.2 +#define ADVANCE_TIMEGAP 5 +#define EPSILON 0.00001 +#define STATUS_PERIOD 0.01 +#define RANGES std::vector{ 10.0, 50.0 } +#define DEROTATOR std::string("GFR1") + +std::atomic terminate = false; + + +class DerotatorProgramTrackTest : public ::testing::Test +{ +protected: + std::vector startingCoordinates = { RANGES[0] }; + std::string directory; + std::thread statusThread; + + static void printStatus(std::string filename) + { + SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance(); + SRTMinorServoAnswerMap DerotatorStatus; + + ofstream statusFile; + statusFile.open(filename, ios::out); + + long unsigned int counter = 0; + + double tn = CIRATools::getUNIXEpoch(); + + while(!terminate) + { + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("Derotatore" + DEROTATOR)); + std::string status = serializeStatus(DerotatorStatus); + + statusFile << status << std::endl; + if(counter % (long unsigned int)(0.1 / STATUS_PERIOD) == 0) + std::cout << status << std::endl; + counter++; + + tn += STATUS_PERIOD; + std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, tn - CIRATools::getUNIXEpoch())))); + } + + statusFile.close(); + } + + static void sigintHandler(int sig_num) + { + std::cout << std::endl << "Terminating..." << std::endl; + terminate = true; + } + + static std::string serializeStatus(SRTMinorServoAnswerMap map) + { + return serializeCoordinates(std::get(map["TIMESTAMP"]), getCoordinates(map)); + } + + static std::string serializeCoordinates(double timestamp, std::vector coordinates) + { + std::stringstream stream; + stream << std::fixed << std::setprecision(6) << timestamp; + for(double coordinate : coordinates) + stream << "\t" << coordinate; + return stream.str(); + } + + static std::vector getCoordinates(SRTMinorServoAnswerMap DerotatorStatus) + { + std::vector currentCoordinates; + std::vector coordinates = { DEROTATOR + "_ROTATION" }; + + for(std::string coordinate : coordinates) + { + auto value = DerotatorStatus[coordinate]; + + try + { + currentCoordinates.push_back(std::get(value)); + } + catch(std::bad_variant_access const& ex) + { + std::cout << ex.what() << ", variant index: " << value.index() << std::endl; + currentCoordinates.push_back(200.0); + } + } + + return currentCoordinates; + } + + static bool moveAxis(std::vector &coordinates, int axis_to_move, int sign) + { + sign = sign / abs(sign); + double offset_to_add = 3.3 * TIMEGAP; + coordinates[axis_to_move] += sign * offset_to_add; + if(sign > 0) + { + coordinates[axis_to_move] = std::min(RANGES[1], coordinates[axis_to_move]); + } + else + { + coordinates[axis_to_move] = std::max(RANGES[0], coordinates[axis_to_move]); + } + if(coordinates[axis_to_move] == RANGES[0] || coordinates[axis_to_move] == RANGES[1]) + return true; + return false; + } + + void SetUp() override + { + srand((int)CIRATools::getUNIXEpoch()); + std::cout << std::fixed << std::setprecision(6); + + try + { + SRTMinorServoSocket::getInstance(ADDRESS, PORT, SOCKET_TIMEOUT); + std::cout << "Socket connected." << std::endl; + } + catch(ComponentErrors::SocketErrorExImpl& ex) + { + if(ex.getData("Reason") == std::string("Cannot connect the socket.").c_str()) + FAIL() << "Socket failed to connect. Check if the simulator or the hardware can be reached." << std::endl; + else + FAIL() << "Unexpected failure." << std::endl; + } + + SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance(); + + std::cout << "Sending MS STATUS command..."; + + SRTMinorServoAnswerMap MSStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status()); + EXPECT_EQ(std::get(MSStatus["OUTPUT"]), "GOOD"); + EXPECT_EQ(std::get(MSStatus["CONTROL"]), 1); + EXPECT_EQ(std::get(MSStatus["POWER"]), 1); + EXPECT_EQ(std::get(MSStatus["EMERGENCY"]), 2); + EXPECT_EQ(std::get(MSStatus["ENABLED"]), 1); + std::cout << "OK." << std::endl; + + SRTMinorServoAnswerMap::iterator iterator; + for(iterator = MSStatus.begin(); iterator != MSStatus.end(); ++iterator) + { + std::visit([iterator](const auto& var) mutable { std::cout << iterator->first << ": " << var << std::endl; }, iterator->second); + } + + std::cout << "Sending initial Derotator STATUS command..."; + + SRTMinorServoAnswerMap DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("Derotatore" + DEROTATOR)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + EXPECT_EQ(std::get(DerotatorStatus[DEROTATOR + "_STATUS"]), 1); + EXPECT_EQ(std::get(DerotatorStatus[DEROTATOR + "_BLOCK"]), 2); + std::cout << "OK." << std::endl; + + for(iterator = DerotatorStatus.begin(); iterator != DerotatorStatus.end(); ++iterator) + { + std::visit([iterator](const auto& var) mutable { std::cout << iterator->first << ": " << var << std::endl; }, iterator->second); + } + + std::cout << "Sending derotator to the initial position..." << std::endl; + + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::preset("Derotatore" + DEROTATOR, startingCoordinates)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + + signal(SIGINT, DerotatorProgramTrackTest::sigintHandler); + + do + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("Derotatore" + DEROTATOR)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + + std::cout << serializeStatus(DerotatorStatus) << std::endl; + + if(terminate) + FAIL() << "Aborting test..." << std::endl; + } + while(std::get(DerotatorStatus[DEROTATOR + "_OPERATIVE_MODE"]) != 40); + EXPECT_EQ(std::get(DerotatorStatus[DEROTATOR + "_OPERATIVE_MODE"]), 40); + + std::cout << "OK." << std::endl; + + std::time_t tn = std::time(0); + std::tm* now = std::localtime(&tn); + std::stringstream directory_ss; + directory_ss << "TESTS/DEROTATOR"; + boost::filesystem::create_directory(directory_ss.str()); + directory_ss << "/"; + directory_ss << ::testing::UnitTest::GetInstance()->current_test_info()->name(); + directory_ss << "-"; + directory_ss << 1900 + now->tm_year; + directory_ss << std::setfill('0') << std::setw(2) << now->tm_mon + 1; + directory_ss << std::setfill('0') << std::setw(2) << now->tm_mday; + directory_ss << "-"; + directory_ss << std::setfill('0') << std::setw(2) << now->tm_hour; + directory_ss << std::setfill('0') << std::setw(2) << now->tm_min; + directory_ss << std::setfill('0') << std::setw(2) << now->tm_sec; + directory = directory_ss.str(); + boost::filesystem::create_directory(directory); + + statusThread = std::thread(&DerotatorProgramTrackTest::printStatus, directory + "/status.txt"); + } + + void TearDown() override + { + SRTMinorServoSocket::destroyInstance(); + terminate = false; + } +}; + +TEST_F(DerotatorProgramTrackTest, ContinuousMovementTest) +{ + double start_time = CIRATools::getUNIXEpoch() + ADVANCE_TIMEGAP; + std::cout << "PRESET position reached, starting PROGRAMTRACK with start time: " << start_time << std::endl; + long unsigned int trajectory_id = int(start_time); + unsigned int point_id = 0; + std::vector programTrackCoordinates = startingCoordinates; + + SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance(); + SRTMinorServoAnswerMap DerotatorStatus; + std::string command = SRTMinorServoCommandLibrary::programTrack("Derotatore" + DEROTATOR, trajectory_id, point_id, programTrackCoordinates, start_time); + std::cout << command << std::endl; + DerotatorStatus = socket.sendCommand(command); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("Derotatore" + DEROTATOR)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + EXPECT_EQ(std::get(DerotatorStatus[DEROTATOR + "_OPERATIVE_MODE"]), 50); + + ofstream programTrackFile; + programTrackFile.open(directory + "/trajectory.txt", ios::out); + programTrackFile << DerotatorProgramTrackTest::serializeCoordinates(start_time, programTrackCoordinates) << std::endl; + + double next_expected_time = start_time; + int sign = 1; + unsigned int idle_count = 0; + bool idle = false; + + while(!terminate) + { + while(!terminate) + { + next_expected_time += TIMEGAP; + + std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, next_expected_time - ADVANCE_TIMEGAP - CIRATools::getUNIXEpoch())))); + point_id++; + + if(idle) + { + idle_count++; + if(idle_count == 5) + { + idle_count = 0; + idle = false; + } + } + else if(moveAxis(programTrackCoordinates, 0, sign)) + { + sign *= -1; + idle = true; + } + + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("Derotatore" + DEROTATOR, trajectory_id, point_id, programTrackCoordinates)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + //std::cout << DerotatorProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl; + programTrackFile << DerotatorProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl; + + if(round(programTrackCoordinates[0] * 100) == 10 && sign == 1) + { + programTrackCoordinates[0] = RANGES[0]; + break; + } + } + } + + programTrackFile.close(); + statusThread.join(); +} + +TEST_F(DerotatorProgramTrackTest, SineWaveMovementTest) +{ + double start_time = CIRATools::getUNIXEpoch() + ADVANCE_TIMEGAP; + std::cout << "PRESET position reached, starting PROGRAMTRACK with start time: " << start_time << std::endl; + long unsigned int trajectory_id = int(start_time); + unsigned int point_id = 0; + std::vector programTrackCoordinates = startingCoordinates; + + double phase_shift = (double)std::rand() / RAND_MAX * 60; + double amplitude = (RANGES[1] - RANGES[0]) / 2; + double center = (RANGES[0] + RANGES[1]) / 2; + double period = 80; + programTrackCoordinates[0] = center + amplitude * sin(phase_shift * 2 * M_PI / period); + + SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance(); + SRTMinorServoAnswerMap DerotatorStatus; + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("Derotatore" + DEROTATOR, trajectory_id, point_id, programTrackCoordinates, start_time)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("Derotatore" + DEROTATOR)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + EXPECT_EQ(std::get(DerotatorStatus[DEROTATOR + "_OPERATIVE_MODE"]), 50); + + ofstream programTrackFile; + programTrackFile.open(directory + "/trajectory.txt", ios::out); + programTrackFile << DerotatorProgramTrackTest::serializeCoordinates(start_time, programTrackCoordinates) << std::endl; + + double next_expected_time = start_time; + + while(!terminate) + { + next_expected_time += TIMEGAP; + double time_delta = next_expected_time - start_time; + + std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, next_expected_time - ADVANCE_TIMEGAP - CIRATools::getUNIXEpoch())))); + point_id++; + + programTrackCoordinates[0] = center + amplitude * sin((time_delta + phase_shift) * 2 * M_PI / period); + + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("Derotatore" + DEROTATOR, trajectory_id, point_id, programTrackCoordinates)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + //std::cout << DerotatorProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl; + programTrackFile << DerotatorProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl; + } + + programTrackFile.close(); + statusThread.join(); +} + +TEST_F(DerotatorProgramTrackTest, SeparateMovementTest) +{ + SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance(); + SRTMinorServoAnswerMap DerotatorStatus; + + ofstream programTrackFile; + programTrackFile.open(directory + "/trajectory.txt", ios::out); + + int sign = 1; + unsigned int idle_count = 0; + bool idle = false; + + std::cout << "PRESET position reached, starting PROGRAMTRACK" << std::endl; + std::vector programTrackCoordinates = startingCoordinates; + + bool immediate = true; + + while(!terminate) + { + double start_time = CIRATools::getUNIXEpoch() + ADVANCE_TIMEGAP; + long unsigned int trajectory_id = int(start_time); + double next_expected_time = start_time; + unsigned int point_id = 0; + + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("Derotatore" + DEROTATOR, trajectory_id, point_id, programTrackCoordinates, start_time)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + programTrackFile << DerotatorProgramTrackTest::serializeCoordinates(start_time, programTrackCoordinates) << std::endl; + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("Derotatore" + DEROTATOR)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + EXPECT_EQ(std::get(DerotatorStatus[DEROTATOR + "_OPERATIVE_MODE"]), 50); + + while(!terminate) + { + next_expected_time += TIMEGAP; + + std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, next_expected_time - ADVANCE_TIMEGAP - CIRATools::getUNIXEpoch())))); + point_id++; + + if(idle) + { + idle_count++; + //if(idle_count == ADVANCE_TIMEGAP / TIMEGAP) + if(idle_count == ADVANCE_TIMEGAP / TIMEGAP || immediate) + { + idle_count = 0; + idle = false; + break; + } + } + else + { + if(moveAxis(programTrackCoordinates, 0, sign)) + { + sign *= -1; + idle = true; + } + + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("Derotatore" + DEROTATOR, trajectory_id, point_id, programTrackCoordinates)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + } + + //std::cout << DerotatorProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl; + programTrackFile << DerotatorProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl; + } + + //immediate = immediate ? false : true; + } + + programTrackFile.close(); + statusThread.join(); +} + +TEST_F(DerotatorProgramTrackTest, RapidTrajectoryTest) +{ + SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance(); + SRTMinorServoAnswerMap DerotatorStatus; + + ofstream programTrackFile; + programTrackFile.open(directory + "/trajectory.txt", ios::out); + + int sign = -1; + unsigned int idle_count = 0; + bool idle = false; + + std::cout << "PRESET position reached, starting PROGRAMTRACK" << std::endl; + + while(!terminate) + { + std::vector programTrackCoordinates = startingCoordinates; + programTrackCoordinates[0] = RANGES[1]; + + double start_time = CIRATools::getUNIXEpoch() + ADVANCE_TIMEGAP; + long unsigned int trajectory_id = int(start_time); + double next_expected_time = start_time; + unsigned int point_id = 0; + + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("Derotatore" + DEROTATOR, trajectory_id, point_id, programTrackCoordinates, start_time)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + programTrackFile << DerotatorProgramTrackTest::serializeCoordinates(start_time, programTrackCoordinates) << std::endl; + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("Derotatore" + DEROTATOR)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + EXPECT_EQ(std::get(DerotatorStatus[DEROTATOR + "_OPERATIVE_MODE"]), 50); + + while(!terminate) + { + next_expected_time += TIMEGAP; + + std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, next_expected_time - ADVANCE_TIMEGAP - CIRATools::getUNIXEpoch())))); + point_id++; + + if(idle) + { + idle_count++; + if(idle_count == (ADVANCE_TIMEGAP * 2) / TIMEGAP) + { + idle_count = 0; + idle = false; + break; + } + } + else + { + if(moveAxis(programTrackCoordinates, 0, sign)) + { + idle = true; + } + + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("Derotatore" + DEROTATOR, trajectory_id, point_id, programTrackCoordinates)); + EXPECT_EQ(std::get(DerotatorStatus["OUTPUT"]), "GOOD"); + } + + //std::cout << DerotatorProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl; + programTrackFile << DerotatorProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl; + } + } + + programTrackFile.close(); + statusThread.join(); +} diff --git a/SRT/Servers/SRTMinorServo/test/Makefile b/SRT/Servers/SRTMinorServo/test/Makefile index cbc49a4479619d38e8a6695820d2c23441f20fac..97a093eaba9c1d81bf8a0ad132e84522ae80eb15 100644 --- a/SRT/Servers/SRTMinorServo/test/Makefile +++ b/SRT/Servers/SRTMinorServo/test/Makefile @@ -1,11 +1,10 @@ # CPP UNIT TESTING SETUP #-------------- -# GTEST_HOME=/usr/local/include/gtest -# GMOCK_HOME=/usr/local/include/gmock -# GTEST_LIBS=gtest gtest_main +GTEST_HOME=/usr/local/include/gtest +GMOCK_HOME=/usr/local/include/gmock +GTEST_LIBS=gtest gtest_main -# USER_INC=-I$(GTEST_HOME) -I$(GMOCK_HOME) -# USER_LIBS=C++ pthread +USER_INC=-I$(GTEST_HOME) -I$(GMOCK_HOME) # END OF CPP UNIT TESTING SETUP #--------------------- @@ -15,9 +14,26 @@ # unittest_OBJECTS = unittest # unittest_LIBS = $(GTEST_LIBS) -# EXECUTABLES_L = unittest -# unittest_OBJECTS = unittest -# unittest_LIBS = $(GTEST_LIBS) +EXECUTABLES_L = SRPProgramTrackTest #DerotatorProgramTrackTest CombinedProgramTrackTest ReadStatusOnlyTest + +SRPProgramTrackTest_OBJECTS = SRPProgramTrackTest +SRPProgramTrackTest_CFLAGS = -std=c++17 -fconcepts +SRPProgramTrackTest_LIBS = $(GTEST_LIBS) SRTMinorServoSocketLibrary SRTMinorServoCommandLibrary IRALibrary ComponentErrors boost_filesystem + +DerotatorProgramTrackTest_OBJECTS = DerotatorProgramTrackTest +DerotatorProgramTrackTest_CFLAGS = -std=c++17 +DerotatorProgramTrackTest_LIBS = $(GTEST_LIBS) SRTMinorServoSocketLibrary SRTMinorServoCommandLibrary IRALibrary ComponentErrors boost_filesystem +DerotatorProgramTrackTest_LDFLAGS = -lstdc++ -lpthread + +CombinedProgramTrackTest_OBJECTS = CombinedProgramTrackTest +CombinedProgramTrackTest_CFLAGS = -std=c++17 +CombinedProgramTrackTest_LIBS = $(GTEST_LIBS) SRTMinorServoSocketLibrary SRTMinorServoCommandLibrary IRALibrary ComponentErrors boost_filesystem +CombinedProgramTrackTest_LDFLAGS = -lstdc++ -lpthread + +ReadStatusOnlyTest_OBJECTS = ReadStatusOnlyTest +ReadStatusOnlyTest_CFLAGS = -std=c++17 +ReadStatusOnlyTest_LIBS = $(GTEST_LIBS) SRTMinorServoSocketLibrary SRTMinorServoCommandLibrary IRALibrary ComponentErrors boost_filesystem +ReadStatusOnlyTest_LDFLAGS = -lstdc++ -lpthread # END OF CUSTOMIZATION # do not edit below this line @@ -39,7 +55,11 @@ endif do_unit: all @echo "running cpp unit tests" - ../bin/unittest --gtest_output=xml:results/cppunittest.xml + ../bin/SRTMinorServoSocketTest --gtest_output=xml:results/cppSRTMinorServoSocketTest.xml + ../bin/SRPProgramTrackTest --gtest_output=xml:results/cppSRPProgramTrackTest.xml + ../bin/DerotatorProgramTrackTest --gtest_output=xml:results/cppDerotatorProgramTrackTest.xml + ../bin/CombinedProgramTrackTest --gtest_output=xml:results/cppCombinedProgramTrackTest.xml + ../bin/ReadStatusOnlyTest --gtest_output=xml:results/cppReadStatusOnlyTest.xml do_pyunit: @echo "running python unit tests" diff --git a/SRT/Servers/SRTMinorServo/test/ReadStatusOnlyTest.cpp b/SRT/Servers/SRTMinorServo/test/ReadStatusOnlyTest.cpp new file mode 100644 index 0000000000000000000000000000000000000000..833348bba83682a1edb8dbe629da2260c06f3451 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/test/ReadStatusOnlyTest.cpp @@ -0,0 +1,216 @@ +#include "gtest/gtest.h" +#include +#include +#include +#include +#include +#include "SRTMinorServoSocket.h" +#include "SRTMinorServoCommandLibrary.h" + +// This address and port are the ones set in the simulator +// In order for the test to properly be executed, the simulator should be launched with the following command: +// discos-simulator -s minor_servo start & +//#define SIMULATION +#ifdef SIMULATION + #define ADDRESS std::string("127.0.0.1") + #define PORT 12800 +#else + #define ADDRESS std::string("192.168.200.13") + #define PORT 4758 +#endif + +#define SOCKET_TIMEOUT 0.5 +#define STATUS_PERIOD 0.1 +#define SRP_COORDINATES std::vector{ "SRP_TX", "SRP_TY", "SRP_TZ", "SRP_RX", "SRP_RY", "SRP_RZ" } +#define DEROTATOR std::string("GFR1") +#define DEROTATOR_COORDINATES std::vector{ DEROTATOR + "_ROTATION" } + + +std::atomic terminate = false; + + +class ReadStatusOnlyTest : public ::testing::Test +{ +protected: + std::string directory; + + static void printStatus(std::string directory, bool timestamp_only=false) + { + SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance(); + SRTMinorServoAnswerMap SRPStatus, DerotatorStatus; + + ofstream SRPStatusFile, DerotatorStatusFile; + SRPStatusFile.open(directory + "/SRP/status.txt", ios::out); + SRPStatusFile << std::fixed << std::setprecision(6); + DerotatorStatusFile.open(directory + "/DEROTATOR/status.txt", ios::out); + DerotatorStatusFile << std::fixed << std::setprecision(6); + + long unsigned int counter = 0; + + double tn = CIRATools::getUNIXEpoch(); + + while(!terminate) + { + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP")); + DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("Derotatore" + DEROTATOR)); + std::string SRPStringStatus = serializeSRPStatus(SRPStatus); + std::string DerotatorStringStatus = serializeDerotatorStatus(DerotatorStatus); + + if(timestamp_only) + { + SRPStatusFile << std::get(SRPStatus["TIMESTAMP"]) << std::endl; + DerotatorStatusFile << std::get(DerotatorStatus["TIMESTAMP"]) << std::endl; + } + else + { + SRPStatusFile << SRPStringStatus << std::endl; + DerotatorStatusFile << DerotatorStringStatus << std::endl; + } + + if(counter % 10 == 0) + { + std::cout << SRPStringStatus << std::endl; + std::cout << DerotatorStringStatus << std::endl; + } + counter++; + + tn += STATUS_PERIOD; + std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, tn - CIRATools::getUNIXEpoch())))); + } + + SRPStatusFile.close(); + DerotatorStatusFile.close(); + } + + static void sigintHandler(int sig_num) + { + std::cout << std::endl << "Terminating..." << std::endl; + terminate = true; + } + + static std::string serializeSRPStatus(SRTMinorServoAnswerMap map) + { + std::string status = serializeCoordinates(std::get(map["TIMESTAMP"]), getCoordinates(map, SRP_COORDINATES)); + status += serializeElongations(getElongations(map)); + return status; + } + + static std::string serializeDerotatorStatus(SRTMinorServoAnswerMap map) + { + return serializeCoordinates(std::get(map["TIMESTAMP"]), getCoordinates(map, DEROTATOR_COORDINATES)); + } + + static std::string serializeCoordinates(double timestamp, std::vector coordinates) + { + std::stringstream stream; + stream << std::fixed << std::setprecision(6) << timestamp; + for(double coordinate : coordinates) + stream << "\t" << coordinate; + return stream.str(); + } + + static std::string serializeElongations(std::vector elongations) + { + std::stringstream stream; + stream << std::fixed << std::setprecision(6); + for(double elongation : elongations) + stream << "\t" << elongation; + return stream.str(); + } + + static std::vector getCoordinates(SRTMinorServoAnswerMap SRPStatus, std::vector coordinates) + { + std::vector currentCoordinates; + + for(std::string coordinate : coordinates) + { + auto value = SRPStatus[coordinate]; + + try + { + currentCoordinates.push_back(std::get(value)); + } + catch(std::bad_variant_access const& ex) + { + std::cout << ex.what() << ", variant index: " << value.index() << std::endl; + currentCoordinates.push_back(200.0); + } + } + + return currentCoordinates; + } + + static std::vector getElongations(SRTMinorServoAnswerMap SRPStatus) + { + std::vector currentElongations; + std::vector elongations = { "SRP_ELONG_Z1", "SRP_ELONG_Z2", "SRP_ELONG_Z3", "SRP_ELONG_Y1", "SRP_ELONG_Y2", "SRP_ELONG_X1" }; + + for(std::string elongation : elongations) + { + auto value = SRPStatus[elongation]; + + try + { + currentElongations.push_back(std::get(value)); + } + catch(std::bad_variant_access const& ex) + { + std::cout << ex.what() << ", variant index: " << value.index() << std::endl; + currentElongations.push_back(10000.0); + } + } + + return currentElongations; + } + + void SetUp() override + { + std::cout << std::fixed << std::setprecision(6); + + try + { + SRTMinorServoSocket::getInstance(ADDRESS, PORT, SOCKET_TIMEOUT); + std::cout << "Socket connected." << std::endl; + } + catch(ComponentErrors::SocketErrorExImpl& ex) + { + if(ex.getData("Reason") == std::string("Cannot connect the socket.").c_str()) + FAIL() << "Socket failed to connect. Check if the simulator or the hardware can be reached." << std::endl; + else + FAIL() << "Unexpected failure." << std::endl; + } + + signal(SIGINT, ReadStatusOnlyTest::sigintHandler); + + std::time_t tn = std::time(0); + std::tm* now = std::localtime(&tn); + std::stringstream directory_ss; + directory_ss << "TESTS/COMBINED"; + boost::filesystem::create_directory(directory_ss.str()); + directory_ss << "/"; + directory_ss << ::testing::UnitTest::GetInstance()->current_test_info()->name(); + directory_ss << "-"; + directory_ss << 1900 + now->tm_year; + directory_ss << std::setfill('0') << std::setw(2) << now->tm_mon + 1; + directory_ss << std::setfill('0') << std::setw(2) << now->tm_mday; + directory_ss << "-"; + directory_ss << std::setfill('0') << std::setw(2) << now->tm_hour; + directory_ss << std::setfill('0') << std::setw(2) << now->tm_min; + directory_ss << std::setfill('0') << std::setw(2) << now->tm_sec; + directory = directory_ss.str(); + boost::filesystem::create_directory(directory); + boost::filesystem::create_directory(directory + "/SRP"); + boost::filesystem::create_directory(directory + "/DEROTATOR"); + } + + void TearDown() override + { + SRTMinorServoSocket::destroyInstance(); + terminate = false; + } +}; + +TEST_F(ReadStatusOnlyTest, ReadStatusTest) +{ + ReadStatusOnlyTest::printStatus(directory, true); +} diff --git a/SRT/Servers/SRTMinorServo/test/SRPProgramTrackTest.cpp b/SRT/Servers/SRTMinorServo/test/SRPProgramTrackTest.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e1033748eb879d786d4ae5696ce16b36ea41c89c --- /dev/null +++ b/SRT/Servers/SRTMinorServo/test/SRPProgramTrackTest.cpp @@ -0,0 +1,632 @@ +#include "gtest/gtest.h" +#include +#include +#include +#include +#include +#include "SRTMinorServoTestingSocket.h" +#include "SRTMinorServoCommandLibrary.h" + +// This address and port are the ones set in the simulator +// In order for the test to properly be executed, the simulator should be launched with the following command: +// discos-simulator -s minor_servo start & +//#define SIMULATION +#ifdef SIMULATION + #define ADDRESS std::string("127.0.0.1") + #define PORT 12800 +#else + #define ADDRESS std::string("192.168.200.13") + #define PORT 4758 +#endif +#define NOISE_THRESHOLD 1 +#define TIMEGAP 0.2 +#define ADVANCE_TIMEGAP 2.6 +#define MAX_RANGES std::vector{ 40, 40, 40, 0.2, 0.2, 0.2 } +#define MAX_RANGES std::vector{ 50, 110, 50, 0.25, 0.25, 0.25 } +#define MAX_SPEED std::vector{ 4, 4, 4, 0.04, 0.04, 0.04 } +#define STATUS_PERIOD 0.01 + +std::atomic terminate = false; + +using namespace MinorServo; +using namespace IRA; + + +class SRPProgramTrackTest : public ::testing::Test +{ +protected: + std::vector startingCoordinates = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + std::string directory; + std::thread statusThread; + + static void printStatus(std::string filename) + { + SRTMinorServoSocket& socket = SRTMinorServoTestingSocket::getInstance(); + SRTMinorServoAnswerMap SRPStatus; + + ofstream statusFile; + statusFile.open(filename, ios::out); + + long unsigned int counter = 0; + + double tn = CIRATools::getUNIXEpoch(); + + while(!terminate) + { + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP")); + std::string status = serializeStatus(SRPStatus); + + statusFile << status << std::endl; + if(counter % 10 == 0) + std::cout << status << std::endl; + counter++; + + tn += STATUS_PERIOD; + std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, tn - CIRATools::getUNIXEpoch())))); + } + + statusFile.close(); + } + + static void sigintHandler(int sig_num) + { + std::cout << std::endl << "Terminating..." << std::endl; + terminate = true; + } + + static std::string serializeStatus(SRTMinorServoAnswerMap map) + { + std::string status; + try + { + status = serializeCoordinates(CIRATools::ACSTime2UNIXEpoch(map.getTimestamp()), getCoordinates(map)); + } + catch(std::bad_variant_access const& ex) + { + std::cout << "Bad timestamp!" << std::endl; + + /*SRTMinorServoAnswerMap::iterator iterator; + for(iterator = map.begin(); iterator != map.end(); ++iterator) + { + std::visit([iterator](const auto& var) mutable { std::cout << iterator->first << ": " << var << std::endl; }, iterator->second); + }*/ + } + status += serializeElongations(getElongations(map)); + return status; + } + + static std::string serializeCoordinates(double timestamp, std::vector coordinates) + { + std::stringstream stream; + stream << std::fixed << std::setprecision(6) << timestamp; + for(double coordinate : coordinates) + stream << "\t" << coordinate; + return stream.str(); + } + + static std::string serializeElongations(std::vector elongations) + { + std::stringstream stream; + stream << std::fixed << std::setprecision(6); + for(double elongation : elongations) + stream << "\t" << elongation; + return stream.str(); + } + + static std::vector getCoordinates(SRTMinorServoAnswerMap SRPStatus) + { + std::vector currentCoordinates; + std::vector coordinates = { "SRP_TX", "SRP_TY", "SRP_TZ", "SRP_RX", "SRP_RY", "SRP_RZ" }; + + for(std::string coordinate : coordinates) + { + try + { + currentCoordinates.push_back(SRPStatus.get(coordinate)); + } + catch(std::bad_variant_access const& ex) + { + std::cout << "Bad floating point coordinate:" << SRPStatus.get(coordinate) << std::endl; + currentCoordinates.push_back(double(SRPStatus.get(coordinate))); + } + } + + return currentCoordinates; + } + + static std::vector getElongations(SRTMinorServoAnswerMap SRPStatus) + { + std::vector currentElongations; + std::vector elongations = { "SRP_ELONG_Z1", "SRP_ELONG_Z2", "SRP_ELONG_Z3", "SRP_ELONG_Y1", "SRP_ELONG_Y2", "SRP_ELONG_X1" }; + + for(std::string elongation : elongations) + { + try + { + currentElongations.push_back(SRPStatus.get(elongation)); + } + catch(std::bad_variant_access const& ex) + { + std::cout << "Bad floating point elongation:" << SRPStatus.get(elongation) << std::endl; + currentElongations.push_back(double(SRPStatus.get(elongation))); + } + } + + return currentElongations; + } + + static bool moveAxis(std::vector &coordinates, int axis_to_move, int sign) + { + double starting_sign = coordinates[axis_to_move] > 0 ? 1 : (coordinates[axis_to_move] < 0 ? -1 : 0); + double offset_to_add = MAX_SPEED[axis_to_move] / 5; + coordinates[axis_to_move] += sign * offset_to_add; + double ending_sign = coordinates[axis_to_move] > 0 ? 1 : (coordinates[axis_to_move] < 0 ? -1 : 0); + if(starting_sign == -1 && ending_sign >= 0) + { + // Zero crossed + coordinates[axis_to_move] = 0.0; + return false; + } + if(fabs(coordinates[axis_to_move]) >= MAX_RANGES[axis_to_move]) + { + coordinates[axis_to_move] = sign * MAX_RANGES[axis_to_move]; + return true; + } + return false; + } + + void SetUp() override + { + srand((int)CIRATools::getUNIXEpoch()); + std::cout << std::fixed << std::setprecision(6); + + try + { + SRTMinorServoTestingSocket::getInstance(ADDRESS, PORT, 0.2); + std::cout << "Socket connected." << std::endl; + } + catch(ComponentErrors::SocketErrorExImpl& ex) + { + if(ex.getData("Reason") == std::string("Cannot connect the socket.").c_str()) + FAIL() << "Socket failed to connect. Check if the simulator or the hardware can be reached." << std::endl; + else + FAIL() << "Unexpected failure." << std::endl; + } + + SRTMinorServoSocket& socket = SRTMinorServoTestingSocket::getInstance(); + + std::cout << "Sending MS STATUS command..."; + + SRTMinorServoAnswerMap MSStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status()); + EXPECT_TRUE(MSStatus.checkOutput()); + EXPECT_EQ(MSStatus.get("CONTROL"), 1); + //EXPECT_EQ(MSStatus.get("POWER"), 1); + EXPECT_EQ(MSStatus.get("EMERGENCY"), 2); + //EXPECT_EQ(MSStatus.get("ENABLED"), 1); + std::cout << "OK." << std::endl; + + /*SRTMinorServoAnswerMap::iterator iterator; + for(iterator = MSStatus.begin(); iterator != MSStatus.end(); ++iterator) + { + std::visit([iterator](const auto& var) mutable { std::cout << iterator->first << ": " << var << std::endl; }, iterator->second); + }*/ + + std::cout << "Sending initial SRP STATUS command..."; + + SRTMinorServoAnswerMap SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP")); + EXPECT_TRUE(SRPStatus.checkOutput()); + //EXPECT_EQ(SRPStatus.get("SRP_STATUS"), 1); + EXPECT_EQ(SRPStatus.get("SRP_BLOCK"), 2); + std::cout << "OK." << std::endl; + + /*for(iterator = SRPStatus.begin(); iterator != SRPStatus.end(); ++iterator) + { + std::visit([iterator](const auto& var) mutable { std::cout << iterator->first << ": " << var << std::endl; }, iterator->second); + }*/ + + std::cout << "Sending all axes to the starting position..." << std::endl; + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::preset("SRP", startingCoordinates)); + EXPECT_TRUE(SRPStatus.checkOutput()); + + signal(SIGINT, SRPProgramTrackTest::sigintHandler); + + do + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP")); + EXPECT_TRUE(SRPStatus.checkOutput()); + std::cout << serializeStatus(SRPStatus) << std::endl; + + if(terminate) + FAIL() << "Aborting test..." << std::endl; + } + while(SRPStatus.get("SRP_OPERATIVE_MODE") != 40); + EXPECT_EQ(SRPStatus.get("SRP_OPERATIVE_MODE"), 40); + + std::cout << "OK." << std::endl; + + startingCoordinates = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + std::time_t tn = std::time(0); + std::tm* now = std::localtime(&tn); + std::stringstream directory_ss; + directory_ss << "TESTS/SRP"; + boost::filesystem::create_directory(directory_ss.str()); + directory_ss << "/"; + directory_ss << ::testing::UnitTest::GetInstance()->current_test_info()->name(); + directory_ss << "-"; + directory_ss << 1900 + now->tm_year; + directory_ss << std::setfill('0') << std::setw(2) << now->tm_mon + 1; + directory_ss << std::setfill('0') << std::setw(2) << now->tm_mday; + directory_ss << "-"; + directory_ss << std::setfill('0') << std::setw(2) << now->tm_hour; + directory_ss << std::setfill('0') << std::setw(2) << now->tm_min; + directory_ss << std::setfill('0') << std::setw(2) << now->tm_sec; + directory = directory_ss.str(); + boost::filesystem::create_directory(directory); + + statusThread = std::thread(&SRPProgramTrackTest::printStatus, directory + "/status.txt"); + } + + void TearDown() override + { + SRTMinorServoTestingSocket::destroyInstance(); + terminate = false; + } +}; + +TEST_F(SRPProgramTrackTest, ContinuousMovementTest) +{ + double start_time = CIRATools::getUNIXEpoch() + ADVANCE_TIMEGAP; + std::cout << "PRESET position reached, starting PROGRAMTRACK with start time: " << start_time << std::endl; + long unsigned int trajectory_id = int(start_time); + unsigned int point_id = 0; + std::vector programTrackCoordinates = startingCoordinates; + + SRTMinorServoSocket& socket = SRTMinorServoTestingSocket::getInstance(); + SRTMinorServoAnswerMap SRPStatus; + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("SRP", trajectory_id, point_id, programTrackCoordinates, start_time)); + EXPECT_TRUE(SRPStatus.checkOutput()); + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP")); + EXPECT_TRUE(SRPStatus.checkOutput()); + EXPECT_EQ(SRPStatus.get("SRP_OPERATIVE_MODE"), 50); + + ofstream programTrackFile; + programTrackFile.open(directory + "/trajectory.txt", ios::out); + programTrackFile << SRPProgramTrackTest::serializeCoordinates(start_time, programTrackCoordinates) << std::endl; + + double next_expected_time = start_time; + unsigned int axis_to_move = 0; + int sign = 1; + unsigned int idle_count = 0; + bool idle = false; + + while(!terminate) + { + while(!terminate) + { + next_expected_time += TIMEGAP; + + std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, next_expected_time - ADVANCE_TIMEGAP - CIRATools::getUNIXEpoch())))); + point_id++; + + if(idle) + { + idle_count++; + if(idle_count == 5) + { + idle_count = 0; + idle = false; + } + } + else if(moveAxis(programTrackCoordinates, axis_to_move, sign)) + { + sign *= -1; + idle = true; + } + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("SRP", trajectory_id, point_id, programTrackCoordinates)); + EXPECT_TRUE(SRPStatus.checkOutput()); + //std::cout << SRPProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl; + programTrackFile << SRPProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl; + + if(round(programTrackCoordinates[axis_to_move] * 100) == 0 && sign == 1) + { + programTrackCoordinates[axis_to_move] = 0.0; + break; + } + } + + axis_to_move == 5 ? axis_to_move = 0 : axis_to_move++; + } + + programTrackFile.close(); + statusThread.join(); +} + +TEST_F(SRPProgramTrackTest, AllAxesMovementTest) +{ + double start_time = CIRATools::getUNIXEpoch() + ADVANCE_TIMEGAP; + std::cout << "PRESET position reached, starting PROGRAMTRACK with start time: " << start_time << std::endl; + long unsigned int trajectory_id = int(start_time); + unsigned int point_id = 0; + std::vector programTrackCoordinates = startingCoordinates; + + SRTMinorServoSocket& socket = SRTMinorServoTestingSocket::getInstance(); + SRTMinorServoAnswerMap SRPStatus; + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("SRP", trajectory_id, point_id, programTrackCoordinates, start_time)); + EXPECT_TRUE(SRPStatus.checkOutput()); + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP")); + EXPECT_TRUE(SRPStatus.checkOutput()); + EXPECT_EQ(SRPStatus.get("SRP_OPERATIVE_MODE"), 50); + + ofstream programTrackFile; + programTrackFile.open(directory + "/trajectory.txt", ios::out); + programTrackFile << SRPProgramTrackTest::serializeCoordinates(start_time, programTrackCoordinates) << std::endl; + + double next_expected_time = start_time; + std::vector sign = { 1, 1, 1 }; + std::vector idle_count = { 0, 0, 0 }; + std::vector idle = { false, false, false }; + + while(!terminate) + { + next_expected_time += TIMEGAP; + + std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, next_expected_time - ADVANCE_TIMEGAP - CIRATools::getUNIXEpoch())))); + point_id++; + + for(size_t axis = 0; axis < 3; axis++) + { + if(!idle[axis]) + { + if(moveAxis(programTrackCoordinates, axis, sign[axis])) + { + sign[axis] *= -1; + idle[axis] = true; + } + } + else + { + idle_count[axis]++; + if(idle_count[axis] == 5) + { + idle_count[axis] = 0; + idle[axis] = false; + } + } + } + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("SRP", trajectory_id, point_id, programTrackCoordinates)); + EXPECT_TRUE(SRPStatus.checkOutput()); + //std::cout << SRPProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl; + programTrackFile << SRPProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl; + + for(size_t axis = 0; axis < 3; axis++) + { + if(round(programTrackCoordinates[axis] * 100) == 0 && sign[axis] == 1) + { + programTrackCoordinates[axis] = 0.0; + } + } + } + + programTrackFile.close(); + statusThread.join(); +} + +TEST_F(SRPProgramTrackTest, SineWaveMovementTest) +{ + double start_time = CIRATools::getUNIXEpoch() + ADVANCE_TIMEGAP; + std::cout << "PRESET position reached, starting PROGRAMTRACK with start time: " << start_time << std::endl; + long unsigned int trajectory_id = int(start_time); + unsigned int point_id = 0; + std::vector programTrackCoordinates = startingCoordinates; + + std::vector phase_shift; + std::vector period; + for(size_t axis = 0; axis < 6; axis++) + { + double period_multiplier = axis < 3 ? 4 : 4; + period.push_back(MAX_RANGES[axis] / MAX_SPEED[axis] * period_multiplier); + phase_shift.push_back((double)std::rand() / RAND_MAX * period[axis]); + programTrackCoordinates[axis] = MAX_RANGES[axis] * sin(phase_shift[axis] * 2 * M_PI / period[axis]); + } + + SRTMinorServoSocket& socket = SRTMinorServoTestingSocket::getInstance(); + SRTMinorServoAnswerMap SRPStatus; + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("SRP", trajectory_id, point_id, programTrackCoordinates, start_time)); + EXPECT_TRUE(SRPStatus.checkOutput()); + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP")); + EXPECT_TRUE(SRPStatus.checkOutput()); + EXPECT_EQ(SRPStatus.get("SRP_OPERATIVE_MODE"), 50); + + ofstream programTrackFile; + programTrackFile.open(directory + "/trajectory.txt", ios::out); + programTrackFile << SRPProgramTrackTest::serializeCoordinates(start_time, programTrackCoordinates) << std::endl; + + double next_expected_time = start_time; + + while(!terminate) + { + next_expected_time += TIMEGAP; + double time_delta = next_expected_time - start_time; + + std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, next_expected_time - ADVANCE_TIMEGAP - CIRATools::getUNIXEpoch())))); + point_id++; + + for(size_t axis = 0; axis < 6; axis++) + { + programTrackCoordinates[axis] = MAX_RANGES[axis] * sin((time_delta + phase_shift[axis]) * 2 * M_PI / period[axis]); + } + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("SRP", trajectory_id, point_id, programTrackCoordinates)); + EXPECT_TRUE(SRPStatus.checkOutput()); + programTrackFile << SRPProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl; + } + + programTrackFile.close(); + statusThread.join(); +} + +TEST_F(SRPProgramTrackTest, SeparateMovementTest) +{ + SRTMinorServoSocket& socket = SRTMinorServoTestingSocket::getInstance(); + SRTMinorServoAnswerMap SRPStatus; + + ofstream programTrackFile; + programTrackFile.open(directory + "/trajectory.txt", ios::out); + + unsigned int axis_to_move = 0; + int sign = 1; + unsigned int idle_count = 0; + bool idle = false; + + std::cout << "PRESET position reached, starting PROGRAMTRACK" << std::endl; + std::vector programTrackCoordinates = startingCoordinates; + + bool immediate = true; + + while(!terminate) + { + double start_time = CIRATools::getUNIXEpoch() + ADVANCE_TIMEGAP; + long unsigned int trajectory_id = int(start_time); + double next_expected_time = start_time; + unsigned int point_id = 0; + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("SRP", trajectory_id, point_id, programTrackCoordinates, start_time)); + EXPECT_TRUE(SRPStatus.checkOutput()); + programTrackFile << SRPProgramTrackTest::serializeCoordinates(start_time, programTrackCoordinates) << std::endl; + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP")); + EXPECT_TRUE(SRPStatus.checkOutput()); + EXPECT_EQ(SRPStatus.get("SRP_OPERATIVE_MODE"), 50); + + while(!terminate) + { + next_expected_time += TIMEGAP; + + std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, next_expected_time - ADVANCE_TIMEGAP - CIRATools::getUNIXEpoch())))); + point_id++; + + if(idle) + { + idle_count++; + if(idle_count == (ADVANCE_TIMEGAP / TIMEGAP) || immediate) + { + idle_count = 0; + idle = false; + break; + } + } + else + { + if(moveAxis(programTrackCoordinates, axis_to_move, sign)) + { + sign *= -1; + idle = true; + } + else if(programTrackCoordinates[axis_to_move] == 0.0 && sign == 1) + { + axis_to_move == 5 ? axis_to_move = 0 : axis_to_move++; + idle = true; + } + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("SRP", trajectory_id, point_id, programTrackCoordinates)); + EXPECT_TRUE(SRPStatus.checkOutput()); + } + + //std::cout << SRPProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl; + programTrackFile << SRPProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl; + } + + //immediate = immediate ? false : true; + } + + programTrackFile.close(); + statusThread.join(); +} + +TEST_F(SRPProgramTrackTest, RapidTrajectoryTest) +{ + SRTMinorServoSocket& socket = SRTMinorServoTestingSocket::getInstance(); + SRTMinorServoAnswerMap SRPStatus; + + ofstream programTrackFile; + programTrackFile.open(directory + "/trajectory.txt", ios::out); + + unsigned int axis_to_move = 2; //Always Z + int sign = -1; + unsigned int idle_count = 0; + bool idle = false; + + std::cout << "PRESET position reached, starting PROGRAMTRACK" << std::endl; + + while(!terminate) + { + std::vector programTrackCoordinates = startingCoordinates; + programTrackCoordinates[axis_to_move] = MAX_RANGES[axis_to_move]; + + double start_time = CIRATools::getUNIXEpoch() + ADVANCE_TIMEGAP; + long unsigned int trajectory_id = int(start_time); + double next_expected_time = start_time; + unsigned int point_id = 0; + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("SRP", trajectory_id, point_id, programTrackCoordinates, start_time)); + EXPECT_TRUE(SRPStatus.checkOutput()); + programTrackFile << SRPProgramTrackTest::serializeCoordinates(start_time, programTrackCoordinates) << std::endl; + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP")); + EXPECT_TRUE(SRPStatus.checkOutput()); + EXPECT_EQ(SRPStatus.get("SRP_OPERATIVE_MODE"), 50); + + while(!terminate) + { + next_expected_time += TIMEGAP; + + std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, next_expected_time - ADVANCE_TIMEGAP - CIRATools::getUNIXEpoch())))); + point_id++; + + if(idle) + { + idle_count++; + if(idle_count == ADVANCE_TIMEGAP / TIMEGAP) + { + idle_count = 0; + idle = false; + break; + } + } + else + { + if(moveAxis(programTrackCoordinates, axis_to_move, sign)) + { + idle = true; + } + + SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("SRP", trajectory_id, point_id, programTrackCoordinates)); + EXPECT_TRUE(SRPStatus.checkOutput()); + } + + //std::cout << SRPProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl; + programTrackFile << SRPProgramTrackTest::serializeCoordinates(next_expected_time, programTrackCoordinates) << std::endl; + } + } + + programTrackFile.close(); + statusThread.join(); +} diff --git a/SRT/Servers/SRTMinorServo/test/TESTS/plotCombined.py b/SRT/Servers/SRTMinorServo/test/TESTS/plotCombined.py new file mode 100755 index 0000000000000000000000000000000000000000..87127cccb859a1c1aefb30f762d9779645ce1c37 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/test/TESTS/plotCombined.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python +import os +import numpy as np +import matplotlib.pyplot as plt +from argparse import ArgumentParser + +parser = ArgumentParser() +parser.add_argument( + 'directory', + help="Directory containing the 'status.txt' and 'trajectory.txt' files" +) + +arguments = parser.parse_args() + +srp_status_time = [] +derotator_status_time = [] +status_tx = [] +status_ty = [] +status_tz = [] +status_rx = [] +status_ry = [] +status_rz = [] +status_rotation = [] +starttime = None + +with open(os.path.join(arguments.directory, 'SRP', 'status.txt'), 'r') as statusfile: + for line in statusfile: + args = line.strip().split() + if not starttime: + starttime = float(args[0]) + srp_status_time.append(float(args[0]) - starttime) + status_tx.append(float(args[1])) + status_ty.append(float(args[2])) + status_tz.append(float(args[3])) + status_rx.append(float(args[4])) + status_ry.append(float(args[5])) + status_rz.append(float(args[6])) + +with open(os.path.join(arguments.directory, 'DEROTATOR', 'status.txt'), 'r') as statusfile: + for line in statusfile: + args = line.strip().split() + derotator_status_time.append(float(args[0]) - starttime) + status_rotation.append(float(args[1])) + +trajectory_time = [] +trajectory_tx = [] +trajectory_ty = [] +trajectory_tz = [] +trajectory_rx = [] +trajectory_ry = [] +trajectory_rz = [] +trajectory_rotation = [] + +with open(os.path.join(arguments.directory, 'SRP', 'trajectory.txt'), 'r') as trajectoryfile: + for line in trajectoryfile: + args = line.strip().split() + trajectory_time.append(float(args[0]) - starttime) + trajectory_tx.append(float(args[1])) + trajectory_ty.append(float(args[2])) + trajectory_tz.append(float(args[3])) + trajectory_rx.append(float(args[4])) + trajectory_ry.append(float(args[5])) + trajectory_rz.append(float(args[6])) + +with open(os.path.join(arguments.directory, 'DEROTATOR', 'trajectory.txt'), 'r') as trajectoryfile: + for line in trajectoryfile: + args = line.strip().split() + trajectory_rotation.append(float(args[1])) + +fig, axs = plt.subplots(7, sharex=True) +fig.canvas.manager.set_window_title('Program Track Positions') +fig.suptitle('Program Track Positions') + +axs[0].plot(srp_status_time, status_tx, 'r') +axs[0].plot(trajectory_time, trajectory_tx, 'b') +axs[0].set_ylabel('tx (mm)') +axs[0].sharey(axs[1]) + +axs[1].plot(srp_status_time, status_ty, 'r') +axs[1].plot(trajectory_time, trajectory_ty, 'b') +axs[1].set_ylabel('ty (mm)') +axs[1].sharey(axs[2]) + +axs[2].plot(srp_status_time, status_tz, 'r') +axs[2].plot(trajectory_time, trajectory_tz, 'b') +axs[2].set_ylabel('tz (mm)') + +axs[3].plot(srp_status_time, status_rx, 'r') +axs[3].plot(trajectory_time, trajectory_rx, 'b') +axs[3].set_ylabel('rx (deg)') +axs[3].sharey(axs[4]) + +axs[4].plot(srp_status_time, status_ry, 'r') +axs[4].plot(trajectory_time, trajectory_ry, 'b') +axs[4].set_ylabel('ry (deg)') +axs[4].sharey(axs[5]) + +axs[5].plot(srp_status_time, status_rz, 'r') +axs[5].plot(trajectory_time, trajectory_rz, 'b') +axs[5].set_ylabel('rz (deg)') + +axs[6].plot(derotator_status_time, status_rotation, 'r') +axs[6].plot(trajectory_time, trajectory_rotation, 'b') +axs[6].set_ylabel('derot (deg)') +axs[6].set_xlabel('time (s)') + + +plt.get_current_fig_manager().window.attributes('-zoomed', True) +try: + plt.show() +except KeyboardInterrupt: + pass diff --git a/SRT/Servers/SRTMinorServo/test/TESTS/plotDerotatorTrajectory.py b/SRT/Servers/SRTMinorServo/test/TESTS/plotDerotatorTrajectory.py new file mode 100755 index 0000000000000000000000000000000000000000..649ae367a2072737d9c22c1f143de71dafa5cae1 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/test/TESTS/plotDerotatorTrajectory.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python +import os +import numpy as np +import matplotlib.pyplot as plt +from argparse import ArgumentParser + +parser = ArgumentParser() +parser.add_argument( + 'directory', + help="Directory containing the 'status.txt' and 'trajectory.txt' files" +) + +arguments = parser.parse_args() + +status_time = [] +status_rotation = [] +starttime = None + +with open(os.path.join(arguments.directory, 'status.txt'), 'r') as statusfile: + for line in statusfile: + args = line.strip().split() + if not starttime: + starttime = float(args[0]) + status_time.append(float(args[0]) - starttime) + status_rotation.append(float(args[1])) + +trajectory_time = [] +trajectory_rotation = [] + +with open(os.path.join(arguments.directory, 'trajectory.txt'), 'r') as trajectoryfile: + for line in trajectoryfile: + args = line.strip().split() + trajectory_time.append(float(args[0]) - starttime) + trajectory_rotation.append(float(args[1])) + +#plt.canvas.manager.set_window_title('Derotator positions') +plt.suptitle('Derotator positions') +plt.plot(status_time, status_rotation, 'r') +plt.plot(trajectory_time, trajectory_rotation, 'b') +plt.ylabel('rot (deg)') +plt.xlabel('time (sec)') + +plt.get_current_fig_manager().window.attributes('-zoomed', True) +try: + plt.show() +except KeyboardInterrupt: + pass diff --git a/SRT/Servers/SRTMinorServo/test/TESTS/plotSRPTrajectories.py b/SRT/Servers/SRTMinorServo/test/TESTS/plotSRPTrajectories.py new file mode 100755 index 0000000000000000000000000000000000000000..e2f39fe779e03acfd6487dea35f2e36213c10471 --- /dev/null +++ b/SRT/Servers/SRTMinorServo/test/TESTS/plotSRPTrajectories.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python +import os +import numpy as np +import matplotlib.pyplot as plt +from argparse import ArgumentParser + +parser = ArgumentParser() +parser.add_argument( + 'directory', + help="Directory containing the 'status.txt' and 'trajectory.txt' files" +) + +arguments = parser.parse_args() + +status_time = [] +status_tx = [] +status_ty = [] +status_tz = [] +status_rx = [] +status_ry = [] +status_rz = [] +status_ez1 = [] +status_ez2 = [] +status_ez3 = [] +status_ey1 = [] +status_ey2 = [] +status_ex1 = [] +starttime = None + +with open(os.path.join(arguments.directory, 'status.txt'), 'r') as statusfile: + for line in statusfile: + args = line.strip().split() + if not starttime: + starttime = float(args[0]) + status_time.append(float(args[0]) - starttime) + status_tx.append(float(args[1])) + status_ty.append(float(args[2])) + status_tz.append(float(args[3])) + status_rx.append(float(args[4])) + status_ry.append(float(args[5])) + status_rz.append(float(args[6])) + status_ez1.append(float(args[7])) + status_ez2.append(float(args[8])) + status_ez3.append(float(args[9])) + status_ey1.append(float(args[10])) + status_ey2.append(float(args[11])) + status_ex1.append(float(args[12])) + +trajectory_time = [] +trajectory_tx = [] +trajectory_ty = [] +trajectory_tz = [] +trajectory_rx = [] +trajectory_ry = [] +trajectory_rz = [] + +with open(os.path.join(arguments.directory, 'trajectory.txt'), 'r') as trajectoryfile: + for line in trajectoryfile: + args = line.strip().split() + trajectory_time.append(float(args[0]) - starttime) + trajectory_tx.append(float(args[1])) + trajectory_ty.append(float(args[2])) + trajectory_tz.append(float(args[3])) + trajectory_rx.append(float(args[4])) + trajectory_ry.append(float(args[5])) + trajectory_rz.append(float(args[6])) + +fig = plt.figure() +fig.canvas.manager.set_window_title('SRP Positions') +fig.suptitle('SRP Positions') + +gs = fig.add_gridspec(6, 2, hspace=0) +((tx, ex1), (ty, ey1), (tz, ey2), (rx, ez1), (ry, ez2), (rz, ez3)) = gs.subplots(sharex=True) + +tx.plot(status_time, status_tx, 'r') +tx.plot(trajectory_time, trajectory_tx, 'b') +tx.set_ylabel('tx (mm)') +tx.sharey(ty) + +ty.plot(status_time, status_ty, 'r') +ty.plot(trajectory_time, trajectory_ty, 'b') +ty.set_ylabel('ty (mm)') +ty.sharey(tz) + +tz.plot(status_time, status_tz, 'r') +tz.plot(trajectory_time, trajectory_tz, 'b') +tz.set_ylabel('tz (mm)') +tz.sharey(ex1) + +rx.plot(status_time, status_rx, 'r') +rx.plot(trajectory_time, trajectory_rx, 'b') +rx.set_ylabel('rx (deg)') +rx.sharey(ry) + +ry.plot(status_time, status_ry, 'r') +ry.plot(trajectory_time, trajectory_ry, 'b') +ry.set_ylabel('ry (deg)') +ry.sharey(rz) + +rz.plot(status_time, status_rz, 'r') +rz.plot(trajectory_time, trajectory_rz, 'b') +rz.set_ylabel('rz (deg)') +rz.set_xlabel('time (s)') + + +ex1.plot(status_time, status_ex1, 'g') +ex1.set_ylabel('ex1 (mm)') +ex1.sharey(ey1) + +ey1.plot(status_time, status_ey1, 'g') +ey1.set_ylabel('ey1 (mm)') +ey1.sharey(ey2) + +ey2.plot(status_time, status_ey2, 'g') +ey2.set_ylabel('ey2 (mm)') +ey2.sharey(ez1) + +ez1.plot(status_time, status_ez1, 'g') +ez1.set_ylabel('ez1 (mm)') +ez1.sharey(ez2) + +ez2.plot(status_time, status_ez2, 'g') +ez2.set_ylabel('ez2 (mm)') +ez2.sharey(ez3) + +ez3.plot(status_time, status_ez3, 'g') +ez3.set_ylabel('ez3 (mm)') +ez3.set_xlabel('time (s)') + +plt.get_current_fig_manager().window.attributes('-zoomed', True) +try: + plt.show() +except KeyboardInterrupt: + pass diff --git a/SRT/Servers/SRTMinorServo/config/CDB/schemas/MinorServo.xsd b/SRT/Servers/SRTOldMinorServo/config/CDB/schemas/MinorServo.xsd similarity index 100% rename from SRT/Servers/SRTMinorServo/config/CDB/schemas/MinorServo.xsd rename to SRT/Servers/SRTOldMinorServo/config/CDB/schemas/MinorServo.xsd diff --git a/SRT/Servers/SRTMinorServo/doc/derotator/derotator.pdf b/SRT/Servers/SRTOldMinorServo/doc/derotator/derotator.pdf similarity index 100% rename from SRT/Servers/SRTMinorServo/doc/derotator/derotator.pdf rename to SRT/Servers/SRTOldMinorServo/doc/derotator/derotator.pdf diff --git a/SRT/Servers/SRTMinorServo/doc/derotator/icd.pdf b/SRT/Servers/SRTOldMinorServo/doc/derotator/icd.pdf similarity index 100% rename from SRT/Servers/SRTMinorServo/doc/derotator/icd.pdf rename to SRT/Servers/SRTOldMinorServo/doc/derotator/icd.pdf diff --git a/SRT/Servers/SRTMinorServo/doc/derotator/icd_bus.pdf b/SRT/Servers/SRTOldMinorServo/doc/derotator/icd_bus.pdf similarity index 100% rename from SRT/Servers/SRTMinorServo/doc/derotator/icd_bus.pdf rename to SRT/Servers/SRTOldMinorServo/doc/derotator/icd_bus.pdf diff --git a/SRT/Servers/SRTMinorServo/doc/derotator/sensor.pdf b/SRT/Servers/SRTOldMinorServo/doc/derotator/sensor.pdf similarity index 100% rename from SRT/Servers/SRTMinorServo/doc/derotator/sensor.pdf rename to SRT/Servers/SRTOldMinorServo/doc/derotator/sensor.pdf diff --git a/SRT/Servers/SRTMinorServo/doc/minor_servo/gimbal.pdf b/SRT/Servers/SRTOldMinorServo/doc/minor_servo/gimbal.pdf similarity index 100% rename from SRT/Servers/SRTMinorServo/doc/minor_servo/gimbal.pdf rename to SRT/Servers/SRTOldMinorServo/doc/minor_servo/gimbal.pdf diff --git a/SRT/Servers/SRTMinorServo/doc/minor_servo/minor_servo_system.pdf b/SRT/Servers/SRTOldMinorServo/doc/minor_servo/minor_servo_system.pdf similarity index 100% rename from SRT/Servers/SRTMinorServo/doc/minor_servo/minor_servo_system.pdf rename to SRT/Servers/SRTOldMinorServo/doc/minor_servo/minor_servo_system.pdf diff --git a/SRT/Servers/SRTMinorServo/include/DerotatorImpl.h b/SRT/Servers/SRTOldMinorServo/include/DerotatorImpl.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/DerotatorImpl.h rename to SRT/Servers/SRTOldMinorServo/include/DerotatorImpl.h diff --git a/SRT/Servers/SRTMinorServo/include/DevIOASConfiguration.h b/SRT/Servers/SRTOldMinorServo/include/DevIOASConfiguration.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/DevIOASConfiguration.h rename to SRT/Servers/SRTOldMinorServo/include/DevIOASConfiguration.h diff --git a/SRT/Servers/SRTMinorServo/include/DevIOActualSetup.h b/SRT/Servers/SRTOldMinorServo/include/DevIOActualSetup.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/DevIOActualSetup.h rename to SRT/Servers/SRTOldMinorServo/include/DevIOActualSetup.h diff --git a/SRT/Servers/SRTMinorServo/include/DevIOElevationTrack.h b/SRT/Servers/SRTOldMinorServo/include/DevIOElevationTrack.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/DevIOElevationTrack.h rename to SRT/Servers/SRTOldMinorServo/include/DevIOElevationTrack.h diff --git a/SRT/Servers/SRTMinorServo/include/DevIOMotionInfo.h b/SRT/Servers/SRTOldMinorServo/include/DevIOMotionInfo.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/DevIOMotionInfo.h rename to SRT/Servers/SRTOldMinorServo/include/DevIOMotionInfo.h diff --git a/SRT/Servers/SRTMinorServo/include/DevIOParking.h b/SRT/Servers/SRTOldMinorServo/include/DevIOParking.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/DevIOParking.h rename to SRT/Servers/SRTOldMinorServo/include/DevIOParking.h diff --git a/SRT/Servers/SRTMinorServo/include/DevIOReady.h b/SRT/Servers/SRTOldMinorServo/include/DevIOReady.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/DevIOReady.h rename to SRT/Servers/SRTOldMinorServo/include/DevIOReady.h diff --git a/SRT/Servers/SRTMinorServo/include/DevIOScanActive.h b/SRT/Servers/SRTOldMinorServo/include/DevIOScanActive.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/DevIOScanActive.h rename to SRT/Servers/SRTOldMinorServo/include/DevIOScanActive.h diff --git a/SRT/Servers/SRTMinorServo/include/DevIOScanning.h b/SRT/Servers/SRTOldMinorServo/include/DevIOScanning.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/DevIOScanning.h rename to SRT/Servers/SRTOldMinorServo/include/DevIOScanning.h diff --git a/SRT/Servers/SRTMinorServo/include/DevIOStarting.h b/SRT/Servers/SRTOldMinorServo/include/DevIOStarting.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/DevIOStarting.h rename to SRT/Servers/SRTOldMinorServo/include/DevIOStarting.h diff --git a/SRT/Servers/SRTMinorServo/include/DevIOTracking.h b/SRT/Servers/SRTOldMinorServo/include/DevIOTracking.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/DevIOTracking.h rename to SRT/Servers/SRTOldMinorServo/include/DevIOTracking.h diff --git a/SRT/Servers/SRTMinorServo/include/MSBossConfiguration.h b/SRT/Servers/SRTOldMinorServo/include/MSBossConfiguration.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/MSBossConfiguration.h rename to SRT/Servers/SRTOldMinorServo/include/MSBossConfiguration.h diff --git a/SRT/Servers/SRTMinorServo/include/MSBossPublisher.h b/SRT/Servers/SRTOldMinorServo/include/MSBossPublisher.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/MSBossPublisher.h rename to SRT/Servers/SRTOldMinorServo/include/MSBossPublisher.h diff --git a/SRT/Servers/SRTMinorServo/include/MSParameters.h b/SRT/Servers/SRTOldMinorServo/include/MSParameters.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/MSParameters.h rename to SRT/Servers/SRTOldMinorServo/include/MSParameters.h diff --git a/SRT/Servers/SRTMinorServo/include/MinorServoBossImpl.h b/SRT/Servers/SRTOldMinorServo/include/MinorServoBossImpl.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/MinorServoBossImpl.h rename to SRT/Servers/SRTOldMinorServo/include/MinorServoBossImpl.h diff --git a/SRT/Servers/SRTMinorServo/include/ParkThread.h b/SRT/Servers/SRTOldMinorServo/include/ParkThread.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/ParkThread.h rename to SRT/Servers/SRTOldMinorServo/include/ParkThread.h diff --git a/SRT/Servers/SRTMinorServo/include/PdoubleSeqDevIO.h b/SRT/Servers/SRTOldMinorServo/include/PdoubleSeqDevIO.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/PdoubleSeqDevIO.h rename to SRT/Servers/SRTOldMinorServo/include/PdoubleSeqDevIO.h diff --git a/SRT/Servers/SRTMinorServo/include/RequestDispatcher.h b/SRT/Servers/SRTOldMinorServo/include/RequestDispatcher.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/RequestDispatcher.h rename to SRT/Servers/SRTOldMinorServo/include/RequestDispatcher.h diff --git a/SRT/Servers/SRTMinorServo/include/ScanThread.h b/SRT/Servers/SRTOldMinorServo/include/ScanThread.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/ScanThread.h rename to SRT/Servers/SRTOldMinorServo/include/ScanThread.h diff --git a/SRT/Servers/SRTMinorServo/include/SetupThread.h b/SRT/Servers/SRTOldMinorServo/include/SetupThread.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/SetupThread.h rename to SRT/Servers/SRTOldMinorServo/include/SetupThread.h diff --git a/SRT/Servers/SRTMinorServo/include/SocketListener.h b/SRT/Servers/SRTOldMinorServo/include/SocketListener.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/SocketListener.h rename to SRT/Servers/SRTOldMinorServo/include/SocketListener.h diff --git a/SRT/Servers/SRTMinorServo/include/SubsystemStatusDevIO.h b/SRT/Servers/SRTOldMinorServo/include/SubsystemStatusDevIO.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/SubsystemStatusDevIO.h rename to SRT/Servers/SRTOldMinorServo/include/SubsystemStatusDevIO.h diff --git a/SRT/Servers/SRTMinorServo/include/SubsystemVStatusDevIO.h b/SRT/Servers/SRTOldMinorServo/include/SubsystemVStatusDevIO.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/SubsystemVStatusDevIO.h rename to SRT/Servers/SRTOldMinorServo/include/SubsystemVStatusDevIO.h diff --git a/SRT/Servers/SRTMinorServo/include/TrackingThread.h b/SRT/Servers/SRTOldMinorServo/include/TrackingThread.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/TrackingThread.h rename to SRT/Servers/SRTOldMinorServo/include/TrackingThread.h diff --git a/SRT/Servers/SRTMinorServo/include/WPServoImpl.h b/SRT/Servers/SRTOldMinorServo/include/WPServoImpl.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/WPServoImpl.h rename to SRT/Servers/SRTOldMinorServo/include/WPServoImpl.h diff --git a/SRT/Servers/SRTMinorServo/include/WPServoSocket.h b/SRT/Servers/SRTOldMinorServo/include/WPServoSocket.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/WPServoSocket.h rename to SRT/Servers/SRTOldMinorServo/include/WPServoSocket.h diff --git a/SRT/Servers/SRTMinorServo/include/WPServoTalker.h b/SRT/Servers/SRTOldMinorServo/include/WPServoTalker.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/WPServoTalker.h rename to SRT/Servers/SRTOldMinorServo/include/WPServoTalker.h diff --git a/SRT/Servers/SRTMinorServo/include/WPStatusDevIO.h b/SRT/Servers/SRTOldMinorServo/include/WPStatusDevIO.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/WPStatusDevIO.h rename to SRT/Servers/SRTOldMinorServo/include/WPStatusDevIO.h diff --git a/SRT/Servers/SRTMinorServo/include/WPStatusUpdater.h b/SRT/Servers/SRTOldMinorServo/include/WPStatusUpdater.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/WPStatusUpdater.h rename to SRT/Servers/SRTOldMinorServo/include/WPStatusUpdater.h diff --git a/SRT/Servers/SRTMinorServo/include/WPUtils.h b/SRT/Servers/SRTOldMinorServo/include/WPUtils.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/WPUtils.h rename to SRT/Servers/SRTOldMinorServo/include/WPUtils.h diff --git a/SRT/Servers/SRTMinorServo/include/libCom.h b/SRT/Servers/SRTOldMinorServo/include/libCom.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/libCom.h rename to SRT/Servers/SRTOldMinorServo/include/libCom.h diff --git a/SRT/Servers/SRTMinorServo/include/macros.def b/SRT/Servers/SRTOldMinorServo/include/macros.def similarity index 100% rename from SRT/Servers/SRTMinorServo/include/macros.def rename to SRT/Servers/SRTOldMinorServo/include/macros.def diff --git a/SRT/Servers/SRTMinorServo/include/utils.h b/SRT/Servers/SRTOldMinorServo/include/utils.h similarity index 100% rename from SRT/Servers/SRTMinorServo/include/utils.h rename to SRT/Servers/SRTOldMinorServo/include/utils.h diff --git a/SRT/Servers/SRTMinorServo/src/MSBossConfiguration.cpp b/SRT/Servers/SRTOldMinorServo/src/MSBossConfiguration.cpp similarity index 100% rename from SRT/Servers/SRTMinorServo/src/MSBossConfiguration.cpp rename to SRT/Servers/SRTOldMinorServo/src/MSBossConfiguration.cpp diff --git a/SRT/Servers/SRTMinorServo/src/MSBossPublisher.cpp b/SRT/Servers/SRTOldMinorServo/src/MSBossPublisher.cpp similarity index 100% rename from SRT/Servers/SRTMinorServo/src/MSBossPublisher.cpp rename to SRT/Servers/SRTOldMinorServo/src/MSBossPublisher.cpp diff --git a/SRT/Servers/SRTOldMinorServo/src/Makefile b/SRT/Servers/SRTOldMinorServo/src/Makefile new file mode 100644 index 0000000000000000000000000000000000000000..6c47bef427c984badc375e47f541d6ed1cb212d2 --- /dev/null +++ b/SRT/Servers/SRTOldMinorServo/src/Makefile @@ -0,0 +1,75 @@ +#***************************************** +#----------------------------------------- +# Marco Buttu +#----------------------------------------- +#***************************************** + +PY_PACKAGES = +PY_SCRIPTS = + +# On-Line Database Files +# ---------------------- +CDB_SCHEMAS = MinorServo + +# ---------------------------- +# Libraries (public and local) +# ---------------------------- +LIBRARIES = WPServoImpl MinorServoBossImpl + +WPServoImpl_OBJECTS = WPServoImpl WPServoSocket WPServoTalker RequestDispatcher \ + WPStatusUpdater SocketListener utils libCom WPUtils +WPServoImpl_LIBS = MinorServoStubs IRALibrary ComponentErrors \ + SRTMinorServoLibrary MinorServoErrors + +MinorServoBossImpl_OBJECTS = MinorServoBossImpl utils SetupThread ParkThread \ + TrackingThread ScanThread MSBossPublisher libCom \ + MSBossConfiguration +MinorServoBossImpl_LIBS = MinorServoStubs MinorServoBossStubs SRTMinorServoLibrary \ + ManagmentDefinitionsStubs ManagementErrors \ + IRALibrary ComponentErrors MinorServoErrors acsnc \ + ParserErrors DiscosVersion AntennaDefinitionsStubs MountStubs \ + AntennaBossStubs AntennaErrors ActiveSurfaceBossStubs \ + SRTActiveSurfaceBossStubs MinorServoDefinitionsStubs +# ---------------------------------------------------------------------- +# List of all possible C-sources (used to create automatic dependencies) +# ---------------------------------------------------------------------- +CSOURCENAMES = \ + $(foreach exe, $(EXECUTABLES) $(EXECUTABLES_L), $($(exe)_OBJECTS)) \ + $(foreach lib, $(LIBRARIES) $(LIBRARIES_L), $($(lib)_OBJECTS)) + + +# ----------------- +# Include Standards +# ----------------- + +MAKEDIRTMP := $(shell searchFile include/acsMakefile) +ifneq ($(MAKEDIRTMP),\#error\#) + MAKEDIR := $(MAKEDIRTMP)/include + include $(MAKEDIR)/acsMakefile +endif + + +# TARGETS +all: do_all + @echo " . . . 'all' done" + +clean : clean_all + $(RM) *~ ../include/*~ ../idl/*~ ../*~ ../../*~ core + $(RM) ../doc/html + $(RM) tmp.txt acsexmplbeans.jar ../doc/abeans.log + $(RM) ../lib/python/site-packages/* + $(RM) $(INTROOT)/lib/python/site-packages/SRTMinorServoTest + $(RM) $(INTROOT)/bin/mscu-runserver + @echo " . . . clean done" + +clean_dist : clean clean_dist_all + @echo " . . . clean_dist done" + +man : do_man + # cp ../doc/html/group__ACSEXMPLDOC.html ../doc/html/main.html + @echo " . . . man page(s) done" + +install : install_all + @echo " . . . installation done" + +#___oOo___ diff --git a/SRT/Servers/SRTMinorServo/src/MinorServoBossImpl.cpp b/SRT/Servers/SRTOldMinorServo/src/MinorServoBossImpl.cpp similarity index 100% rename from SRT/Servers/SRTMinorServo/src/MinorServoBossImpl.cpp rename to SRT/Servers/SRTOldMinorServo/src/MinorServoBossImpl.cpp diff --git a/SRT/Servers/SRTMinorServo/src/ParkThread.cpp b/SRT/Servers/SRTOldMinorServo/src/ParkThread.cpp similarity index 100% rename from SRT/Servers/SRTMinorServo/src/ParkThread.cpp rename to SRT/Servers/SRTOldMinorServo/src/ParkThread.cpp diff --git a/SRT/Servers/SRTMinorServo/src/RequestDispatcher.cpp b/SRT/Servers/SRTOldMinorServo/src/RequestDispatcher.cpp similarity index 100% rename from SRT/Servers/SRTMinorServo/src/RequestDispatcher.cpp rename to SRT/Servers/SRTOldMinorServo/src/RequestDispatcher.cpp diff --git a/SRT/Servers/SRTMinorServo/src/ScanThread.cpp b/SRT/Servers/SRTOldMinorServo/src/ScanThread.cpp similarity index 100% rename from SRT/Servers/SRTMinorServo/src/ScanThread.cpp rename to SRT/Servers/SRTOldMinorServo/src/ScanThread.cpp diff --git a/SRT/Servers/SRTMinorServo/src/SetupThread.cpp b/SRT/Servers/SRTOldMinorServo/src/SetupThread.cpp similarity index 100% rename from SRT/Servers/SRTMinorServo/src/SetupThread.cpp rename to SRT/Servers/SRTOldMinorServo/src/SetupThread.cpp diff --git a/SRT/Servers/SRTMinorServo/src/SocketListener.cpp b/SRT/Servers/SRTOldMinorServo/src/SocketListener.cpp similarity index 100% rename from SRT/Servers/SRTMinorServo/src/SocketListener.cpp rename to SRT/Servers/SRTOldMinorServo/src/SocketListener.cpp diff --git a/SRT/Servers/SRTMinorServo/src/TrackingThread.cpp b/SRT/Servers/SRTOldMinorServo/src/TrackingThread.cpp similarity index 100% rename from SRT/Servers/SRTMinorServo/src/TrackingThread.cpp rename to SRT/Servers/SRTOldMinorServo/src/TrackingThread.cpp diff --git a/SRT/Servers/SRTMinorServo/src/WPServoImpl.cpp b/SRT/Servers/SRTOldMinorServo/src/WPServoImpl.cpp similarity index 100% rename from SRT/Servers/SRTMinorServo/src/WPServoImpl.cpp rename to SRT/Servers/SRTOldMinorServo/src/WPServoImpl.cpp diff --git a/SRT/Servers/SRTMinorServo/src/WPServoSocket.cpp b/SRT/Servers/SRTOldMinorServo/src/WPServoSocket.cpp similarity index 100% rename from SRT/Servers/SRTMinorServo/src/WPServoSocket.cpp rename to SRT/Servers/SRTOldMinorServo/src/WPServoSocket.cpp diff --git a/SRT/Servers/SRTMinorServo/src/WPServoTalker.cpp b/SRT/Servers/SRTOldMinorServo/src/WPServoTalker.cpp similarity index 100% rename from SRT/Servers/SRTMinorServo/src/WPServoTalker.cpp rename to SRT/Servers/SRTOldMinorServo/src/WPServoTalker.cpp diff --git a/SRT/Servers/SRTMinorServo/src/WPStatusUpdater.cpp b/SRT/Servers/SRTOldMinorServo/src/WPStatusUpdater.cpp similarity index 100% rename from SRT/Servers/SRTMinorServo/src/WPStatusUpdater.cpp rename to SRT/Servers/SRTOldMinorServo/src/WPStatusUpdater.cpp diff --git a/SRT/Servers/SRTMinorServo/src/WPUtils.cpp b/SRT/Servers/SRTOldMinorServo/src/WPUtils.cpp similarity index 100% rename from SRT/Servers/SRTMinorServo/src/WPUtils.cpp rename to SRT/Servers/SRTOldMinorServo/src/WPUtils.cpp diff --git a/SRT/Servers/SRTMinorServo/src/libCom.cpp b/SRT/Servers/SRTOldMinorServo/src/libCom.cpp similarity index 100% rename from SRT/Servers/SRTMinorServo/src/libCom.cpp rename to SRT/Servers/SRTOldMinorServo/src/libCom.cpp diff --git a/SRT/Servers/SRTMinorServo/src/utils.cpp b/SRT/Servers/SRTOldMinorServo/src/utils.cpp similarity index 100% rename from SRT/Servers/SRTMinorServo/src/utils.cpp rename to SRT/Servers/SRTOldMinorServo/src/utils.cpp diff --git a/SRT/Servers/SRTOldMinorServo/test/Makefile b/SRT/Servers/SRTOldMinorServo/test/Makefile new file mode 100644 index 0000000000000000000000000000000000000000..cbc49a4479619d38e8a6695820d2c23441f20fac --- /dev/null +++ b/SRT/Servers/SRTOldMinorServo/test/Makefile @@ -0,0 +1,90 @@ +# CPP UNIT TESTING SETUP +#-------------- +# GTEST_HOME=/usr/local/include/gtest +# GMOCK_HOME=/usr/local/include/gmock +# GTEST_LIBS=gtest gtest_main + +# USER_INC=-I$(GTEST_HOME) -I$(GMOCK_HOME) +# USER_LIBS=C++ pthread +# END OF CPP UNIT TESTING SETUP +#--------------------- + +# DEFINE YOUR CPP UNIT TEST EXECUTABLES HERE as: +# +# EXECTUABLES_L = unittest +# unittest_OBJECTS = unittest +# unittest_LIBS = $(GTEST_LIBS) + +# EXECUTABLES_L = unittest +# unittest_OBJECTS = unittest +# unittest_LIBS = $(GTEST_LIBS) + +# END OF CUSTOMIZATION +# do not edit below this line +#---------------------------- + +CSOURCENAMES = \ + $(foreach exe, $(EXECUTABLES) $(EXECUTABLES_L), $($(exe)_OBJECTS)) \ + $(foreach rtos, $(RTAI_MODULES) , $($(rtos)_OBJECTS)) \ + $(foreach lib, $(LIBRARIES) $(LIBRARIES_L), $($(lib)_OBJECTS)) + +MAKEDIRTMP := $(shell searchFile include/acsMakefile) +ifneq ($(MAKEDIRTMP),\#error\#) + MAKEDIR := $(MAKEDIRTMP)/include + include $(MAKEDIR)/acsMakefile +endif + +# TEST TARGETS +#TODO: unittest(2) discover pyunit + +do_unit: all + @echo "running cpp unit tests" + ../bin/unittest --gtest_output=xml:results/cppunittest.xml + +do_pyunit: + @echo "running python unit tests" + python -m unittest pyunit + +do_functional: + @echo "running python functional tests" + python -m unittest functional + +do_external: + @echo "running python external tests" + python -m unittest external + +clean_test: + rm -f results/*.xml + rm -f functional/*.pyc + rm -f pyunit/*.pyc + rm -f external/*.pyc + +unit: do_unit + @echo " . . . 'unit' done" + +pyunit: do_pyunit + @echo " . . . 'pyunit' done" + +functional: do_functional + @echo " . . . 'functional' done" + +external: do_external + @echo " . . . 'external' done" + +# TARGETS +# ------- +all: do_all + @echo " . . . 'all' done" + +clean : clean_all clean_test + @echo " . . . clean done" + +clean_dist : clean_all clean_dist_all clean_test + @echo " . . . clean_dist done" + +man : do_man + @echo " . . . man page(s) done" + +install : install_all + @echo " . . . installation done" + diff --git a/SRT/Servers/SRTOldMinorServo/test/external/__init__.py b/SRT/Servers/SRTOldMinorServo/test/external/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/SRT/Servers/SRTOldMinorServo/test/functional/__init__.py b/SRT/Servers/SRTOldMinorServo/test/functional/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/SRT/Servers/SRTMinorServo/test/functional/test_container_crash.py b/SRT/Servers/SRTOldMinorServo/test/functional/test_container_crash.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/functional/test_container_crash.py rename to SRT/Servers/SRTOldMinorServo/test/functional/test_container_crash.py diff --git a/SRT/Servers/SRTMinorServo/test/functional/test_failure.py b/SRT/Servers/SRTOldMinorServo/test/functional/test_failure.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/functional/test_failure.py rename to SRT/Servers/SRTOldMinorServo/test/functional/test_failure.py diff --git a/SRT/Servers/SRTMinorServo/test/functional/test_setup.py b/SRT/Servers/SRTOldMinorServo/test/functional/test_setup.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/functional/test_setup.py rename to SRT/Servers/SRTOldMinorServo/test/functional/test_setup.py diff --git a/SRT/Servers/SRTMinorServo/test/functional/test_setup_after_manual_movement.py b/SRT/Servers/SRTOldMinorServo/test/functional/test_setup_after_manual_movement.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/functional/test_setup_after_manual_movement.py rename to SRT/Servers/SRTOldMinorServo/test/functional/test_setup_after_manual_movement.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/Makefile b/SRT/Servers/SRTOldMinorServo/test/no_auto/Makefile similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/Makefile rename to SRT/Servers/SRTOldMinorServo/test/no_auto/Makefile diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/05082013.rst b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/05082013.rst similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/05082013.rst rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/05082013.rst diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/SRP_linear/plot_positions.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/SRP_linear/plot_positions.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/SRP_linear/plot_positions.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/SRP_linear/plot_positions.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/SRP_linear/test_SRP_linear_movement.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/SRP_linear/test_SRP_linear_movement.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/SRP_linear/test_SRP_linear_movement.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/SRP_linear/test_SRP_linear_movement.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/TODO.rst b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/TODO.rst similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/TODO.rst rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/TODO.rst diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/clean_test.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/clean_test.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/clean_test.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/clean_test.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/command_input.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/command_input.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/command_input.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/command_input.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/getAxesPosition/test_getAxesPosition.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/getAxesPosition/test_getAxesPosition.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/getAxesPosition/test_getAxesPosition.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/getAxesPosition/test_getAxesPosition.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/getAxesPosition/test_getAxesPositionSpeed.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/getAxesPosition/test_getAxesPositionSpeed.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/getAxesPosition/test_getAxesPositionSpeed.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/getAxesPosition/test_getAxesPositionSpeed.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/getFromHistory/test_getFromHistory.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/getFromHistory/test_getFromHistory.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/getFromHistory/test_getFromHistory.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/getFromHistory/test_getFromHistory.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/offsets/testSRP_after_startFocusScan.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/offsets/testSRP_after_startFocusScan.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/offsets/testSRP_after_startFocusScan.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/offsets/testSRP_after_startFocusScan.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/offsets/testSRP_before_startFocusScan.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/offsets/testSRP_before_startFocusScan.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/offsets/testSRP_before_startFocusScan.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/offsets/testSRP_before_startFocusScan.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/parameters.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/parameters.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/parameters.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/parameters.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/plot_positions.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/plot_positions.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/plot_positions.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/plot_positions.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/property/property_sampler.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/property/property_sampler.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/property/property_sampler.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/property/property_sampler.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/sampling_time/test_getstatus_speed.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/sampling_time/test_getstatus_speed.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/sampling_time/test_getstatus_speed.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/sampling_time/test_getstatus_speed.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/scan/testSRP_checkFocusScan.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/scan/testSRP_checkFocusScan.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/scan/testSRP_checkFocusScan.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/scan/testSRP_checkFocusScan.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/scan/testSRP_checkMinPositioninTime.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/scan/testSRP_checkMinPositioninTime.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/scan/testSRP_checkMinPositioninTime.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/scan/testSRP_checkMinPositioninTime.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/scan/testSRP_multiple_startFocusScan.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/scan/testSRP_multiple_startFocusScan.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/scan/testSRP_multiple_startFocusScan.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/scan/testSRP_multiple_startFocusScan.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/scan/testSRP_pointingDuringScan.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/scan/testSRP_pointingDuringScan.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/scan/testSRP_pointingDuringScan.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/scan/testSRP_pointingDuringScan.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/scan/testSRP_startFocusScan.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/scan/testSRP_startFocusScan.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/scan/testSRP_startFocusScan.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/scan/testSRP_startFocusScan.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/scan/testSRP_startFocusScanNow.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/scan/testSRP_startFocusScanNow.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/scan/testSRP_startFocusScanNow.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/scan/testSRP_startFocusScanNow.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/scan/testSRP_stopScan.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/scan/testSRP_stopScan.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/scan/testSRP_stopScan.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/scan/testSRP_stopScan.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/timed_pos/timed_positions1SRP.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/timed_pos/timed_positions1SRP.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/timed_pos/timed_positions1SRP.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/timed_pos/timed_positions1SRP.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/timed_pos/timed_positions2PFP.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/timed_pos/timed_positions2PFP.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/timed_pos/timed_positions2PFP.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/timed_pos/timed_positions2PFP.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/timed_pos/timed_positions2SRP.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/timed_pos/timed_positions2SRP.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/timed_pos/timed_positions2SRP.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/timed_pos/timed_positions2SRP.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/commissioning/tracking/test_SRP_elevation_tracking.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/tracking/test_SRP_elevation_tracking.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/commissioning/tracking/test_SRP_elevation_tracking.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/commissioning/tracking/test_SRP_elevation_tracking.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/performances/get_axes_positions.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/performances/get_axes_positions.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/performances/get_axes_positions.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/performances/get_axes_positions.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/real2virtual.c b/SRT/Servers/SRTOldMinorServo/test/no_auto/real2virtual.c similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/real2virtual.c rename to SRT/Servers/SRTOldMinorServo/test/no_auto/real2virtual.c diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/send_command.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/send_command.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/send_command.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/send_command.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/simple_talk.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/simple_talk.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/simple_talk.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/simple_talk.py diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/srp_refsystems/hexlib/Makefile b/SRT/Servers/SRTOldMinorServo/test/no_auto/srp_refsystems/hexlib/Makefile similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/srp_refsystems/hexlib/Makefile rename to SRT/Servers/SRTOldMinorServo/test/no_auto/srp_refsystems/hexlib/Makefile diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/srp_refsystems/hexlib/hexdata.txt b/SRT/Servers/SRTOldMinorServo/test/no_auto/srp_refsystems/hexlib/hexdata.txt similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/srp_refsystems/hexlib/hexdata.txt rename to SRT/Servers/SRTOldMinorServo/test/no_auto/srp_refsystems/hexlib/hexdata.txt diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/srp_refsystems/hexlib/hexlib.c b/SRT/Servers/SRTOldMinorServo/test/no_auto/srp_refsystems/hexlib/hexlib.c similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/srp_refsystems/hexlib/hexlib.c rename to SRT/Servers/SRTOldMinorServo/test/no_auto/srp_refsystems/hexlib/hexlib.c diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/srp_refsystems/hexlib/hexlib.h b/SRT/Servers/SRTOldMinorServo/test/no_auto/srp_refsystems/hexlib/hexlib.h similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/srp_refsystems/hexlib/hexlib.h rename to SRT/Servers/SRTOldMinorServo/test/no_auto/srp_refsystems/hexlib/hexlib.h diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/srp_refsystems/hexlib/real2virtual.c b/SRT/Servers/SRTOldMinorServo/test/no_auto/srp_refsystems/hexlib/real2virtual.c similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/srp_refsystems/hexlib/real2virtual.c rename to SRT/Servers/SRTOldMinorServo/test/no_auto/srp_refsystems/hexlib/real2virtual.c diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/srp_refsystems/hexlib/virtual2real.c b/SRT/Servers/SRTOldMinorServo/test/no_auto/srp_refsystems/hexlib/virtual2real.c similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/srp_refsystems/hexlib/virtual2real.c rename to SRT/Servers/SRTOldMinorServo/test/no_auto/srp_refsystems/hexlib/virtual2real.c diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/srp_refsystems/srp_refsystems.pyw b/SRT/Servers/SRTOldMinorServo/test/no_auto/srp_refsystems/srp_refsystems.pyw similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/srp_refsystems/srp_refsystems.pyw rename to SRT/Servers/SRTOldMinorServo/test/no_auto/srp_refsystems/srp_refsystems.pyw diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/virtual2real.c b/SRT/Servers/SRTOldMinorServo/test/no_auto/virtual2real.c similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/virtual2real.c rename to SRT/Servers/SRTOldMinorServo/test/no_auto/virtual2real.c diff --git a/SRT/Servers/SRTMinorServo/test/no_auto/wpservo_test.py b/SRT/Servers/SRTOldMinorServo/test/no_auto/wpservo_test.py similarity index 100% rename from SRT/Servers/SRTMinorServo/test/no_auto/wpservo_test.py rename to SRT/Servers/SRTOldMinorServo/test/no_auto/wpservo_test.py diff --git a/SRT/Servers/SRTOldMinorServo/test/pyunit/__init__.py b/SRT/Servers/SRTOldMinorServo/test/pyunit/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/SRT/Servers/SRTOldMinorServo/test/unittest.cpp b/SRT/Servers/SRTOldMinorServo/test/unittest.cpp new file mode 100644 index 0000000000000000000000000000000000000000..59cb6295fe60c848aca6a832cdfd4f82375a9022 --- /dev/null +++ b/SRT/Servers/SRTOldMinorServo/test/unittest.cpp @@ -0,0 +1,6 @@ +#include "gtest/gtest.h" + +TEST(FakeTest, Success){ + EXPECT_EQ(1, 1); +} + diff --git a/SystemMake/Makefile b/SystemMake/Makefile index fe778c6bef7d5e9c54add2efb08cd2a7c67ef65c..32834ba76e90d7ddda34afe9b2e7f7db7d45fc96 100644 --- a/SystemMake/Makefile +++ b/SystemMake/Makefile @@ -49,7 +49,7 @@ COMMON_SIMULATORS:= TCPGenericProtocolSim ReceiverBoardSim SRT_DOC:=SRTDox SRT_ERRORS:= SRT_INTERFACES:=SRTAntennaInterface SRTActiveSurfaceInterface \ - SRTReceiversInterface + SRTMinorServoInterface SRTReceiversInterface SRT_LIBRARIES:=SRTMinorServoLibrary SRT_SERVERS:=SRTMount SRTActiveSurfaceBoss SRTMinorServo SRTKBandMFReceiver \ SRT7GHzReceiver SRT5GHzReceiver SRTLPBandReceiver SRTPyIFDistributor WeatherStation