9 #include "iec_types_all.h" |
9 #include "iec_types_all.h" |
10 |
10 |
11 #include "accessor.h" |
11 #include "accessor.h" |
12 #include "POUS.h" |
12 #include "POUS.h" |
13 |
13 |
|
14 /* From CiA402, page 27 |
|
15 |
|
16 Table 30 - State coding |
|
17 Statusword | PDS FSA state |
|
18 xxxx xxxx x0xx 0000 | Not ready to switch on |
|
19 xxxx xxxx x1xx 0000 | Switch on disabled |
|
20 xxxx xxxx x01x 0001 | Ready to switch on |
|
21 xxxx xxxx x01x 0011 | Switched on |
|
22 xxxx xxxx x01x 0111 | Operation enabled |
|
23 xxxx xxxx x00x 0111 | Quick stop active |
|
24 xxxx xxxx x0xx 1111 | Fault reaction active |
|
25 xxxx xxxx x0xx 1000 | Fault |
|
26 */ |
|
27 #define FSAFromStatusWord(SW) (SW & 0x006f) |
|
28 #define NotReadyToSwitchOn 0b00000000 FSA_sep 0b00100000 |
|
29 #define SwitchOnDisabled 0b01000000 FSA_sep 0b01100000 |
|
30 #define ReadyToSwitchOn 0b00100001 |
|
31 #define SwitchedOn 0b00100011 |
|
32 #define OperationEnabled 0b00100111 |
|
33 #define QuickStopActive 0b00000111 |
|
34 #define FaultReactionActive 0b00001111 FSA_sep 0b00101111 |
|
35 #define Fault 0b00001000 FSA_sep 0b00101000 |
|
36 |
|
37 // SatusWord bits : |
|
38 #define SW_ReadyToSwitchOn 0x0001 |
|
39 #define SW_SwitchedOn 0x0002 |
|
40 #define SW_OperationEnabled 0x0004 |
|
41 #define SW_Fault 0x0008 |
|
42 #define SW_VoltageEnabled 0x0010 |
|
43 #define SW_QuickStop 0x0020 |
|
44 #define SW_SwitchOnDisabled 0x0040 |
|
45 #define SW_Warning 0x0080 |
|
46 #define SW_Remote 0x0200 |
|
47 #define SW_TargetReached 0x0400 |
|
48 #define SW_InternalLimitActive 0x0800 |
|
49 |
|
50 // ControlWord bits : |
|
51 #define SwitchOn 0x0001 |
|
52 #define EnableVoltage 0x0002 |
|
53 #define QuickStop 0x0004 |
|
54 #define EnableOperation 0x0008 |
|
55 #define FaultReset 0x0080 |
|
56 #define Halt 0x0100 |
|
57 |
|
58 |
14 IEC_INT beremiz__IW%(location)s_0; |
59 IEC_INT beremiz__IW%(location)s_0; |
15 IEC_INT *__IW%(location)s_0 = &beremiz__IW%(location)s_0; |
60 IEC_INT *__IW%(location)s_0 = &beremiz__IW%(location)s_0; |
16 |
61 |
17 %(MCL_headers)s |
62 %(MCL_headers)s |
18 |
63 |
19 static IEC_UINT __InactiveMask = 0x4f; |
|
20 static IEC_UINT __ActiveMask = 0x6f; |
|
21 static IEC_UINT __PowerMask = 0x10; |
|
22 static IEC_BOOL __FirstTick = 1; |
64 static IEC_BOOL __FirstTick = 1; |
23 |
|
24 typedef enum { |
|
25 __Unknown, |
|
26 __NotReadyToSwitchOn, |
|
27 __SwitchOnDisabled, |
|
28 __ReadyToSwitchOn, |
|
29 __SwitchedOn, |
|
30 __OperationEnabled, |
|
31 __QuickStopActive, |
|
32 __FaultReactionActive, |
|
33 __Fault, |
|
34 } __CIA402NodeState; |
|
35 |
65 |
36 typedef struct { |
66 typedef struct { |
37 %(entry_variables)s |
67 %(entry_variables)s |
38 __CIA402NodeState state; |
|
39 axis_s* axis; |
68 axis_s* axis; |
40 } __CIA402Node; |
69 } __CIA402Node; |
41 |
70 |
42 #define AXIS_UNIT_TO_USER_UNIT(param, type, name)\ |
71 #define AxsPub __CIA402Node_%(location)s |
43 (IEC_##type)(param) * __CIA402Node_%(location)s.axis->name##RatioDenominator / __CIA402Node_%(location)s.axis->name##RatioNumerator |
|
44 #define USER_UNIT_TO_AXIS_UNIT(param, type, name)\ |
|
45 (IEC_##type)(param * __CIA402Node_%(location)s.axis->name##RatioNumerator / __CIA402Node_%(location)s.axis->name##RatioDenominator) |
|
46 |
72 |
47 #define DEFAULT_AXIS_UNIT_TO_USER_UNIT(param) AXIS_UNIT_TO_USER_UNIT(param, LREAL,) |
73 static __CIA402Node AxsPub; |
48 #define DEFAULT_USER_UNIT_TO_AXIS_UNIT(param) USER_UNIT_TO_AXIS_UNIT(param, DINT,) |
|
49 #define TORQUE_AXIS_UNIT_TO_USER_UNIT(param) AXIS_UNIT_TO_USER_UNIT(param, LREAL, Torque) |
|
50 #define TORQUE_USER_UNIT_TO_AXIS_UNIT(param) USER_UNIT_TO_AXIS_UNIT(param, INT, Torque) |
|
51 |
|
52 static __CIA402Node __CIA402Node_%(location)s; |
|
53 |
74 |
54 %(extern_located_variables_declaration)s |
75 %(extern_located_variables_declaration)s |
55 |
76 |
56 %(fieldbus_interface_declaration)s |
77 %(fieldbus_interface_declaration)s |
57 |
78 |
58 int __init_%(location)s() |
79 int __init_%(location)s() |
59 { |
80 { |
60 __FirstTick = 1; |
81 __FirstTick = 1; |
61 %(init_entry_variables)s |
82 %(init_entry_variables)s |
62 *(__CIA402Node_%(location)s.ModesOfOperation) = 0x08; |
83 *(AxsPub.ModesOfOperation) = 0x08; |
63 return 0; |
84 return 0; |
64 } |
85 } |
65 |
86 |
66 void __cleanup_%(location)s() |
87 void __cleanup_%(location)s() |
67 { |
88 { |
68 } |
89 } |
69 |
90 |
70 void __retrieve_%(location)s() |
91 void __retrieve_%(location)s() |
71 { |
92 { |
72 IEC_UINT statusword_inactive = *(__CIA402Node_%(location)s.StatusWord) & __InactiveMask; |
|
73 IEC_UINT statusword_active = *(__CIA402Node_%(location)s.StatusWord) & __ActiveMask; |
|
74 |
|
75 if (__FirstTick) { |
93 if (__FirstTick) { |
76 *__IW%(location)s_0 = __MK_Alloc_AXIS_REF(); |
94 *__IW%(location)s_0 = __MK_Alloc_AXIS_REF(); |
77 __CIA402Node_%(location)s.axis = __MK_GetPublic_AXIS_REF(*__IW%(location)s_0); |
95 AxsPub.axis = |
78 __CIA402Node_%(location)s.axis->NetworkPosition = %(slave_pos)d; |
96 __MK_GetPublic_AXIS_REF(*__IW%(location)s_0); |
|
97 AxsPub.axis->NetworkPosition = %(slave_pos)d; |
79 %(init_axis_params)s |
98 %(init_axis_params)s |
80 %(fieldbus_interface_definition)s |
99 %(fieldbus_interface_definition)s |
81 __FirstTick = 0; |
100 __FirstTick = 0; |
82 } |
101 } |
83 |
102 |
84 // CIA402 node state computation |
|
85 __CIA402Node_%(location)s.state = __Unknown; |
|
86 switch (statusword_inactive) { |
|
87 case 0x00: |
|
88 __CIA402Node_%(location)s.state = __NotReadyToSwitchOn; |
|
89 break; |
|
90 case 0x40: |
|
91 __CIA402Node_%(location)s.state = __SwitchOnDisabled; |
|
92 break; |
|
93 case 0x0f: |
|
94 __CIA402Node_%(location)s.state = __FaultReactionActive; |
|
95 break; |
|
96 case 0x08: |
|
97 __CIA402Node_%(location)s.state = __Fault; |
|
98 break; |
|
99 default: |
|
100 break; |
|
101 } |
|
102 switch (statusword_active) { |
|
103 case 0x21: |
|
104 __CIA402Node_%(location)s.state = __ReadyToSwitchOn; |
|
105 break; |
|
106 case 0x23: |
|
107 __CIA402Node_%(location)s.state = __SwitchedOn; |
|
108 break; |
|
109 case 0x27: |
|
110 __CIA402Node_%(location)s.state = __OperationEnabled; |
|
111 break; |
|
112 case 0x07: |
|
113 __CIA402Node_%(location)s.state = __QuickStopActive; |
|
114 break; |
|
115 default: |
|
116 break; |
|
117 } |
|
118 if (__CIA402Node_%(location)s.state == __Unknown) { |
|
119 return; |
|
120 } |
|
121 |
|
122 // Default variables retrieve |
103 // Default variables retrieve |
123 __CIA402Node_%(location)s.axis->CommunicationReady = *(__CIA402Node_%(location)s.StatusWord) != 0; |
104 AxsPub.axis->CommunicationReady = |
124 __CIA402Node_%(location)s.axis->ReadyForPowerOn = __CIA402Node_%(location)s.state == __SwitchedOn || __OperationEnabled; |
105 *(AxsPub.StatusWord) != 0; |
125 __CIA402Node_%(location)s.axis->PowerFeedback = __CIA402Node_%(location)s.state == __OperationEnabled; |
106 #define FSA_sep || FSA == |
126 __CIA402Node_%(location)s.axis->ActualPosition = DEFAULT_AXIS_UNIT_TO_USER_UNIT(*(__CIA402Node_%(location)s.ActualPosition)); |
107 { |
127 __CIA402Node_%(location)s.axis->ActualVelocity = DEFAULT_AXIS_UNIT_TO_USER_UNIT(*(__CIA402Node_%(location)s.ActualVelocity)); |
108 uint16_t FSA = FSAFromStatusWord(*(AxsPub.StatusWord)); |
128 __CIA402Node_%(location)s.axis->ActualTorque = TORQUE_AXIS_UNIT_TO_USER_UNIT(*(__CIA402Node_%(location)s.ActualTorque)); |
109 AxsPub.axis->ReadyForPowerOn = FSA == ReadyToSwitchOn; |
|
110 AxsPub.axis->PowerFeedback = FSA == OperationEnabled; |
|
111 } |
|
112 #undef FSA_sep |
|
113 AxsPub.axis->ActualRawPosition = *(AxsPub.ActualPosition); |
|
114 AxsPub.axis->ActualRawVelocity = *(AxsPub.ActualVelocity); |
|
115 AxsPub.axis->ActualRawTorque = *(AxsPub.ActualTorque); |
129 |
116 |
130 // Extra variables retrieve |
117 // Extra variables retrieve |
131 %(extra_variables_retrieve)s |
118 %(extra_variables_retrieve)s |
132 } |
119 } |
133 |
120 |
134 void __publish_%(location)s() |
121 void __publish_%(location)s() |
135 { |
122 { |
136 IEC_BOOL power = ((*(__CIA402Node_%(location)s.StatusWord) & __PowerMask) > 0) && __CIA402Node_%(location)s.axis->Power; |
123 IEC_BOOL power = |
|
124 ((*(AxsPub.StatusWord) & SW_VoltageEnabled) != 0) |
|
125 && AxsPub.axis->Power; |
|
126 uint16_t CW = *(AxsPub.ControlWord); |
137 |
127 |
|
128 #define FSA_sep : case |
138 // CIA402 node state transition computation |
129 // CIA402 node state transition computation |
139 switch (__CIA402Node_%(location)s.state) { |
130 switch (FSAFromStatusWord(*(AxsPub.StatusWord))) { |
140 case __SwitchOnDisabled: |
131 case SwitchOnDisabled : |
141 *(__CIA402Node_%(location)s.ControlWord) = (*(__CIA402Node_%(location)s.ControlWord) & ~0x87) | 0x06; |
132 CW &= ~(SwitchOn | FaultReset); |
|
133 CW |= EnableVoltage | QuickStop; |
142 break; |
134 break; |
143 case __ReadyToSwitchOn: |
135 case ReadyToSwitchOn : |
144 case __OperationEnabled: |
136 case OperationEnabled : |
145 if (!power) { |
137 if (!power) { |
146 *(__CIA402Node_%(location)s.ControlWord) = (*(__CIA402Node_%(location)s.ControlWord) & ~0x8f) | 0x07; |
138 CW &= ~(FaultReset | EnableOperation); |
|
139 CW |= SwitchOn | EnableVoltage | QuickStop; |
147 break; |
140 break; |
148 } |
141 } |
149 case __SwitchedOn: |
142 case SwitchedOn : |
150 if (power) { |
143 if (power) { |
151 *(__CIA402Node_%(location)s.ControlWord) = (*(__CIA402Node_%(location)s.ControlWord) & ~0x8f) | 0x0f; |
144 CW &= ~(FaultReset); |
|
145 CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation; |
152 } |
146 } |
153 break; |
147 break; |
154 case __Fault: |
148 case Fault : |
155 *(__CIA402Node_%(location)s.ControlWord) = (*(__CIA402Node_%(location)s.ControlWord) & ~0x8f) | 0x80; |
149 /* TODO reset fault only when MC_Reset */ |
|
150 CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation); |
|
151 CW |= FaultReset; |
156 break; |
152 break; |
157 default: |
153 default: |
158 break; |
154 break; |
159 } |
155 } |
|
156 #undef FSA_sep |
|
157 *(AxsPub.ControlWord) = CW; |
160 |
158 |
161 // CIA402 node modes of operation computation according to axis motion mode |
159 // CIA402 node modes of operation computation according to axis motion mode |
162 switch (__CIA402Node_%(location)s.axis->AxisMotionMode) { |
160 switch (AxsPub.axis->AxisMotionMode) { |
163 case mc_mode_cst: |
161 case mc_mode_cst: |
164 *(__CIA402Node_%(location)s.ModesOfOperation) = 0x0a; |
162 *(AxsPub.ModesOfOperation) = 0x0a; |
165 break; |
163 break; |
166 case mc_mode_csv: |
164 case mc_mode_csv: |
167 *(__CIA402Node_%(location)s.ModesOfOperation) = 0x09; |
165 *(AxsPub.ModesOfOperation) = 0x09; |
168 break; |
166 break; |
169 default: |
167 default: |
170 *(__CIA402Node_%(location)s.ModesOfOperation) = 0x08; |
168 *(AxsPub.ModesOfOperation) = 0x08; |
171 break; |
169 break; |
172 } |
170 } |
173 |
171 |
174 // Default variables publish |
172 // Default variables publish |
175 *(__CIA402Node_%(location)s.TargetPosition) = DEFAULT_USER_UNIT_TO_AXIS_UNIT(__CIA402Node_%(location)s.axis->PositionSetPoint); |
173 *(AxsPub.TargetPosition) = |
176 *(__CIA402Node_%(location)s.TargetVelocity) = DEFAULT_USER_UNIT_TO_AXIS_UNIT(__CIA402Node_%(location)s.axis->VelocitySetPoint); |
174 AxsPub.axis->RawPositionSetPoint; |
177 *(__CIA402Node_%(location)s.TargetTorque) = TORQUE_USER_UNIT_TO_AXIS_UNIT(__CIA402Node_%(location)s.axis->TorqueSetPoint); |
175 *(AxsPub.TargetVelocity) = |
|
176 AxsPub.axis->RawVelocitySetPoint; |
|
177 *(AxsPub.TargetTorque) = |
|
178 AxsPub.axis->RawTorqueSetPoint; |
178 |
179 |
179 // Extra variables publish |
180 // Extra variables publish |
180 %(extra_variables_publish)s |
181 %(extra_variables_publish)s |
181 } |
182 } |