# HG changeset patch # User Edouard Tisserant # Date 1369119820 -32400 # Node ID 08514552944fd0ca645ae897e2e78182cdea6e9d # Parent 7f473761c93249a2ffceb00550d1c302b2123f7c Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans. diff -r 7f473761c932 -r 08514552944f etherlab/plc_cia402node.c --- a/etherlab/plc_cia402node.c Thu May 16 23:21:01 2013 +0200 +++ b/etherlab/plc_cia402node.c Tue May 21 16:03:40 2013 +0900 @@ -11,45 +11,66 @@ #include "accessor.h" #include "POUS.h" +/* From CiA402, page 27 + + Table 30 - State coding + Statusword | PDS FSA state +xxxx xxxx x0xx 0000 | Not ready to switch on +xxxx xxxx x1xx 0000 | Switch on disabled +xxxx xxxx x01x 0001 | Ready to switch on +xxxx xxxx x01x 0011 | Switched on +xxxx xxxx x01x 0111 | Operation enabled +xxxx xxxx x00x 0111 | Quick stop active +xxxx xxxx x0xx 1111 | Fault reaction active +xxxx xxxx x0xx 1000 | Fault +*/ +#define FSAFromStatusWord(SW) (SW & 0x006f) +#define NotReadyToSwitchOn 0b00000000 FSA_sep 0b00100000 +#define SwitchOnDisabled 0b01000000 FSA_sep 0b01100000 +#define ReadyToSwitchOn 0b00100001 +#define SwitchedOn 0b00100011 +#define OperationEnabled 0b00100111 +#define QuickStopActive 0b00000111 +#define FaultReactionActive 0b00001111 FSA_sep 0b00101111 +#define Fault 0b00001000 FSA_sep 0b00101000 + +// SatusWord bits : +#define SW_ReadyToSwitchOn 0x0001 +#define SW_SwitchedOn 0x0002 +#define SW_OperationEnabled 0x0004 +#define SW_Fault 0x0008 +#define SW_VoltageEnabled 0x0010 +#define SW_QuickStop 0x0020 +#define SW_SwitchOnDisabled 0x0040 +#define SW_Warning 0x0080 +#define SW_Remote 0x0200 +#define SW_TargetReached 0x0400 +#define SW_InternalLimitActive 0x0800 + +// ControlWord bits : +#define SwitchOn 0x0001 +#define EnableVoltage 0x0002 +#define QuickStop 0x0004 +#define EnableOperation 0x0008 +#define FaultReset 0x0080 +#define Halt 0x0100 + + IEC_INT beremiz__IW%(location)s_0; IEC_INT *__IW%(location)s_0 = &beremiz__IW%(location)s_0; %(MCL_headers)s -static IEC_UINT __InactiveMask = 0x4f; -static IEC_UINT __ActiveMask = 0x6f; -static IEC_UINT __PowerMask = 0x10; static IEC_BOOL __FirstTick = 1; -typedef enum { - __Unknown, - __NotReadyToSwitchOn, - __SwitchOnDisabled, - __ReadyToSwitchOn, - __SwitchedOn, - __OperationEnabled, - __QuickStopActive, - __FaultReactionActive, - __Fault, -} __CIA402NodeState; - typedef struct { %(entry_variables)s - __CIA402NodeState state; axis_s* axis; } __CIA402Node; -#define AXIS_UNIT_TO_USER_UNIT(param, type, name)\ -(IEC_##type)(param) * __CIA402Node_%(location)s.axis->name##RatioDenominator / __CIA402Node_%(location)s.axis->name##RatioNumerator -#define USER_UNIT_TO_AXIS_UNIT(param, type, name)\ -(IEC_##type)(param * __CIA402Node_%(location)s.axis->name##RatioNumerator / __CIA402Node_%(location)s.axis->name##RatioDenominator) +#define AxsPub __CIA402Node_%(location)s -#define DEFAULT_AXIS_UNIT_TO_USER_UNIT(param) AXIS_UNIT_TO_USER_UNIT(param, LREAL,) -#define DEFAULT_USER_UNIT_TO_AXIS_UNIT(param) USER_UNIT_TO_AXIS_UNIT(param, DINT,) -#define TORQUE_AXIS_UNIT_TO_USER_UNIT(param) AXIS_UNIT_TO_USER_UNIT(param, LREAL, Torque) -#define TORQUE_USER_UNIT_TO_AXIS_UNIT(param) USER_UNIT_TO_AXIS_UNIT(param, INT, Torque) - -static __CIA402Node __CIA402Node_%(location)s; +static __CIA402Node AxsPub; %(extern_located_variables_declaration)s @@ -59,7 +80,7 @@ { __FirstTick = 1; %(init_entry_variables)s - *(__CIA402Node_%(location)s.ModesOfOperation) = 0x08; + *(AxsPub.ModesOfOperation) = 0x08; return 0; } @@ -69,63 +90,29 @@ void __retrieve_%(location)s() { - IEC_UINT statusword_inactive = *(__CIA402Node_%(location)s.StatusWord) & __InactiveMask; - IEC_UINT statusword_active = *(__CIA402Node_%(location)s.StatusWord) & __ActiveMask; - if (__FirstTick) { *__IW%(location)s_0 = __MK_Alloc_AXIS_REF(); - __CIA402Node_%(location)s.axis = __MK_GetPublic_AXIS_REF(*__IW%(location)s_0); - __CIA402Node_%(location)s.axis->NetworkPosition = %(slave_pos)d; + AxsPub.axis = + __MK_GetPublic_AXIS_REF(*__IW%(location)s_0); + AxsPub.axis->NetworkPosition = %(slave_pos)d; %(init_axis_params)s %(fieldbus_interface_definition)s __FirstTick = 0; } - // CIA402 node state computation - __CIA402Node_%(location)s.state = __Unknown; - switch (statusword_inactive) { - case 0x00: - __CIA402Node_%(location)s.state = __NotReadyToSwitchOn; - break; - case 0x40: - __CIA402Node_%(location)s.state = __SwitchOnDisabled; - break; - case 0x0f: - __CIA402Node_%(location)s.state = __FaultReactionActive; - break; - case 0x08: - __CIA402Node_%(location)s.state = __Fault; - break; - default: - break; - } - switch (statusword_active) { - case 0x21: - __CIA402Node_%(location)s.state = __ReadyToSwitchOn; - break; - case 0x23: - __CIA402Node_%(location)s.state = __SwitchedOn; - break; - case 0x27: - __CIA402Node_%(location)s.state = __OperationEnabled; - break; - case 0x07: - __CIA402Node_%(location)s.state = __QuickStopActive; - break; - default: - break; - } - if (__CIA402Node_%(location)s.state == __Unknown) { - return; - } - // Default variables retrieve - __CIA402Node_%(location)s.axis->CommunicationReady = *(__CIA402Node_%(location)s.StatusWord) != 0; - __CIA402Node_%(location)s.axis->ReadyForPowerOn = __CIA402Node_%(location)s.state == __SwitchedOn || __OperationEnabled; - __CIA402Node_%(location)s.axis->PowerFeedback = __CIA402Node_%(location)s.state == __OperationEnabled; - __CIA402Node_%(location)s.axis->ActualPosition = DEFAULT_AXIS_UNIT_TO_USER_UNIT(*(__CIA402Node_%(location)s.ActualPosition)); - __CIA402Node_%(location)s.axis->ActualVelocity = DEFAULT_AXIS_UNIT_TO_USER_UNIT(*(__CIA402Node_%(location)s.ActualVelocity)); - __CIA402Node_%(location)s.axis->ActualTorque = TORQUE_AXIS_UNIT_TO_USER_UNIT(*(__CIA402Node_%(location)s.ActualTorque)); + AxsPub.axis->CommunicationReady = + *(AxsPub.StatusWord) != 0; +#define FSA_sep || FSA == + { + uint16_t FSA = FSAFromStatusWord(*(AxsPub.StatusWord)); + AxsPub.axis->ReadyForPowerOn = FSA == ReadyToSwitchOn; + AxsPub.axis->PowerFeedback = FSA == OperationEnabled; + } +#undef FSA_sep + AxsPub.axis->ActualRawPosition = *(AxsPub.ActualPosition); + AxsPub.axis->ActualRawVelocity = *(AxsPub.ActualVelocity); + AxsPub.axis->ActualRawTorque = *(AxsPub.ActualTorque); // Extra variables retrieve %(extra_variables_retrieve)s @@ -133,48 +120,62 @@ void __publish_%(location)s() { - IEC_BOOL power = ((*(__CIA402Node_%(location)s.StatusWord) & __PowerMask) > 0) && __CIA402Node_%(location)s.axis->Power; + IEC_BOOL power = + ((*(AxsPub.StatusWord) & SW_VoltageEnabled) != 0) + && AxsPub.axis->Power; + uint16_t CW = *(AxsPub.ControlWord); +#define FSA_sep : case // CIA402 node state transition computation - switch (__CIA402Node_%(location)s.state) { - case __SwitchOnDisabled: - *(__CIA402Node_%(location)s.ControlWord) = (*(__CIA402Node_%(location)s.ControlWord) & ~0x87) | 0x06; + switch (FSAFromStatusWord(*(AxsPub.StatusWord))) { + case SwitchOnDisabled : + CW &= ~(SwitchOn | FaultReset); + CW |= EnableVoltage | QuickStop; break; - case __ReadyToSwitchOn: - case __OperationEnabled: + case ReadyToSwitchOn : + case OperationEnabled : if (!power) { - *(__CIA402Node_%(location)s.ControlWord) = (*(__CIA402Node_%(location)s.ControlWord) & ~0x8f) | 0x07; + CW &= ~(FaultReset | EnableOperation); + CW |= SwitchOn | EnableVoltage | QuickStop; break; } - case __SwitchedOn: + case SwitchedOn : if (power) { - *(__CIA402Node_%(location)s.ControlWord) = (*(__CIA402Node_%(location)s.ControlWord) & ~0x8f) | 0x0f; + CW &= ~(FaultReset); + CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation; } break; - case __Fault: - *(__CIA402Node_%(location)s.ControlWord) = (*(__CIA402Node_%(location)s.ControlWord) & ~0x8f) | 0x80; + case Fault : + /* TODO reset fault only when MC_Reset */ + CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation); + CW |= FaultReset; break; default: break; } +#undef FSA_sep + *(AxsPub.ControlWord) = CW; // CIA402 node modes of operation computation according to axis motion mode - switch (__CIA402Node_%(location)s.axis->AxisMotionMode) { + switch (AxsPub.axis->AxisMotionMode) { case mc_mode_cst: - *(__CIA402Node_%(location)s.ModesOfOperation) = 0x0a; + *(AxsPub.ModesOfOperation) = 0x0a; break; case mc_mode_csv: - *(__CIA402Node_%(location)s.ModesOfOperation) = 0x09; + *(AxsPub.ModesOfOperation) = 0x09; break; default: - *(__CIA402Node_%(location)s.ModesOfOperation) = 0x08; + *(AxsPub.ModesOfOperation) = 0x08; break; } // Default variables publish - *(__CIA402Node_%(location)s.TargetPosition) = DEFAULT_USER_UNIT_TO_AXIS_UNIT(__CIA402Node_%(location)s.axis->PositionSetPoint); - *(__CIA402Node_%(location)s.TargetVelocity) = DEFAULT_USER_UNIT_TO_AXIS_UNIT(__CIA402Node_%(location)s.axis->VelocitySetPoint); - *(__CIA402Node_%(location)s.TargetTorque) = TORQUE_USER_UNIT_TO_AXIS_UNIT(__CIA402Node_%(location)s.axis->TorqueSetPoint); + *(AxsPub.TargetPosition) = + AxsPub.axis->RawPositionSetPoint; + *(AxsPub.TargetVelocity) = + AxsPub.axis->RawVelocitySetPoint; + *(AxsPub.TargetTorque) = + AxsPub.axis->RawTorqueSetPoint; // Extra variables publish %(extra_variables_publish)s