beremiz

Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.

2013-05-21, Edouard Tisserant
08514552944f
Parents 7f473761c932
Children 015dab6a915f
Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
1 files changed, 95 insertions(+), 94 deletions(-)
  • +95 -94
    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